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