src/AIModel/AIBallistic.cxx

   1 // FGAIBallistic - FGAIBase-derived class creates a ballistic object
   2 //
   3 // Written by David Culp, started November 2003.
   4 // - davidculp2@comcast.net
   5 //
   6 // With major additions by Mathias Froehlich & Vivian Meazza 2004-2007
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <simgear/math/point3d.hxx>
  27 #include <simgear/math/sg_random.h>
  28 #include <simgear/scene/material/mat.hxx>
  29 #include <simgear/math/sg_geodesy.hxx>
  30
  31 #include <Scenery/scenery.hxx>
  32
  33 #include "AIBallistic.hxx"
  34
  35 const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
  36
  37 FGAIBallistic::FGAIBallistic() :
  38     FGAIBase(otBallistic),
  39     _elevation(0),
  40     _aero_stabilised(false),
  41     _drag_area(0.007),
  42     _life_timer(0.0),
  43     _gravity(32),
  44     _buoyancy(0),
  45     _random(false),
  46     _ht_agl_ft(0),
  47     _load_resistance(0),
  48     _solid(false),
  49     _report_collision(false),
  50     _report_impact(false),
  51     _impact_report_node(fgGetNode("/ai/models/model-impact", true)),
  52     _external_force(false)
  53
  54 {
  55     no_roll = false;
  56 }
  57
  58 FGAIBallistic::~FGAIBallistic() {
  59 }
  60
  61 void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
  62     if (!scFileNode)
  63         return;
  64
  65     FGAIBase::readFromScenario(scFileNode);
  66
  67     setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
  68     setElevation(scFileNode->getDoubleValue("elevation", 0.0));
  69     setDragArea(scFileNode->getDoubleValue("eda", 0.007));
  70     setLife(scFileNode->getDoubleValue("life", 900.0));
  71     setBuoyancy(scFileNode->getDoubleValue("buoyancy", 0));
  72     setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0));
  73     setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0));
  74     setWind(scFileNode->getBoolValue("wind", false));
  75     setRoll(scFileNode->getDoubleValue("roll", 0.0));
  76     setCd(scFileNode->getDoubleValue("cd", 0.029));
  77     setMass(scFileNode->getDoubleValue("mass", 0.007));
  78     setStabilisation(scFileNode->getBoolValue("aero_stabilized", false));
  79     setNoRoll(scFileNode->getBoolValue("no-roll", false));
  80     setRandom(scFileNode->getBoolValue("random", false));
  81     setImpact(scFileNode->getBoolValue("impact", false));
  82     setImpactReportNode(scFileNode->getStringValue("impact-reports"));
  83     setName(scFileNode->getStringValue("name", "Bomb"));
  84     setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
  85     setSMPath(scFileNode->getStringValue("submodel-path", ""));
  86     setSubID(scFileNode->getIntValue("SubID", 0));
  87 }
  88
  89 bool FGAIBallistic::init(bool search_in_AI_path) {
  90     FGAIBase::init(search_in_AI_path);
  91
  92     props->setStringValue("material/name", "");
  93     props->setStringValue("name", _name.c_str());
  94     props->setStringValue("submodels/path", _submodel.c_str());
  95
  96     // start with high value so that animations don't trigger yet
  97     _ht_agl_ft = 1e10;
  98     hdg = _azimuth;
  99     pitch = _elevation;
 100     roll = _rotation;
 101     Transform();
 102
 103     return true;
 104 }
 105
 106 void FGAIBallistic::bind() {
 107     //    FGAIBase::bind();
 108     props->tie("sim/time/elapsed-sec",
 109         SGRawValueMethods<FGAIBallistic,double>(*this,
 110         &FGAIBallistic::_getTime));
 111     props->tie("material/load-resistance",
 112                 SGRawValuePointer<double>(&_load_resistance));
 113     props->tie("material/solid",
 114                 SGRawValuePointer<bool>(&_solid));
 115     props->tie("altitude-agl-ft",
 116                 SGRawValuePointer<double>(&_ht_agl_ft));
 117 }
 118
 119 void FGAIBallistic::unbind() {
 120     //    FGAIBase::unbind();
 121     props->untie("sim/time/elapsed-sec");
 122     props->untie("material/load-resistance");
 123     props->untie("material/solid");
 124     props->untie("altitude-agl-ft");
 125 }
 126
 127 void FGAIBallistic::update(double dt) {
 128     FGAIBase::update(dt);
 129     Run(dt);
 130     Transform();
 131 }
 132
 133 void FGAIBallistic::setAzimuth(double az) {
 134     hdg = _azimuth = az;
 135 }
 136
 137 void FGAIBallistic::setElevation(double el) {
 138     pitch = _elevation = el;
 139 }
 140
 141 void FGAIBallistic::setRoll(double rl) {
 142     _rotation = rl;
 143 }
 144
 145 void FGAIBallistic::setStabilisation(bool val) {
 146     _aero_stabilised = val;
 147 }
 148
 149 void FGAIBallistic::setNoRoll(bool nr) {
 150     no_roll = nr;
 151 }
 152
 153 void FGAIBallistic::setDragArea(double a) {
 154     _drag_area = a;
 155 }
 156
 157 void FGAIBallistic::setLife(double seconds) {
 158     life = seconds;
 159 }
 160
 161 void FGAIBallistic::setBuoyancy(double fpss) {
 162     _buoyancy = fpss;
 163 }
 164
 165 void FGAIBallistic::setWind_from_east(double fps) {
 166     _wind_from_east = fps;
 167 }
 168
 169 void FGAIBallistic::setWind_from_north(double fps) {
 170     _wind_from_north = fps;
 171 }
 172
 173 void FGAIBallistic::setWind(bool val) {
 174     _wind = val;
 175 }
 176
 177 void FGAIBallistic::setCd(double c) {
 178     _Cd = c;
 179 }
 180
 181 void FGAIBallistic::setMass(double m) {
 182     _mass = m;
 183 }
 184
 185 void FGAIBallistic::setRandom(bool r) {
 186     _random = r;
 187 }
 188
 189 void FGAIBallistic::setImpact(bool i) {
 190     _report_impact = i;
 191 }
 192
 193 void FGAIBallistic::setCollision(bool c) {
 194     _report_collision = c;
 195 }
 196
 197 void FGAIBallistic::setExternalForce(bool f) {
 198     _external_force = f;
 199 }
 200
 201 void FGAIBallistic::setImpactReportNode(const string& path) {
 202
 203     if (!path.empty())
 204         _impact_report_node = fgGetNode(path.c_str(), true);
 205 }
 206
 207 void FGAIBallistic::setName(const string& n) {
 208     _name = n;
 209 }
 210
 211 void FGAIBallistic::setSMPath(const string& s) {
 212     _submodel = s;
 213 }
 214
 215 void FGAIBallistic::setFuseRange(double f) {
 216     _fuse_range = f;
 217 }
 218
 219 void FGAIBallistic::setSubID(int i) {
 220     _subID = i;
 221     //cout << "sub id " << _subID << " name " << _name << endl;
 222 }
 223
 224 void FGAIBallistic::setSubmodel(const string& s) {
 225     _submodel = s;
 226 }
 227
 228 void FGAIBallistic::setForcePath(const string& p) {
 229     _force_path = p;
 230     if (!_force_path.empty()) {
 231         SGPropertyNode *fnode = fgGetNode(_force_path.c_str(), 0, true );
 232         _force_node = fnode->getChild("force-lb", 0, true);
 233         _force_azimuth_node = fnode->getChild("force-azimuth-deg", 0, true);
 234         _force_elevation_node = fnode->getChild("force-elevation-deg", 0, true);
 235     }
 236 }
 237
 238 void FGAIBallistic::Run(double dt) {
 239     _life_timer += dt;
 240      //cout << "life timer" <<_name <<" " << _life_timer <<  dt << endl;
 241     if (_life_timer > life)
 242         setDie(true);
 243
 244     //randomise Cd by +- 5%
 245     if (_random)
 246         _Cd = _Cd * 0.95 + (0.05 * sg_random());
 247
 248     // Adjust Cd by Mach number. The equations are based on curves
 249     // for a conventional shell/bullet (no boat-tail).
 250     double Cdm;
 251
 252     if (Mach < 0.7)
 253         Cdm = 0.0125 * Mach + _Cd;
 254     else if (Mach < 1.2 )
 255         Cdm = 0.3742 * pow(Mach, 2) - 0.252 * Mach + 0.0021 + _Cd;
 256     else
 257         Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd;
 258
 259     //cout << " Mach , " << Mach << " , Cdm , " << Cdm << " ballistic speed kts //"<< speed <<  endl;
 260
 261     // drag = Cd * 0.5 * rho * speed * speed * drag_area;
 262     // rho is adjusted for altitude in void FGAIBase::update,
 263     // using Standard Atmosphere (sealevel temperature 15C)
 264     // acceleration = drag/mass;
 265     // adjust speed by drag
 266     speed -= (Cdm * 0.5 * rho * speed * speed * _drag_area/_mass) * dt;
 267
 268     // don't let speed become negative
 269     if ( speed < 0.0 )
 270         speed = 0.0;
 271
 272     double speed_fps = speed * SG_KT_TO_FPS;
 273     double hs;
 274
 275     // calculate vertical and horizontal speed components
 276     if (speed == 0.0) {
 277         hs = vs = 0.0;
 278     } else {
 279         vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
 280         hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
 281     }
 282
 283     //resolve horizontal speed into north and east components:
 284     double speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
 285     double speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
 286
 287     // convert horizontal speed (fps) to degrees per second
 288     double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
 289     double speed_east_deg_sec  = speed_east_fps / ft_per_deg_lon;
 290
 291     // if wind not required, set to zero
 292     if (!_wind) {
 293         _wind_from_north = 0;
 294         _wind_from_east = 0;
 295     }
 296
 297     // convert wind speed (fps) to degrees lat/lon per second
 298     double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
 299     double wind_speed_from_east_deg_sec  = _wind_from_east / ft_per_deg_lon;
 300
 301     //calculate velocity due to external force
 302     double force_speed_north_deg_sec = 0;
 303     double force_speed_east_deg_sec = 0;
 304     double vs_force_fps = 0;
 305     double hs_force_fps = 0;
 306     double v_force_acc_fpss = 0;
 307     double force_speed_north_fps = 0;
 308     double force_speed_east_fps = 0;
 309
 310     if (_external_force) {
 311         SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
 312         double force_lbs            = n->getChild("force-lb", 0, true)->getDoubleValue();
 313         double force_elevation_deg  = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
 314         double force_azimuth_deg    = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
 315
 316         //resolve force into vertical and horizontal components:
 317         double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
 318         double h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
 319
 320         //acceleration = (force(lbsf)/mass(slugs))
 321         v_force_acc_fpss = (v_force_lbs/_mass);
 322         double h_force_acc_fpss = (h_force_lbs/_mass);
 323
 324         // velocity = acceleration * dt
 325         hs_force_fps = h_force_acc_fpss * dt;
 326
 327         //resolve horizontal speed into north and east components:
 328         force_speed_north_fps   = cos(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
 329         force_speed_east_fps    = sin(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
 330
 331         // convert horizontal speed (fps) to degrees per second
 332         double force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat;
 333         double force_speed_east_deg_sec  = force_speed_east_fps / ft_per_deg_lon;
 334
 335         //recombine the horizontal velocity components
 336         hs = sqrt(((speed_north_fps + force_speed_north_fps) * (speed_north_fps + force_speed_north_fps))
 337             + ((speed_east_fps + force_speed_east_fps) * (speed_east_fps + force_speed_east_fps)));
 338
 339         /*cout << "mass " << _mass
 340         << " force " << force_lbs
 341         << " elevation " << force_elevation_deg
 342         << " azimuth " << force_azimuth_deg
 343         << endl; */
 344
 345         //cout << " _hs_force_fps " << hs_force_fps
 346         //<< " force_speed_north_fps " << force_speed_north_fps
 347         //<< " force_speed_east_fps " << force_speed_east_fps
 348         //<< " speed_north_fps " << speed_north_fps
 349         //<< " speed_east_fps " << speed_east_fps
 350         //<< endl;
 351     }
 352
 353     // set new position
 354     pos.setLatitudeDeg( pos.getLatitudeDeg()
 355         + (speed_north_deg_sec - wind_speed_from_north_deg_sec + force_speed_north_deg_sec) * dt );
 356     pos.setLongitudeDeg( pos.getLongitudeDeg()
 357         + (speed_east_deg_sec - wind_speed_from_east_deg_sec + force_speed_east_deg_sec) * dt );
 358
 359     // adjust vertical speed for acceleration of gravity, buoyancy, and vertical force
 360     //v_force_acc_fpss = 0;
 361     vs -= (_gravity - _buoyancy - v_force_acc_fpss) * dt;
 362
 363     // adjust altitude (feet) and set new elevation
 364     altitude_ft += vs * dt;
 365     pos.setElevationFt(altitude_ft);
 366
 367     // recalculate elevation and azimuth (velocity vectors)
 368     _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
 369     _azimuth =  atan2((speed_east_fps + force_speed_east_fps),
 370         (speed_north_fps + force_speed_north_fps)) * SG_RADIANS_TO_DEGREES;
 371
 372     // rationalise azimuth
 373     if (_azimuth < 0) _azimuth += 360;
 374
 375     if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
 376         const double coeff = 0.9;
 377         double c = dt / (coeff + dt);
 378         double recip;
 379
 380         // calculate the recip
 381         if(hdg - 180 < 0){
 382             recip = hdg + 180;
 383         } else {
 384             recip = hdg - 180;
 385         }
 386         //cout << "recip " << recip << endl;
 387
 388         // we assume a symetrical MI about the pitch and yaw axis
 389         pitch = (_elevation * c) + (pitch * (1 - c));
 390
 391         //we need to ensure that we turn the short way to the new hdg
 392         if (_azimuth < recip && _azimuth < hdg && hdg > 180) {
 393             //cout <<  "_azimuth - hdg 1 " << _azimuth << " " << hdg << endl;
 394             hdg = ((_azimuth + 360) * c) + (hdg * (1 - c));
 395         } else if (_azimuth > recip && _azimuth > hdg && hdg <= 180){
 396             //cout <<  "_azimuth - hdg 2 " << _azimuth <<" " << hdg << endl;
 397             hdg = ((_azimuth - 360) * c) + (hdg * (1 - c));
 398         } else {
 399             //cout <<  "_azimuth - hdg 3 " << _azimuth <<" " << hdg << endl;
 400             hdg = (_azimuth * c) + (hdg * (1 - c));
 401         }
 402
 403     }
 404
 405     // recalculate total speed
 406     speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
 407
 408     /*cout << "_elevation " << _elevation
 409     << " pitch " << pitch
 410     <<" _yaw " << _yaw
 411     << " hdg " << hdg
 412     << " speed " << speed
 413     << endl;*/
 414
 415     //do impacts and collisions
 416     if (_report_impact && !_impact_reported)
 417         handle_impact();
 418
 419     if (_report_collision && !_collision_reported)
 420         handle_collision();
 421
 422     // set destruction flag if altitude less than sea level -1000
 423     if (altitude_ft < -1000.0)
 424         setDie(true);
 425
 426 }  // end Run
 427
 428 double FGAIBallistic::_getTime() const {
 429     //    cout << "life timer 2" << _life_timer << endl;
 430     return _life_timer;
 431 }
 432
 433 void FGAIBallistic::handle_impact() {
 434     double elevation_m;
 435     const SGMaterial* material;
 436
 437     // try terrain intersection
 438     if (!globals->get_scenery()->get_elevation_m(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
 439             10000.0, elevation_m, &material))
 440         return;
 441
 442     if (material) {
 443         const vector<string> names = material->get_names();
 444         string mat_name;
 445
 446         if (!names.empty())
 447             mat_name = names[0].c_str();
 448
 449         _solid = material->get_solid();
 450         _load_resistance = material->get_load_resistance();
 451         props->setStringValue("material/name", mat_name.c_str());
 452         //cout << "material " << mat_name << " solid " << _solid << " load " << _load_resistance << endl;
 453     }
 454
 455     _ht_agl_ft = pos.getElevationFt() - elevation_m * SG_METER_TO_FEET;
 456
 457     if (_ht_agl_ft <= 0) {
 458         SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: terrain impact");
 459         report_impact(elevation_m);
 460         _impact_reported = true;
 461
 462         // kill the AIObject if there is no subsubmodel
 463         if (_subID == 0)
 464             setDie(true);
 465     }
 466 }
 467
 468 void FGAIBallistic::handle_collision()
 469 {
 470     const FGAIBase *object = manager->calcCollision(pos.getElevationFt(),
 471             pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
 472
 473     if (object) {
 474         SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object hit");
 475         report_impact(pos.getElevationM(), object);
 476         _collision_reported = true;
 477     }
 478 }
 479
 480 void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
 481 {
 482     _impact_lat    = pos.getLatitudeDeg();
 483     _impact_lon    = pos.getLongitudeDeg();
 484     _impact_elev   = elevation;
 485     _impact_speed  = speed * SG_KT_TO_MPS;
 486     _impact_hdg    = hdg;
 487     _impact_pitch  = pitch;
 488     _impact_roll   = roll;
 489
 490     SGPropertyNode *n = props->getNode("impact", true);
 491     if (object)
 492         n->setStringValue("type", object->getTypeString());
 493     else
 494         n->setStringValue("type", "terrain");
 495
 496     n->setDoubleValue("longitude-deg", _impact_lon);
 497     n->setDoubleValue("latitude-deg", _impact_lat);
 498     n->setDoubleValue("elevation-m", _impact_elev);
 499     n->setDoubleValue("heading-deg", _impact_hdg);
 500     n->setDoubleValue("pitch-deg", _impact_pitch);
 501     n->setDoubleValue("roll-deg", _impact_roll);
 502     n->setDoubleValue("speed-mps", _impact_speed);
 503
 504     _impact_report_node->setStringValue(props->getPath());
 505 }
 506
 507 // end AIBallistic
 508