]> git.mxchange.org Git - flightgear.git/blob - src/AIModel/AIBallistic.cxx
Clear chat messages when an aircraft becomes inactive in the property tree.
[flightgear.git] / 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-2008
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/math/sg_geodesy.hxx>
29
30 #include <Scenery/scenery.hxx>
31
32 #include "AIBallistic.hxx"
33
34 const double FGAIBallistic::slugs_to_kgs = 14.5939029372;
35 const double FGAIBallistic::slugs_to_lbs = 32.1740485564;
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.1740485564),
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 _wind(true),
52     _impact_report_node(fgGetNode("/ai/models/model-impact", true)),
53     _external_force(false)
54
55 {
56     no_roll = false;
57 }
58
59 FGAIBallistic::~FGAIBallistic() {
60 }
61
62 void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
63     if (!scFileNode){
64         return;
65     }
66
67     FGAIBase::readFromScenario(scFileNode);
68
69     //setPath(scFileNode->getStringValue("model", "Models/Geometry/rocket.ac"));
70     setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
71     setElevation(scFileNode->getDoubleValue("elevation", 0));
72     setDragArea(scFileNode->getDoubleValue("eda", 0.007));
73     setLife(scFileNode->getDoubleValue("life", 900.0));
74     setBuoyancy(scFileNode->getDoubleValue("buoyancy", 0));
75     setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0));
76     setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0));
77     setWind(scFileNode->getBoolValue("wind", false));
78     setRoll(scFileNode->getDoubleValue("roll", 0.0));
79     setCd(scFileNode->getDoubleValue("cd", 0.029));
80     setMass(scFileNode->getDoubleValue("mass", 0.007));
81     setStabilisation(scFileNode->getBoolValue("aero_stabilized", false));
82     setNoRoll(scFileNode->getBoolValue("no-roll", false));
83     setRandom(scFileNode->getBoolValue("random", false));
84     setImpact(scFileNode->getBoolValue("impact", false));
85     setImpactReportNode(scFileNode->getStringValue("impact-reports"));
86     setName(scFileNode->getStringValue("name", "Rocket"));
87     setFuseRange(scFileNode->getDoubleValue("fuse-range", 0.0));
88     setSMPath(scFileNode->getStringValue("submodel-path", ""));
89     setSubID(scFileNode->getIntValue("SubID", 0));
90     setExternalForce(scFileNode->getBoolValue("external-force", false));
91     setForcePath(scFileNode->getStringValue("force-path", ""));
92     setForceStabilisation(scFileNode->getBoolValue("force_stabilized", false));
93     setXOffset(scFileNode->getDoubleValue("x-offset", 0.0));
94     setYOffset(scFileNode->getDoubleValue("y-offset", 0.0));
95     setZOffset(scFileNode->getDoubleValue("z-offset", 0.0));
96 }
97
98 bool FGAIBallistic::init(bool search_in_AI_path) {
99     FGAIBase::init(search_in_AI_path);
100
101     props->setStringValue("material/name", "");
102     props->setStringValue("name", _name.c_str());
103     props->setStringValue("submodels/path", _submodel.c_str());
104
105     // start with high value so that animations don't trigger yet
106     _ht_agl_ft = 1e10;
107     hdg = _azimuth;
108     pitch = _elevation;
109     roll = _rotation;
110
111     Transform();
112
113     return true;
114 }
115
116 void FGAIBallistic::bind() {
117     //    FGAIBase::bind();
118     props->tie("sim/time/elapsed-sec",
119         SGRawValueMethods<FGAIBallistic,double>(*this,
120         &FGAIBallistic::_getTime));
121     props->tie("material/load-resistance",
122                 SGRawValuePointer<double>(&_load_resistance));
123     props->tie("material/solid",
124                 SGRawValuePointer<bool>(&_solid));
125     props->tie("altitude-agl-ft",
126                 SGRawValuePointer<double>(&_ht_agl_ft));
127 }
128
129 void FGAIBallistic::unbind() {
130     //    FGAIBase::unbind();
131     props->untie("sim/time/elapsed-sec");
132     props->untie("material/load-resistance");
133     props->untie("material/solid");
134     props->untie("altitude-agl-ft");
135 }
136
137 void FGAIBallistic::update(double dt) {
138     FGAIBase::update(dt);
139     Run(dt);
140     Transform();
141 }
142
143 void FGAIBallistic::setAzimuth(double az) {
144     hdg = _azimuth = az;
145 }
146
147 void FGAIBallistic::setElevation(double el) {
148     pitch = _elevation = el;
149 }
150
151 void FGAIBallistic::setRoll(double rl) {
152     _rotation = rl;
153 }
154
155 void FGAIBallistic::setStabilisation(bool val) {
156     _aero_stabilised = val;
157 }
158
159 void FGAIBallistic::setForceStabilisation(bool val) {
160     _force_stabilised = val;
161 }
162
163 void FGAIBallistic::setNoRoll(bool nr) {
164     no_roll = nr;
165 }
166
167 void FGAIBallistic::setDragArea(double a) {
168     _drag_area = a;
169 }
170
171 void FGAIBallistic::setLife(double seconds) {
172     life = seconds;
173 }
174
175 void FGAIBallistic::setBuoyancy(double fpss) {
176     _buoyancy = fpss;
177 }
178
179 void FGAIBallistic::setWind_from_east(double fps) {
180     _wind_from_east = fps;
181 }
182
183 void FGAIBallistic::setWind_from_north(double fps) {
184     _wind_from_north = fps;
185 }
186
187 void FGAIBallistic::setWind(bool val) {
188     _wind = val;
189 }
190
191 void FGAIBallistic::setCd(double c) {
192     _Cd = c;
193 }
194
195 void FGAIBallistic::setMass(double m) {
196     _mass = m;
197 }
198
199 void FGAIBallistic::setRandom(bool r) {
200     _random = r;
201 }
202
203 void FGAIBallistic::setImpact(bool i) {
204     _report_impact = i;
205 }
206
207 void FGAIBallistic::setCollision(bool c) {
208     _report_collision = c;
209 }
210
211 void FGAIBallistic::setExternalForce(bool f) {
212     _external_force = f;
213 }
214
215 void FGAIBallistic::setImpactReportNode(const string& path) {
216
217     if (!path.empty())
218         _impact_report_node = fgGetNode(path.c_str(), true);
219 }
220
221 void FGAIBallistic::setName(const string& n) {
222     _name = n;
223 }
224
225 void FGAIBallistic::setSMPath(const string& s) {
226     _submodel = s;
227 }
228
229 void FGAIBallistic::setFuseRange(double f) {
230     _fuse_range = f;
231 }
232
233 void FGAIBallistic::setXOffset(double x) {
234     _x_offset = x;
235 }
236
237 void FGAIBallistic::setYOffset(double y) {
238     _y_offset = y;
239 }
240
241 void FGAIBallistic::setZOffset(double z) {
242     _z_offset = z;
243 }
244
245 void FGAIBallistic::setSubID(int i) {
246     _subID = i;
247     //cout << "sub id " << _subID << " name " << _name << endl;
248 }
249
250 void FGAIBallistic::setSubmodel(const string& s) {
251     _submodel = s;
252 }
253
254 void FGAIBallistic::setForcePath(const string& p) {
255     _force_path = p;
256     if (!_force_path.empty()) {
257         SGPropertyNode *fnode = fgGetNode(_force_path.c_str(), 0, true );
258         _force_node = fnode->getChild("force-lb", 0, true);
259         _force_azimuth_node = fnode->getChild("force-azimuth-deg", 0, true);
260         _force_elevation_node = fnode->getChild("force-elevation-deg", 0, true);
261     }
262 }
263
264 bool FGAIBallistic::getHtAGL(){
265
266     if (globals->get_scenery()->get_elevation_m(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
267         10000.0, _elevation_m, &_material)){
268             _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
269             if (_material) {
270                 const vector<string>& names = _material->get_names();
271
272                 _solid = _material->get_solid();
273                 _load_resistance = _material->get_load_resistance();
274                 _frictionFactor =_material->get_friction_factor();
275                 if (!names.empty())
276                     props->setStringValue("material/name", names[0].c_str());
277                 else
278                     props->setStringValue("material/name", "");
279                 /*cout << "material " << mat_name 
280                 << " solid " << _solid 
281                 << " load " << _load_resistance
282                 << " frictionFactor " << frictionFactor
283                 << endl;*/
284             }
285             return true;
286     } else {
287         return false;
288     }
289
290 }
291
292 double FGAIBallistic::getRecip(double az){
293     // calculate the reciprocal of the input azimuth 
294     if(az - 180 < 0){
295         return az + 180;
296     } else {
297         return az - 180; 
298     }
299 }
300
301 void FGAIBallistic::setPitch(double e, double dt, double coeff){
302     double c = dt / (coeff + dt);
303     pitch = (e * c) + (pitch * (1 - c));
304 }
305
306 void FGAIBallistic::setHdg(double dt, double coeff){
307     double recip = getRecip(hdg);
308     double c = dt / (coeff + dt);
309     //we need to ensure that we turn the short way to the new hdg
310     if (_azimuth < recip && _azimuth < hdg && hdg > 180) {
311         hdg = ((_azimuth + 360) * c) + (hdg * (1 - c));
312     } else if (_azimuth > recip && _azimuth > hdg && hdg <= 180){
313         hdg = ((_azimuth - 360) * c) + (hdg * (1 - c));
314     } else {
315         hdg = (_azimuth * c) + (hdg * (1 - c));
316     }
317 }
318
319 void FGAIBallistic::Run(double dt) {
320     _life_timer += dt;
321
322     if (_life_timer > life && life != -1)
323         setDie(true);
324
325     //randomise Cd by +- 5%
326     if (_random)
327         _Cd = _Cd * 0.95 + (0.05 * sg_random());
328
329     // Adjust Cd by Mach number. The equations are based on curves
330     // for a conventional shell/bullet (no boat-tail).
331     double Cdm;
332
333     if (Mach < 0.7)
334         Cdm = 0.0125 * Mach + _Cd;
335     else if (Mach < 1.2 )
336         Cdm = 0.3742 * pow(Mach, 2) - 0.252 * Mach + 0.0021 + _Cd;
337     else
338         Cdm = 0.2965 * pow(Mach, -1.1506) + _Cd;
339
340     //cout << " Mach , " << Mach << " , Cdm , " << Cdm << " ballistic speed kts //"<< speed <<  endl;
341
342     // drag = Cd * 0.5 * rho * speed * speed * drag_area;
343     // rho is adjusted for altitude in void FGAIBase::update,
344     // using Standard Atmosphere (sealevel temperature 15C)
345     // acceleration = drag/mass;
346     // adjust speed by drag
347     speed -= (Cdm * 0.5 * rho * speed * speed * _drag_area/_mass) * dt;
348
349     // don't let speed become negative
350     if ( speed < 0.0 )
351         speed = 0.0;
352
353     double speed_fps = speed * SG_KT_TO_FPS;
354     double hs;
355
356     // calculate vertical and horizontal speed components
357     if (speed == 0.0) {
358         hs = vs = 0.0;
359     } else {
360         vs = sin( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
361         hs = cos( _elevation * SG_DEGREES_TO_RADIANS ) * speed_fps;
362     }
363
364     //resolve horizontal speed into north and east components:
365     double speed_north_fps = cos(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
366     double speed_east_fps = sin(_azimuth / SG_RADIANS_TO_DEGREES) * hs;
367
368     // convert horizontal speed (fps) to degrees per second
369     double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
370     double speed_east_deg_sec  = speed_east_fps / ft_per_deg_lon;
371
372     // if wind not required, set to zero
373     if (!_wind) {
374         _wind_from_north = 0;
375         _wind_from_east = 0;
376     } else {
377         _wind_from_north = manager->get_wind_from_north();
378         _wind_from_east = manager->get_wind_from_east();
379     }
380
381     //calculate velocity due to external force
382     double force_speed_north_deg_sec = 0;
383     double force_speed_east_deg_sec = 0;
384     double vs_force_fps = 0;
385     double hs_force_fps = 0;
386     double v_force_acc_fpss = 0;
387     double force_speed_north_fps = 0;
388     double force_speed_east_fps = 0;
389     double h_force_lbs = 0;
390     double normal_force_lbs = 0;
391     double normal_force_fpss = 0;
392     double static_friction_force_lbs = 0;
393     double dynamic_friction_force_lbs = 0;
394     double friction_force_speed_north_fps = 0;
395     double friction_force_speed_east_fps = 0;
396     double friction_force_speed_north_deg_sec = 0;
397     double friction_force_speed_east_deg_sec = 0;
398     double force_elevation_deg = 0;
399
400     if (_external_force) {
401         SGPropertyNode *n = fgGetNode(_force_path.c_str(), true);
402         double force_lbs            = n->getChild("force-lb", 0, true)->getDoubleValue();
403         force_elevation_deg         = n->getChild("force-elevation-deg", 0, true)->getDoubleValue();
404         double force_azimuth_deg    = n->getChild("force-azimuth-deg", 0, true)->getDoubleValue();
405
406         //resolve force into vertical and horizontal components:
407         double v_force_lbs = force_lbs * sin( force_elevation_deg * SG_DEGREES_TO_RADIANS );
408         h_force_lbs = force_lbs * cos( force_elevation_deg * SG_DEGREES_TO_RADIANS );
409
410         //ground interaction 
411
412         if (getHtAGL()){
413             double deadzone = 0.1;
414
415             if ( _ht_agl_ft <= (0 + _z_offset + deadzone) && _solid){
416                 normal_force_lbs = (_mass * slugs_to_lbs) - v_force_lbs;
417                 pos.setElevationFt((_elevation_m * SG_METER_TO_FEET) + _z_offset);
418                 vs = 0;
419
420                 // calculate friction
421                 // we assume a static Coefficient of Friction (mu) of 0.62 (wood on concrete)
422                 double mu = 0.62;
423
424                 static_friction_force_lbs = mu * normal_force_lbs * _frictionFactor;
425
426                 //adjust horizontal force
427                 if (h_force_lbs <= static_friction_force_lbs && hs <= 0.1)
428                     h_force_lbs = hs = 0;
429                 else 
430                     dynamic_friction_force_lbs = (static_friction_force_lbs * 0.75);
431
432                 //ignore wind when on the ground for now
433                 //TODO fix this
434                 _wind_from_north = 0;
435                 _wind_from_east = 0;
436
437             }
438
439         }
440
441         //acceleration = (force(lbsf)/mass(slugs))
442         v_force_acc_fpss = v_force_lbs/_mass;
443         normal_force_fpss = normal_force_lbs/_mass;
444         double h_force_acc_fpss = h_force_lbs/_mass;
445         double dynamic_friction_acc_fpss = dynamic_friction_force_lbs/_mass;
446
447         // velocity = acceleration * dt
448         hs_force_fps = h_force_acc_fpss * dt;
449         double friction_force_fps = dynamic_friction_acc_fpss * dt;
450
451         //resolve horizontal speeds into north and east components:
452         force_speed_north_fps   = cos(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
453         force_speed_east_fps    = sin(force_azimuth_deg * SG_DEGREES_TO_RADIANS) * hs_force_fps;
454
455         double friction_force_speed_north_fps = cos(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
456         double friction_force_speed_east_fps  = sin(getRecip(hdg) * SG_DEGREES_TO_RADIANS) * friction_force_fps;
457
458         // convert horizontal speed (fps) to degrees per second
459         force_speed_north_deg_sec = force_speed_north_fps / ft_per_deg_lat;
460         force_speed_east_deg_sec  = force_speed_east_fps / ft_per_deg_lon;
461
462         friction_force_speed_north_deg_sec = friction_force_speed_north_fps / ft_per_deg_lat;
463         friction_force_speed_east_deg_sec  = friction_force_speed_east_fps / ft_per_deg_lon;
464     }
465
466     // convert wind speed (fps) to degrees lat/lon per second
467     double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
468     double wind_speed_from_east_deg_sec  = _wind_from_east / ft_per_deg_lon;
469
470     // set new position
471     pos.setLatitudeDeg( pos.getLatitudeDeg()
472         + (speed_north_deg_sec - wind_speed_from_north_deg_sec 
473         + force_speed_north_deg_sec + friction_force_speed_north_deg_sec) * dt );
474     pos.setLongitudeDeg( pos.getLongitudeDeg()
475         + (speed_east_deg_sec - wind_speed_from_east_deg_sec 
476         + force_speed_east_deg_sec + friction_force_speed_east_deg_sec) * dt );
477
478     //recombine the horizontal velocity components
479     hs = sqrt(((speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps) 
480         * (speed_north_fps + force_speed_north_fps + friction_force_speed_north_fps))
481         + ((speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps) 
482         * (speed_east_fps + force_speed_east_fps + friction_force_speed_east_fps)));
483
484     if (hs <= 0.00001)
485         hs = 0;
486
487     // adjust vertical speed for acceleration of gravity, buoyancy, and vertical force
488     vs -= (_gravity - _buoyancy - v_force_acc_fpss - normal_force_fpss) * dt;
489
490     if (vs <= 0.00001 && vs >= -0.00001)
491         vs = 0;
492
493     // adjust altitude (feet) and set new elevation
494     altitude_ft = pos.getElevationFt();
495     altitude_ft += vs * dt;
496     pos.setElevationFt(altitude_ft);
497
498     // recalculate total speed
499     if ( vs == 0 && hs == 0)
500         speed = 0;
501     else
502         speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
503
504     // recalculate elevation and azimuth (velocity vectors)
505     _elevation = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
506     _azimuth =  atan2((speed_east_fps + force_speed_east_fps), 
507         (speed_north_fps + force_speed_north_fps)) * SG_RADIANS_TO_DEGREES;
508
509     // rationalise azimuth
510     if (_azimuth < 0)
511         _azimuth += 360;
512
513     if (_aero_stabilised) { // we simulate rotational moment of inertia by using a filter
514         const double coeff = 0.9;
515
516         // we assume a symetrical MI about the pitch and yaw axis
517         setPitch(_elevation, dt, coeff);
518         setHdg(dt, coeff);
519
520     } else if (_force_stabilised) { // we simulate rotational moment of inertia by using a filter
521         const double coeff = 0.9;
522         double ratio = h_force_lbs/(_mass * slugs_to_lbs);
523
524         double force_pitch = acos(ratio) * SG_RADIANS_TO_DEGREES;
525
526         if (force_pitch <= force_elevation_deg)
527             force_pitch = force_elevation_deg;
528
529         // we assume a symetrical MI about the pitch and yaw axis
530         setPitch(force_pitch,dt, coeff);
531         setHdg(dt, coeff);
532     }
533
534     //do impacts and collisions
535     if (_report_impact && !_impact_reported)
536         handle_impact();
537
538     if (_report_collision && !_collision_reported)
539         handle_collision();
540
541     // set destruction flag if altitude less than sea level -1000
542     if (altitude_ft < -1000.0)
543         setDie(true);
544
545 }  // end Run
546
547 double FGAIBallistic::_getTime() const {
548     return _life_timer;
549 }
550
551 void FGAIBallistic::handle_impact() {
552
553     // try terrain intersection
554     if(!getHtAGL()) 
555         return;
556
557     if (_ht_agl_ft <= 0) {
558         SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: terrain impact");
559         report_impact(_elevation_m);
560         _impact_reported = true;
561
562         // kill the AIObject if there is no subsubmodel
563         if (_subID == 0)
564             setDie(true);
565     }
566 }
567
568 void FGAIBallistic::handle_collision()
569 {
570     const FGAIBase *object = manager->calcCollision(pos.getElevationFt(),
571             pos.getLatitudeDeg(),pos.getLongitudeDeg(), _fuse_range);
572
573     if (object) {
574         SG_LOG(SG_GENERAL, SG_DEBUG, "AIBallistic: object hit");
575         report_impact(pos.getElevationM(), object);
576         _collision_reported = true;
577     }
578 }
579
580 void FGAIBallistic::report_impact(double elevation, const FGAIBase *object)
581 {
582     _impact_lat    = pos.getLatitudeDeg();
583     _impact_lon    = pos.getLongitudeDeg();
584     _impact_elev   = elevation;
585     _impact_speed  = speed * SG_KT_TO_MPS;
586     _impact_hdg    = hdg;
587     _impact_pitch  = pitch;
588     _impact_roll   = roll;
589
590     SGPropertyNode *n = props->getNode("impact", true);
591     if (object)
592         n->setStringValue("type", object->getTypeString());
593     else
594         n->setStringValue("type", "terrain");
595
596     n->setDoubleValue("longitude-deg", _impact_lon);
597     n->setDoubleValue("latitude-deg", _impact_lat);
598     n->setDoubleValue("elevation-m", _impact_elev);
599     n->setDoubleValue("heading-deg", _impact_hdg);
600     n->setDoubleValue("pitch-deg", _impact_pitch);
601     n->setDoubleValue("roll-deg", _impact_roll);
602     n->setDoubleValue("speed-mps", _impact_speed);
603
604     _impact_report_node->setStringValue(props->getPath());
605 }
606
607 // end AIBallistic
608