]> git.mxchange.org Git - flightgear.git/blob - src/AIModel/AIMultiplayer.cxx
Conditionally enable hot for multiplayer models
[flightgear.git] / src / AIModel / AIMultiplayer.cxx
1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
2 //
3 // Based on FGAIAircraft
4 // Written by David Culp, started October 2003.
5 // Also by Gregor Richards, started December 2005.
6 //
7 // Copyright (C) 2003  David P. Culp - davidculp2@comcast.net
8 // Copyright (C) 2005  Gregor Richards
9 //
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
14 //
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
18 // General Public License for more details.
19 //
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
23
24 #ifdef HAVE_CONFIG_H
25 #  include <config.h>
26 #endif
27
28 #include <string>
29
30 #include "AIMultiplayer.hxx"
31
32 using std::string;
33
34 // #define SG_DEBUG SG_ALERT
35
36 FGAIMultiplayer::FGAIMultiplayer() :
37    FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false))
38 {
39    no_roll = false;
40
41    mTimeOffsetSet = false;
42    mAllowExtrapolation = true;
43    mLagAdjustSystemSpeed = 10;
44    mLastTimestamp = 0;
45    lastUpdateTime = 0;
46
47
48
49 FGAIMultiplayer::~FGAIMultiplayer() {
50 }
51
52 bool FGAIMultiplayer::init(bool search_in_AI_path) {
53     props->setStringValue("sim/model/path", model_path.c_str());
54     //refuel_node = fgGetNode("systems/refuel/contact", true);
55     isTanker = false; // do this until this property is
56                       // passed over the net
57
58     const string& str1 = _getCallsign();
59     const string str2 = "MOBIL";
60
61     string::size_type loc1= str1.find( str2, 0 );
62     if ( (loc1 != string::npos && str2 != "") ){
63         //         cout << " string found "     << str2 << " in " << str1 << endl;
64         isTanker = true;
65         //         cout << "isTanker " << isTanker << " " << mCallSign <<endl;
66     }
67
68     // load model
69     bool result = FGAIBase::init(search_in_AI_path);
70     // propagate installation state (used by MP pilot list)
71     props->setBoolValue("model-installed", _installed);
72     return result;
73 }
74
75 void FGAIMultiplayer::bind() {
76     FGAIBase::bind();
77
78     tie("refuel/contact", SGRawValuePointer<bool>(&contact));
79     tie("tanker", SGRawValuePointer<bool>(&isTanker));
80
81     tie("controls/invisible",
82         SGRawValuePointer<bool>(&invisible));
83         _uBodyNode = props->getNode("velocities/uBody-fps", true);
84         _vBodyNode = props->getNode("velocities/vBody-fps", true);
85         _wBodyNode = props->getNode("velocities/wBody-fps", true);
86         
87 #define AIMPROProp(type, name) \
88 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
89
90 #define AIMPRWProp(type, name) \
91 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
92       &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
93
94     //tie("callsign", AIMPROProp(const char *, CallSign));
95
96     tie("controls/allow-extrapolation",
97         AIMPRWProp(bool, AllowExtrapolation));
98     tie("controls/lag-adjust-system-speed",
99         AIMPRWProp(double, LagAdjustSystemSpeed));
100
101
102 #undef AIMPROProp
103 #undef AIMPRWProp
104 }
105
106 void FGAIMultiplayer::update(double dt)
107 {
108   using namespace simgear;
109
110   if (dt <= 0)
111     return;
112
113   FGAIBase::update(dt);
114
115   // Check if we already got data
116   if (mMotionInfo.empty())
117     return;
118
119   // The current simulation time we need to update for,
120   // note that the simulation time is updated before calling all the
121   // update methods. Thus it contains the time intervals *end* time
122   double curtime = globals->get_sim_time_sec();
123
124   // Get the last available time
125   MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
126   double curentPkgTime = it->second.time;
127
128   // Dynamically optimize the time offset between the feeder and the client
129   // Well, 'dynamically' means that the dynamic of that update must be very
130   // slow. You would otherwise notice huge jumps in the multiplayer models.
131   // The reason is that we want to avoid huge extrapolation times since
132   // extrapolation is highly error prone. For that we need something
133   // approaching the average latency of the packets. This first order lag
134   // component will provide this. We just take the error of the currently
135   // requested time to the most recent available packet. This is the
136   // target we want to reach in average.
137   double lag = it->second.lag;
138   if (!mTimeOffsetSet) {
139     mTimeOffsetSet = true;
140     mTimeOffset = curentPkgTime - curtime - lag;
141   } else {
142     double offset = curentPkgTime - curtime - lag;
143     if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
144         || (offset - 10 > mTimeOffset)) {
145       mTimeOffset = offset;
146       SG_LOG(SG_AI, SG_DEBUG, "Resetting time offset adjust system to "
147              "avoid extrapolation: time offset = " << mTimeOffset);
148     } else {
149       // the error of the offset, respectively the negative error to avoid
150       // a minus later ...
151       double err = offset - mTimeOffset;
152       // limit errors leading to shorter lag values somehow, that is late
153       // arriving packets will pessimize the overall lag much more than
154       // early packets will shorten the overall lag
155       double sysSpeed;
156       if (err < 0) {
157         // Ok, we have some very late packets and nothing newer increase the
158         // lag by the given speedadjust
159         sysSpeed = mLagAdjustSystemSpeed*err;
160       } else {
161         // We have a too pessimistic display delay shorten that a small bit
162         sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
163       }
164
165       // simple euler integration for that first order system including some
166       // overshooting guard to prevent to aggressive system speeds
167       // (stiff systems) to explode the systems state
168       double systemIncrement = dt*sysSpeed;
169       if (fabs(err) < fabs(systemIncrement))
170         systemIncrement = err;
171       mTimeOffset += systemIncrement;
172       
173       SG_LOG(SG_AI, SG_DEBUG, "Offset adjust system: time offset = "
174              << mTimeOffset << ", expected longitudinal position error due to "
175              " current adjustment of the offset: "
176              << fabs(norm(it->second.linearVel)*systemIncrement));
177     }
178   }
179
180
181   // Compute the time in the feeders time scale which fits the current time
182   // we need to 
183   double tInterp = curtime + mTimeOffset;
184
185   SGVec3d ecPos;
186   SGQuatf ecOrient;
187   SGVec3f ecLinearVel;
188
189   if (tInterp <= curentPkgTime) {
190     // Ok, we need a time prevous to the last available packet,
191     // that is good ...
192
193     // Find the first packet before the target time
194     MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
195     if (nextIt == mMotionInfo.begin()) {
196       SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!");
197       // We have no packet before the target time, just use the first one
198       MotionInfo::iterator firstIt = mMotionInfo.begin();
199       ecPos = firstIt->second.position;
200       ecOrient = firstIt->second.orientation;
201       ecLinearVel = firstIt->second.linearVel;
202       speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
203
204       std::vector<FGPropertyData*>::const_iterator firstPropIt;
205       std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
206       firstPropIt = firstIt->second.properties.begin();
207       firstPropItEnd = firstIt->second.properties.end();
208       while (firstPropIt != firstPropItEnd) {
209         //cout << " Setting property..." << (*firstPropIt)->id;
210         PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
211         if (pIt != mPropertyMap.end())
212         {
213           //cout << "Found " << pIt->second->getPath() << ":";
214           switch ((*firstPropIt)->type) {
215             case props::INT:
216             case props::BOOL:
217             case props::LONG:
218               pIt->second->setIntValue((*firstPropIt)->int_value);
219               //cout << "Int: " << (*firstPropIt)->int_value << "\n";
220               break;
221             case props::FLOAT:
222             case props::DOUBLE:
223               pIt->second->setFloatValue((*firstPropIt)->float_value);
224               //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
225               break;
226             case props::STRING:
227             case props::UNSPECIFIED:
228               pIt->second->setStringValue((*firstPropIt)->string_value);
229               //cout << "Str: " << (*firstPropIt)->string_value << "\n";    
230               break;
231             default:
232               // FIXME - currently defaults to float values
233               pIt->second->setFloatValue((*firstPropIt)->float_value);
234               //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
235               break;
236           }            
237         }
238         else
239         {
240           SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
241         }
242         ++firstPropIt;
243       }
244
245     } else {
246       // Ok, we have really found something where our target time is in between
247       // do interpolation here
248       MotionInfo::iterator prevIt = nextIt;
249       --prevIt;
250
251       // Interpolation coefficient is between 0 and 1
252       double intervalStart = prevIt->second.time;
253       double intervalEnd = nextIt->second.time;
254       double intervalLen = intervalEnd - intervalStart;
255       double tau = (tInterp - intervalStart)/intervalLen;
256
257       SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: ["
258              << intervalStart << ", " << intervalEnd << "], intervalLen = "
259              << intervalLen << ", interpolation parameter = " << tau);
260
261       // Here we do just linear interpolation on the position
262       ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
263       ecOrient = interpolate((float)tau, prevIt->second.orientation,
264                              nextIt->second.orientation);
265       ecLinearVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel);
266       speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
267
268       if (prevIt->second.properties.size()
269           == nextIt->second.properties.size()) {
270         std::vector<FGPropertyData*>::const_iterator prevPropIt;
271         std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
272         std::vector<FGPropertyData*>::const_iterator nextPropIt;
273         std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
274         prevPropIt = prevIt->second.properties.begin();
275         prevPropItEnd = prevIt->second.properties.end();
276         nextPropIt = nextIt->second.properties.begin();
277         nextPropItEnd = nextIt->second.properties.end();
278         while (prevPropIt != prevPropItEnd) {
279           PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
280           //cout << " Setting property..." << (*prevPropIt)->id;
281           
282           if (pIt != mPropertyMap.end())
283           {
284             //cout << "Found " << pIt->second->getPath() << ":";
285           
286             int ival;
287             float val;
288             switch ((*prevPropIt)->type) {
289               case props::INT:
290               case props::BOOL:
291               case props::LONG:
292                 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
293                   tau*((double) (*nextPropIt)->int_value));
294                 pIt->second->setIntValue(ival);
295                 //cout << "Int: " << ival << "\n";
296                 break;
297               case props::FLOAT:
298               case props::DOUBLE:
299                 val = (1-tau)*(*prevPropIt)->float_value +
300                   tau*(*nextPropIt)->float_value;
301                 //cout << "Flo: " << val << "\n";
302                 pIt->second->setFloatValue(val);
303                 break;
304               case props::STRING:
305               case props::UNSPECIFIED:
306                 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
307                 pIt->second->setStringValue((*nextPropIt)->string_value);
308                 break;
309               default:
310                 // FIXME - currently defaults to float values
311                 val = (1-tau)*(*prevPropIt)->float_value +
312                   tau*(*nextPropIt)->float_value;
313                 //cout << "Unk: " << val << "\n";
314                 pIt->second->setFloatValue(val);
315                 break;
316             }            
317           }
318           else
319           {
320             SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
321           }
322           
323           ++prevPropIt;
324           ++nextPropIt;
325         }
326       }
327
328       // Now throw away too old data
329       if (prevIt != mMotionInfo.begin()) 
330       {
331         --prevIt;
332         mMotionInfo.erase(mMotionInfo.begin(), prevIt);
333       }
334     }
335   } else {
336     // Ok, we need to predict the future, so, take the best data we can have
337     // and do some eom computation to guess that for now.
338     FGExternalMotionData& motionInfo = it->second;
339
340     // The time to predict, limit to 5 seconds
341     double t = tInterp - motionInfo.time;
342     t = SGMisc<double>::min(t, 5);
343
344     SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: "
345            "extrapolation time = " << t);
346
347     // Do a few explicit euler steps with the constant acceleration's
348     // This must be sufficient ...
349     ecPos = motionInfo.position;
350     ecOrient = motionInfo.orientation;
351     ecLinearVel = motionInfo.linearVel;
352     SGVec3f angularVel = motionInfo.angularVel;
353     while (0 < t) {
354       double h = 1e-1;
355       if (t < h)
356         h = t;
357
358       SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel));
359       ecPos += h*ecVel;
360       ecOrient += h*ecOrient.derivative(angularVel);
361
362       ecLinearVel += h*(cross(ecLinearVel, angularVel) + motionInfo.linearAccel);
363       angularVel += h*motionInfo.angularAccel;
364       
365       t -= h;
366     }
367
368     std::vector<FGPropertyData*>::const_iterator firstPropIt;
369     std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
370     speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
371     firstPropIt = it->second.properties.begin();
372     firstPropItEnd = it->second.properties.end();
373     while (firstPropIt != firstPropItEnd) {
374       PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
375       //cout << " Setting property..." << (*firstPropIt)->id;
376       
377       if (pIt != mPropertyMap.end())
378       {
379         switch ((*firstPropIt)->type) {
380           case props::INT:
381           case props::BOOL:
382           case props::LONG:
383             pIt->second->setIntValue((*firstPropIt)->int_value);
384             //cout << "Int: " << (*firstPropIt)->int_value << "\n";
385             break;
386           case props::FLOAT:
387           case props::DOUBLE:
388             pIt->second->setFloatValue((*firstPropIt)->float_value);
389             //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
390             break;
391           case props::STRING:
392           case props::UNSPECIFIED:
393             pIt->second->setStringValue((*firstPropIt)->string_value);
394             //cout << "Str: " << (*firstPropIt)->string_value << "\n";
395             break;
396           default:
397             // FIXME - currently defaults to float values
398             pIt->second->setFloatValue((*firstPropIt)->float_value);
399             //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
400             break;
401         }            
402       }
403       else
404       {
405         SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
406       }
407       
408       ++firstPropIt;
409     }
410   }
411   
412   // extract the position
413   pos = SGGeod::fromCart(ecPos);
414   double recent_alt_ft = altitude_ft;
415   altitude_ft = pos.getElevationFt();
416
417   // expose a valid vertical speed
418   if (lastUpdateTime != 0)
419   {
420       double dT = curtime - lastUpdateTime;
421       double Weighting=1;
422       if (dt < 1.0)
423           Weighting = dt;
424       // simple smoothing over 1 second
425       vs = (1.0-Weighting)*vs +  Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
426   }
427   lastUpdateTime = curtime;
428
429   // The quaternion rotating from the earth centered frame to the
430   // horizontal local frame
431   SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
432                                           (float)pos.getLatitudeRad());
433   // The orientation wrt the horizontal local frame
434   SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
435   float hDeg, pDeg, rDeg;
436   hlOr.getEulerDeg(hDeg, pDeg, rDeg);
437   hdg = hDeg;
438   roll = rDeg;
439   pitch = pDeg;
440
441   // expose velocities/u,v,wbody-fps in the mp tree
442   _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET);
443   _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET);
444   _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET);
445
446   SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: "
447          << ecPos << ", " << hlOr);
448
449   //###########################//
450   // do calculations for radar //
451   //###########################//
452     double range_ft2 = UpdateRadar(manager);
453
454     //************************************//
455     // Tanker code                        //
456     //************************************//
457
458
459     if ( isTanker) {
460         //cout << "IS tanker ";
461         if ( (range_ft2 < 250.0 * 250.0) &&
462             (y_shift > 0.0)    &&
463             (elevation > 0.0) ){
464                 // refuel_node->setBoolValue(true);
465                  //cout << "in contact"  << endl;
466             contact = true;
467         } else {
468             // refuel_node->setBoolValue(false);
469             //cout << "not in contact"  << endl;
470             contact = false;
471         }
472     } else {
473         //cout << "NOT tanker " << endl;
474         contact = false;
475     }
476
477   Transform();
478 }
479
480 void
481 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
482                                long stamp)
483 {
484   mLastTimestamp = stamp;
485
486   if (!mMotionInfo.empty()) {
487     double diff = motionInfo.time - mMotionInfo.rbegin()->first;
488
489     // packet is very old -- MP has probably reset (incl. his timebase)
490     if (diff < -10.0)
491       mMotionInfo.clear();
492
493     // drop packets arriving out of order
494     else if (diff < 0.0)
495       return;
496   }
497   mMotionInfo[motionInfo.time] = motionInfo;
498   // We just copied the property (pointer) list - they are ours now. Clear the
499   // properties list in given/returned object, so former owner won't deallocate them.
500   motionInfo.properties.clear();
501 }
502
503 void
504 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
505 {
506   SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
507   pNode->setDoubleValue(val);
508 }