1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
3 // Based on FGAIAircraft
4 // Written by David Culp, started October 2003.
5 // Also by Gregor Richards, started December 2005.
7 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
8 // Copyright (C) 2005 Gregor Richards
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.
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.
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.
30 #include "AIMultiplayer.hxx"
32 // #define SG_DEBUG SG_ALERT
34 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
37 mTimeOffsetSet = false;
38 mAllowExtrapolation = true;
39 mLagAdjustSystemSpeed = 10;
43 FGAIMultiplayer::~FGAIMultiplayer() {
46 bool FGAIMultiplayer::init(bool search_in_AI_path) {
47 props->setStringValue("sim/model/path", model_path.c_str());
48 //refuel_node = fgGetNode("systems/refuel/contact", true);
49 isTanker = false; // do this until this property is
50 // passed over the net
52 string str1 = _getCallsign();
53 string str2 = "MOBIL";
55 string::size_type loc1= str1.find( str2, 0 );
56 if ( (loc1 != string::npos && str2 != "") ){
57 // cout << " string found " << str2 << " in " << str1 << endl;
59 // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
61 return FGAIBase::init(search_in_AI_path);
64 void FGAIMultiplayer::bind() {
67 props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));
68 props->setBoolValue("tanker",isTanker);
70 #define AIMPROProp(type, name) \
71 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
73 #define AIMPRWProp(type, name) \
74 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
75 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
77 //props->tie("callsign", AIMPROProp(const char *, CallSign));
79 props->tie("controls/allow-extrapolation",
80 AIMPRWProp(bool, AllowExtrapolation));
81 props->tie("controls/lag-adjust-system-speed",
82 AIMPRWProp(double, LagAdjustSystemSpeed));
89 void FGAIMultiplayer::unbind() {
92 //props->untie("callsign");
93 props->untie("controls/allow-extrapolation");
94 props->untie("controls/lag-adjust-system-speed");
95 props->untie("refuel/contact");
98 void FGAIMultiplayer::update(double dt)
103 FGAIBase::update(dt);
105 // Check if we already got data
106 if (mMotionInfo.empty())
109 // The current simulation time we need to update for,
110 // note that the simulation time is updated before calling all the
111 // update methods. Thus it contains the time intervals *end* time
112 double curtime = globals->get_sim_time_sec();
114 // Get the last available time
115 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
116 double curentPkgTime = it->second.time;
118 // Dynamically optimize the time offset between the feeder and the client
119 // Well, 'dynamically' means that the dynamic of that update must be very
120 // slow. You would otherwise notice huge jumps in the multiplayer models.
121 // The reason is that we want to avoid huge extrapolation times since
122 // extrapolation is highly error prone. For that we need something
123 // approaching the average latency of the packets. This first order lag
124 // component will provide this. We just take the error of the currently
125 // requested time to the most recent available packet. This is the
126 // target we want to reach in average.
127 double lag = it->second.lag;
128 if (!mTimeOffsetSet) {
129 mTimeOffsetSet = true;
130 mTimeOffset = curentPkgTime - curtime - lag;
132 double offset = curentPkgTime - curtime - lag;
133 if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
134 || (offset - 10 > mTimeOffset)) {
135 mTimeOffset = offset;
136 SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
137 "avoid extrapolation: time offset = " << mTimeOffset);
139 // the error of the offset, respectively the negative error to avoid
141 double err = offset - mTimeOffset;
142 // limit errors leading to shorter lag values somehow, that is late
143 // arriving packets will pessimize the overall lag much more than
144 // early packets will shorten the overall lag
147 // Ok, we have some very late packets and nothing newer increase the
148 // lag by the given speedadjust
149 sysSpeed = mLagAdjustSystemSpeed*err;
151 // We have a too pessimistic display delay shorten that a small bit
152 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
155 // simple euler integration for that first order system including some
156 // overshooting guard to prevent to aggressive system speeds
157 // (stiff systems) to explode the systems state
158 double systemIncrement = dt*sysSpeed;
159 if (fabs(err) < fabs(systemIncrement))
160 systemIncrement = err;
161 mTimeOffset += systemIncrement;
163 SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
164 << mTimeOffset << ", expected longitudinal position error due to "
165 " current adjustment of the offset: "
166 << fabs(norm(it->second.linearVel)*systemIncrement));
171 // Compute the time in the feeders time scale which fits the current time
173 double tInterp = curtime + mTimeOffset;
178 if (tInterp <= curentPkgTime) {
179 // Ok, we need a time prevous to the last available packet,
182 // Find the first packet before the target time
183 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
184 if (nextIt == mMotionInfo.begin()) {
185 SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
186 // We have no packet before the target time, just use the first one
187 MotionInfo::iterator firstIt = mMotionInfo.begin();
188 ecPos = firstIt->second.position;
189 ecOrient = firstIt->second.orientation;
190 speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
192 std::vector<FGPropertyData*>::const_iterator firstPropIt;
193 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
194 firstPropIt = firstIt->second.properties.begin();
195 firstPropItEnd = firstIt->second.properties.end();
196 while (firstPropIt != firstPropItEnd) {
197 //cout << " Setting property..." << (*firstPropIt)->id;
198 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
199 if (pIt != mPropertyMap.end())
201 //cout << "Found " << pIt->second->getPath() << ":";
202 switch ((*firstPropIt)->type) {
203 case SGPropertyNode::INT:
204 case SGPropertyNode::BOOL:
205 case SGPropertyNode::LONG:
206 pIt->second->setIntValue((*firstPropIt)->int_value);
207 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
209 case SGPropertyNode::FLOAT:
210 case SGPropertyNode::DOUBLE:
211 pIt->second->setFloatValue((*firstPropIt)->float_value);
212 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
214 case SGPropertyNode::STRING:
215 case SGPropertyNode::UNSPECIFIED:
216 pIt->second->setStringValue((*firstPropIt)->string_value);
217 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
220 // FIXME - currently defaults to float values
221 pIt->second->setFloatValue((*firstPropIt)->float_value);
222 //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
228 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
234 // Ok, we have really found something where our target time is in between
235 // do interpolation here
236 MotionInfo::iterator prevIt = nextIt;
239 // Interpolation coefficient is between 0 and 1
240 double intervalStart = prevIt->second.time;
241 double intervalEnd = nextIt->second.time;
242 double intervalLen = intervalEnd - intervalStart;
243 double tau = (tInterp - intervalStart)/intervalLen;
245 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
246 << intervalStart << ", " << intervalEnd << "], intervalLen = "
247 << intervalLen << ", interpolation parameter = " << tau);
249 // Here we do just linear interpolation on the position
250 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
251 ecOrient = interpolate((float)tau, prevIt->second.orientation,
252 nextIt->second.orientation);
253 speed = norm((1-tau)*prevIt->second.linearVel
254 + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
256 if (prevIt->second.properties.size()
257 == nextIt->second.properties.size()) {
258 std::vector<FGPropertyData*>::const_iterator prevPropIt;
259 std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
260 std::vector<FGPropertyData*>::const_iterator nextPropIt;
261 std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
262 prevPropIt = prevIt->second.properties.begin();
263 prevPropItEnd = prevIt->second.properties.end();
264 nextPropIt = nextIt->second.properties.begin();
265 nextPropItEnd = nextIt->second.properties.end();
266 while (prevPropIt != prevPropItEnd) {
267 PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
268 //cout << " Setting property..." << (*prevPropIt)->id;
270 if (pIt != mPropertyMap.end())
272 //cout << "Found " << pIt->second->getPath() << ":";
276 switch ((*prevPropIt)->type) {
277 case SGPropertyNode::INT:
278 case SGPropertyNode::BOOL:
279 case SGPropertyNode::LONG:
280 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
281 tau*((double) (*nextPropIt)->int_value));
282 pIt->second->setIntValue(ival);
283 //cout << "Int: " << ival << "\n";
285 case SGPropertyNode::FLOAT:
286 case SGPropertyNode::DOUBLE:
287 val = (1-tau)*(*prevPropIt)->float_value +
288 tau*(*nextPropIt)->float_value;
289 //cout << "Flo: " << val << "\n";
290 pIt->second->setFloatValue(val);
292 case SGPropertyNode::STRING:
293 case SGPropertyNode::UNSPECIFIED:
294 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
295 pIt->second->setStringValue((*nextPropIt)->string_value);
298 // FIXME - currently defaults to float values
299 val = (1-tau)*(*prevPropIt)->float_value +
300 tau*(*nextPropIt)->float_value;
301 //cout << "Unk: " << val << "\n";
302 pIt->second->setFloatValue(val);
308 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
316 // Now throw away too old data
317 if (prevIt != mMotionInfo.begin())
321 MotionInfo::iterator delIt;
322 delIt = mMotionInfo.begin();
324 while (delIt != prevIt)
326 std::vector<FGPropertyData*>::const_iterator propIt;
327 std::vector<FGPropertyData*>::const_iterator propItEnd;
328 propIt = delIt->second.properties.begin();
329 propItEnd = delIt->second.properties.end();
331 //cout << "Deleting data\n";
333 while (propIt != propItEnd)
342 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
346 // Ok, we need to predict the future, so, take the best data we can have
347 // and do some eom computation to guess that for now.
348 FGExternalMotionData motionInfo = it->second;
350 // The time to predict, limit to 5 seconds
351 double t = tInterp - motionInfo.time;
352 t = SGMisc<double>::min(t, 5);
354 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
355 "extrapolation time = " << t);
357 // Do a few explicit euler steps with the constant acceleration's
358 // This must be sufficient ...
359 ecPos = motionInfo.position;
360 ecOrient = motionInfo.orientation;
361 SGVec3f linearVel = motionInfo.linearVel;
362 SGVec3f angularVel = motionInfo.angularVel;
368 SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
370 ecOrient += h*ecOrient.derivative(angularVel);
372 linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
373 angularVel += h*motionInfo.angularAccel;
378 std::vector<FGPropertyData*>::const_iterator firstPropIt;
379 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
380 speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
381 firstPropIt = it->second.properties.begin();
382 firstPropItEnd = it->second.properties.end();
383 while (firstPropIt != firstPropItEnd) {
384 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
385 //cout << " Setting property..." << (*firstPropIt)->id;
387 if (pIt != mPropertyMap.end())
389 switch ((*firstPropIt)->type) {
390 case SGPropertyNode::INT:
391 case SGPropertyNode::BOOL:
392 case SGPropertyNode::LONG:
393 pIt->second->setIntValue((*firstPropIt)->int_value);
394 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
396 case SGPropertyNode::FLOAT:
397 case SGPropertyNode::DOUBLE:
398 pIt->second->setFloatValue((*firstPropIt)->float_value);
399 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
401 case SGPropertyNode::STRING:
402 case SGPropertyNode::UNSPECIFIED:
403 pIt->second->setStringValue((*firstPropIt)->string_value);
404 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
407 // FIXME - currently defaults to float values
408 pIt->second->setFloatValue((*firstPropIt)->float_value);
409 //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
415 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
422 // extract the position
423 pos = SGGeod::fromCart(ecPos);
424 altitude_ft = pos.getElevationFt();
426 // The quaternion rotating from the earth centered frame to the
427 // horizontal local frame
428 SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
429 (float)pos.getLatitudeRad());
430 // The orientation wrt the horizontal local frame
431 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
432 float hDeg, pDeg, rDeg;
433 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
438 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
439 << ecPos << ", " << hlOr);
441 //###########################//
442 // do calculations for radar //
443 //###########################//
444 double range_ft2 = UpdateRadar(manager);
446 //************************************//
448 //************************************//
452 if ( (range_ft2 < 250.0 * 250.0) &&
455 // refuel_node->setBoolValue(true);
458 // refuel_node->setBoolValue(false);
469 FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
472 mLastTimestamp = stamp;
474 if (!mMotionInfo.empty()) {
475 double diff = motionInfo.time - mMotionInfo.rbegin()->first;
477 // packet is very old -- MP has probably reset (incl. his timebase)
481 // drop packets arriving out of order
485 mMotionInfo[motionInfo.time] = motionInfo;
489 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
491 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
492 pNode->setDoubleValue(val);