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