_report_impact(false),
_external_force(false),
_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
- _old_height(0)
+ _old_height(0),
+ _elapsed_time(0)
{
no_roll = false;
FGAIBase::readFromScenario(scFileNode);
- //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac"));
+ //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac"));
+ setRandom(scFileNode->getBoolValue("random", false));
setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
setElevation(scFileNode->getDoubleValue("elevation", 0));
setDragArea(scFileNode->getDoubleValue("eda", 0.007));
setCd(scFileNode->getDoubleValue("cd", 0.029));
//setMass(scFileNode->getDoubleValue("mass", 0.007));
setWeight(scFileNode->getDoubleValue("weight", 0.25));
- setStabilisation(scFileNode->getBoolValue("aero-stabilized", false));
+ setStabilisation(scFileNode->getBoolValue("aero-stabilised", false));
setNoRoll(scFileNode->getBoolValue("no-roll", false));
- setRandom(scFileNode->getBoolValue("random", false));
setImpact(scFileNode->getBoolValue("impact", false));
+ setExpiry(scFileNode->getBoolValue("expiry", false));
+ setCollision(scFileNode->getBoolValue("collision", false));
setImpactReportNode(scFileNode->getStringValue("impact-reports"));
setName(scFileNode->getStringValue("name", "Rocket"));
setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
setSubID(scFileNode->getIntValue("SubID", 0));
setExternalForce(scFileNode->getBoolValue("external-force", false));
setForcePath(scFileNode->getStringValue("force-path", ""));
- setForceStabilisation(scFileNode->getBoolValue("force-stabilized", false));
+ setForceStabilisation(scFileNode->getBoolValue("force-stabilised", false));
setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
setSlaved(scFileNode->getBoolValue("slaved", false));
setSlavedLoad(scFileNode->getBoolValue("slaved-load", false));
setContentsNode(scFileNode->getStringValue("contents"));
- setRandom(scFileNode->getBoolValue("random", false));
}
bool FGAIBallistic::init(bool search_in_AI_path) {
_impact_reported = false;
_collision_reported = false;
+ _expiry_reported = false;
+
+ _impact_lat = 0;
+ _impact_lon = 0;
+ _impact_elev = 0;
+ _impact_hdg = 0;
+ _impact_pitch = 0;
+ _impact_roll = 0;
+ _impact_speed = 0;
+
invisible = false;
_elapsed_time += (sg_random() * 100);
props->setStringValue("material/name", "");
props->setStringValue("name", _name.c_str());
props->setStringValue("submodels/path", _submodel.c_str());
+ props->setStringValue("force/path", _force_path.c_str());
+ //props->setStringValue("vector/path", _vector_path.c_str());
// start with high value so that animations don't trigger yet
_ht_agl_ft = 1e10;
}
void FGAIBallistic::setAzimuth(double az) {
- hdg = _azimuth = az;
+
+
+ if (_random)
+ hdg = _azimuth = (az - 5 ) + (10 * sg_random());
+ else
+ hdg = _azimuth = az;
+
+ //cout << _name << " init hdg " << hdg << " random " << _random << endl;
}
void FGAIBallistic::setElevation(double el) {
}
void FGAIBallistic::setLife(double seconds) {
- life = seconds;
+
+ if (_random){
+ life = seconds * _randomness + (seconds * (1 -_randomness) * sg_random());
+ //cout << "life " << life << endl;
+ } else
+ life = seconds;
}
void FGAIBallistic::setBuoyancy(double fpss) {
_weight_lb = w;
}
+void FGAIBallistic::setRandomness(double r) {
+ _randomness = r;
+}
+
void FGAIBallistic::setRandom(bool r) {
_random = r;
}
_report_collision = c;
}
-void FGAIBallistic::setExpiry(bool e) {\r
- _report_expiry = e;\r
-// cout << "_report_expiry " << _report_expiry << endl;\r
+void FGAIBallistic::setExpiry(bool e) {
+ _report_expiry = e;
+ //cout << "_report_expiry " << _report_expiry << endl;
}
void FGAIBallistic::setExternalForce(bool f) {
}
void FGAIBallistic::setHdg(double az, double dt, double coeff){
- double recip = getRecip(hdg);
- double c = dt / (coeff + dt);
- //we need to ensure that we turn the short way to the new hdg
- if (az < recip && az < hdg && hdg > 180) {
- hdg = ((az + 360) * c) + (hdg * (1 - c));
- } else if (az > recip && az > hdg && hdg <= 180){
- hdg = ((az - 360) * c) + (hdg * (1 - c));
- } else {
- hdg = (az * c) + (hdg * (1 - c));
- }
- }
+ double recip = getRecip(hdg);
+ double c = dt / (coeff + dt);
+ //we need to ensure that we turn the short way to the new hdg
+ if (az < recip && az < hdg && hdg > 180) {
+ hdg = ((az + 360) * c) + (hdg * (1 - c));
+ } else if (az > recip && az > hdg && hdg <= 180){
+ hdg = ((az - 360) * c) + (hdg * (1 - c));
+ } else {
+ hdg = (az * c) + (hdg * (1 - c));
+ }
+}
double FGAIBallistic::getTgtXOffset() const {
return _tgt_x_offset;
_life_timer += dt;
// if life = -1 the object does not die
- if (_life_timer > life && life != -1){\r
-\r
- if (_report_expiry && !_expiry_reported){\r
- handle_expiry();\r
- } else\r
- setDie(true);\r
-\r
+ if (_life_timer > life && life != -1){
+
+ if (_report_expiry && !_expiry_reported){
+ //cout<<"AIBallistic: expiry"<< endl;
+ handle_expiry();
+ } else
+ setDie(true);
+
}
//set the contents in the appropriate tank or other property in the parent to zero
setContents(0);
- //randomise Cd by +- 5%
+ //randomise Cd by +- 10%
if (_random)
- _Cd = _Cd * 0.95 + (0.05 * sg_random());
+ _Cd = _Cd * 0.90 + (0.10 * sg_random());
// Adjust Cd by Mach number. The equations are based on curves
// for a conventional shell/bullet (no boat-tail).
double friction_force_speed_east_deg_sec = 0;
double force_elevation_deg = 0;
- if (_external_force) {
+ if (_external_force) {
+
SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
double force_lbs = n->getChild("force-lb", 0, true)->getDoubleValue();
force_elevation_deg = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
double force_azimuth_deg = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
-
+
//resolve force into vertical and horizontal components:
double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
if (_azimuth < 0)
_azimuth += 360;
+ //cout << "_azimuth " << _azimuth << " hdg "<< hdg << endl;
+
if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
+ cout<< "_aero_stabilised "<< endl;
const double coeff = 0.9;
// we assume a symetrical MI about the pitch and yaw axis
setPch(_elevation, dt, coeff);
setHdg(_azimuth, dt, coeff);
} else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
- const double coeff = 0.9;
+ //cout<< "_force_stabilised "<< endl;
+
+ const double coeff = 0.9;
double ratio = h_force_lbs/(_mass * slugs_to_lbs);
if (ratio > 1) ratio = 1;
}
}
-void FGAIBallistic::handle_expiry() {\r
-\r
- report_impact(pos.getElevationM());\r
- _expiry_reported = true;\r
-\r
- SG_LOG(SG_GENERAL, SG_ALERT, "AIBallistic: expiry");\r
- //if (life == -1){\r
- // invisible = true;\r
- //} else if (_subID == 0) // kill the AIObject if there is no subsubmodel\r
- // setDie(true);\r
- \r
+void FGAIBallistic::handle_expiry() {
+
+ SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: handle_expiry " << pos.getElevationM());
+
+ report_impact(pos.getElevationM());
+ _expiry_reported = true;
+
+ //if (life == -1){
+ // invisible = true;
+ //} else if (_subID == 0) // kill the AIObject if there is no subsubmodel
+ // setDie(true);
+
}
void FGAIBallistic::handle_collision()
// convert geodetic positions to geocentered
SGVec3d cartuserPos = getCartUserPos();
- SGVec3d cartPos = getCartPos();
+ //SGVec3d cartPos = getCartPos();
// Transform to the right coordinate frame, configuration is done in
// the x-forward, y-right, z-up coordinates (feet), computation
continue; // so we can continue
if (_impact || _hit || _expiry) {
- //SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " << _hit );
+ // SG_LOG(SG_GENERAL, SG_ALERT, "Submodel: Impact " << _impact << " hit! " << _hit
+ //<< " exipiry :-( " << _expiry );
submodel_iterator = submodels.begin();
_parent_roll = (*sm_list_itr)->_getImpactRoll();
_parent_speed = (*sm_list_itr)->_getImpactSpeed();
(*submodel_iterator)->first_time = true;
+ //cout << "Impact: parent SubID = child_ID elev " << _parent_elev << endl;
- if (release(*submodel_iterator, dt))
+ if (release(*submodel_iterator, dt))
(*sm_list_itr)->setDie(true);
+
}
++submodel_iterator;
if ((*submodel_iterator)->trigger_node != 0) {
_trigger_node = (*submodel_iterator)->trigger_node;
trigger = _trigger_node->getBoolValue();
- //cout << "trigger node found " << trigger << endl;
+ //cout << (*submodel_iterator)->name << "trigger node found " << trigger << endl;
} else {
trigger = true;
//cout << (*submodel_iterator)->name << "trigger node not found " << trigger << endl;
if (trigger && (*submodel_iterator)->count != 0) {
- //int id = (*submodel_iterator)->id;
- //string name = (*submodel_iterator)->name;
+ int id = (*submodel_iterator)->id;
+ string name = (*submodel_iterator)->name;
/*SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels end: " << (*submodel_iterator)->id
<< " name " << (*submodel_iterator)->name
sm->timer += dt;
if (sm->timer < sm->delay) {
- //cout << "not yet: timer" << sm->timer << " delay " << sm->delay<< endl;
+ //cout << "not yet: timer " << sm->timer << " delay " << sm->delay << endl;
return false;
}
+
+ //cout << "released timer: " << sm->timer << " delay " << sm->delay << endl;
sm->timer = 0.0;
FGAIBallistic* ballist = new FGAIBallistic;
ballist->setPath(sm->model.c_str());
- ballist->setLatitude(IC.lat);
- ballist->setLongitude(IC.lon);
- ballist->setAltitude(IC.alt);
+ ballist->setName(sm->name);
+ ballist->setRandom(sm->random);
+ ballist->setRandomness(sm->randomness);
+ ballist->setLatitude(offsetpos.getLatitudeDeg());
+ ballist->setLongitude(offsetpos.getLongitudeDeg());
+ ballist->setAltitude(offsetpos.getElevationFt());
ballist->setAzimuth(IC.azimuth);
ballist->setElevation(IC.elevation);
ballist->setRoll(IC.roll);
ballist->setCd(sm->cd);
ballist->setStabilisation(sm->aero_stabilised);
ballist->setNoRoll(sm->no_roll);
- ballist->setName(sm->name);
ballist->setCollision(sm->collision);
ballist->setExpiry(sm->expiry);
ballist->setImpact(sm->impact);
if (sm->count > 0)
sm->count--;
-
return true;
}
//cout << " name " << name << " id " << id << " sub id" << sub_id << endl;
- if (_impact || _hit) {
+ // set the Initial Conditions for the types of submodel parent
+
+ if (_impact || _hit || _expiry) {
// set the data for a submodel tied to a submodel
_count++;
//cout << "Submodels: release sub sub " << _count<< endl;
cout << "speed north " << IC.speed_north_fps << endl ;
cout << "parent speed fps in" << IC.speed << "sm speed in " << sm->speed << endl ;*/
+ // Set the Initial Conditions that are common to all types of parent
IC.wind_from_east = _user_wind_from_east_node->getDoubleValue();
IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
- in[0] = sm->x_offset;
- in[1] = sm->y_offset;
- in[2] = sm->z_offset;
+ userpos.setLatitudeDeg(IC.lat);
+ userpos.setLongitudeDeg(IC.lon);
+ userpos.setElevationFt(IC.alt);
+
+ _x_offset = sm->x_offset;
+ _y_offset = sm->y_offset;
+ _z_offset = sm->z_offset;
+
+ setOffsetPos();
+
+ //IC.elevation += sm->pitch_offset;
+ //IC.azimuth += sm->yaw_offset ;
// pre-process the trig functions
cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS);
sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS);
- // set up the transform matrix
- trans[0][0] = cosRy * cosRz;
- trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
- trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz;
+ //// set up the transform matrix
+ //trans[0][0] = cosRy * cosRz;
+ //trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
+ //trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz;
- trans[1][0] = cosRy * sinRz;
- trans[1][1] = cosRx * cosRz + sinRx * sinRy * sinRz;
- trans[1][2] = -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
+ //trans[1][0] = cosRy * sinRz;
+ //trans[1][1] = cosRx * cosRz + sinRx * sinRy * sinRz;
+ //trans[1][2] = -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
- trans[2][0] = -1 * sinRy;
- trans[2][1] = sinRx * cosRy;
- trans[2][2] = cosRx * cosRy;
+ //trans[2][0] = -1 * sinRy;
+ //trans[2][1] = sinRx * cosRy;
+ //trans[2][2] = cosRx * cosRy;
- // multiply the input and transform matrices
- out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
- out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
- out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
+ //// multiply the input and transform matrices
+ //out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
+ //out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
+ //out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
- // convert ft to degrees of latitude
- out[0] = out[0] / (366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
+ //// convert ft to degrees of latitude
+ //out[0] = out[0] / (366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
- // convert ft to degrees of longitude
- out[1] = out[1] / (365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
+ //// convert ft to degrees of longitude
+ //out[1] = out[1] / (365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
- // set submodel initial position
- IC.lat += out[0];
- IC.lon += out[1];
- IC.alt += out[2];
+ //// set submodel initial position
+ //IC.lat += out[0];
+ //IC.lon += out[1];
+ //IC.alt += out[2];
// get aircraft velocity vector angles in XZ and XY planes
//double alpha = _user_alpha_node->getDoubleValue();
//double velXY = IC.azimuth - (IC.elevation - alpha * sinRx);
// Get submodel initial velocity vector angles in XZ and XY planes.
- // This needs to be fixed. This vector should be added to aircraft's vector.
+ // This vector should be added to aircraft's vector.
IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
IC.azimuth += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
// if speeds are low this calculation can become unreliable
if (IC.speed > 1) {
- IC.azimuth = atan2(IC.total_speed_east , IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
+ //IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
// cout << "azimuth1 " << IC.azimuth<<endl;
// rationalise the output
}
-double FGSubmodelMgr::getRange(double lat, double lon, double lat2, double lon2) const
-{
- double course, distance, az2;
-
- //calculate the bearing and range of the second pos from the first
- geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance);
- distance *= SG_METER_TO_NM;
- return distance;
-}
void FGSubmodelMgr::setData(int id, string& path, bool serviceable)
{
sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false);
sm->ext_force = entry_node->getBoolValue("external-force", false);
sm->force_path = entry_node->getStringValue("force-path", "");
+ sm->random = entry_node->getBoolValue("random", false);
+ sm->randomness = entry_node->getDoubleValue("randomness", 0.5);
+
+
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
sm->contents = sm->contents_node->getDoubleValue();
sm->sub_id = 0;
sm->prop = fgGetNode("/ai/submodels/submodel", index, true);
+ sm->prop->tie("delay", SGRawValuePointer<double>(&(sm->delay)));
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
+ sm->prop->tie("random", SGRawValuePointer<bool>(&(sm->random)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
string force_path = sm->force_path;
sm->prop->setStringValue("force_path", force_path.c_str());
- //cout << "set force_path " << force_path << endl;
+ //cout << "set force_path Sub " << force_path << endl;
if (sm->contents_node != 0)
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false);
sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
sm->submodel = entry_node->getStringValue("submodel-path", "");
+ sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false);
sm->ext_force = entry_node->getBoolValue("external-force", false);
sm->force_path = entry_node->getStringValue("force-path", "");
+ sm->random = entry_node->getBoolValue("random", false);
+ sm->randomness = entry_node->getDoubleValue("randomness", 0.5);
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
sm->prop->tie("sub-id", SGRawValuePointer<int>(&(sm->sub_id)));
sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
+ sm->prop->tie("random", SGRawValuePointer<bool>(&(sm->random)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
string submodel = sm->submodel;
sm->prop->setStringValue("submodel", submodel.c_str());
- // cout << " set submodel path " << submodel<< endl;
+ // cout << " set submodel path AI" << submodel<< endl;
string force_path = sm->force_path;
sm->prop->setStringValue("force_path", force_path.c_str());
- //cout << "set force_path " << force_path << endl;
+ cout << "set force_path AI" << force_path << endl;
if (sm->contents_node != 0)
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
//}
}
+SGVec3d FGSubmodelMgr::getCartOffsetPos() const{
+
+ // convert geodetic positions to geocentered
+ SGVec3d cartuserPos = SGVec3d::fromGeod(userpos);
+
+ // Transform to the right coordinate frame, configuration is done in
+ // the x-forward, y-right, z-up coordinates (feet), computation
+ // in the simulation usual body x-forward, y-right, z-down coordinates
+ // (meters) )
+
+ SGVec3d _off(_x_offset * SG_FEET_TO_METER,
+ _y_offset * SG_FEET_TO_METER,
+ -_z_offset * SG_FEET_TO_METER);
+
+ // Transform the user position to the horizontal local coordinate system.
+ SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
+
+ // and postrotate the orientation of the user model wrt the horizontal
+ // local frame
+ hlTrans *= SGQuatd::fromYawPitchRollDeg(
+ IC.azimuth,
+ IC.elevation,
+ IC.roll);
+
+ // The offset converted to the usual body fixed coordinate system
+ // rotated to the earth-fixed coordinates axis
+ SGVec3d off = hlTrans.backTransform(_off);
+
+ // Add the position offset of the user model to get the geocentered position
+ SGVec3d offsetPos = cartuserPos + off;
+
+ return offsetPos;
+}
+
+void FGSubmodelMgr::setOffsetPos(){
+ // convert the offset geocentered position to geodetic
+ SGVec3d cartoffsetPos = getCartOffsetPos();
+
+ SGGeodesy::SGCartToGeod(cartoffsetPos, offsetpos);
+}
// end of submodel.cxx
projects / flightgear.git / commitdiff