1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
\r
3 // Based on FGAIAircraft
\r
4 // Written by David Culp, started October 2003.
\r
5 // Also by Gregor Richards, started December 2005.
\r
7 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
\r
8 // Copyright (C) 2005 Gregor Richards
\r
10 // This program is free software; you can redistribute it and/or
\r
11 // modify it under the terms of the GNU General Public License as
\r
12 // published by the Free Software Foundation; either version 2 of the
\r
13 // License, or (at your option) any later version.
\r
15 // This program is distributed in the hope that it will be useful, but
\r
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
\r
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
\r
18 // General Public License for more details.
\r
20 // You should have received a copy of the GNU General Public License
\r
21 // along with this program; if not, write to the Free Software
\r
22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
\r
24 #ifdef HAVE_CONFIG_H
\r
25 # include <config.h>
\r
30 #include "AIMultiplayer.hxx"
\r
32 #include <simgear/scene/util/SGNodeMasks.hxx>
\r
34 // #define SG_DEBUG SG_ALERT
\r
36 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
\r
39 mTimeOffsetSet = false;
\r
40 mAllowExtrapolation = true;
\r
41 mLagAdjustSystemSpeed = 10;
\r
43 aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT);
\r
48 FGAIMultiplayer::~FGAIMultiplayer() {
\r
51 bool FGAIMultiplayer::init(bool search_in_AI_path) {
\r
52 props->setStringValue("sim/model/path", model_path.c_str());
\r
53 //refuel_node = fgGetNode("systems/refuel/contact", true);
\r
54 isTanker = false; // do this until this property is
\r
55 // passed over the net
\r
57 string str1 = _getCallsign();
\r
58 string str2 = "MOBIL";
\r
60 string::size_type loc1= str1.find( str2, 0 );
\r
61 if ( (loc1 != string::npos && str2 != "") ){
\r
62 // cout << " string found " << str2 << " in " << str1 << endl;
\r
64 // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
\r
66 return FGAIBase::init(search_in_AI_path);
\r
69 void FGAIMultiplayer::bind() {
\r
72 props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));
\r
73 props->tie("tanker", SGRawValuePointer<bool>(&isTanker));
\r
75 props->tie("controls/invisible",
\r
76 SGRawValuePointer<bool>(&invisible));
\r
78 #define AIMPROProp(type, name) \
\r
79 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
\r
81 #define AIMPRWProp(type, name) \
\r
82 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
\r
83 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
\r
85 //props->tie("callsign", AIMPROProp(const char *, CallSign));
\r
87 props->tie("controls/allow-extrapolation",
\r
88 AIMPRWProp(bool, AllowExtrapolation));
\r
89 props->tie("controls/lag-adjust-system-speed",
\r
90 AIMPRWProp(double, LagAdjustSystemSpeed));
\r
97 void FGAIMultiplayer::unbind() {
\r
100 //props->untie("callsign");
\r
101 props->untie("controls/allow-extrapolation");
\r
102 props->untie("controls/lag-adjust-system-speed");
\r
103 props->untie("controls/invisible");
\r
104 props->untie("refuel/contact");
\r
107 void FGAIMultiplayer::update(double dt)
\r
109 using namespace simgear;
\r
114 FGAIBase::update(dt);
\r
116 // Check if we already got data
\r
117 if (mMotionInfo.empty())
\r
120 // The current simulation time we need to update for,
\r
121 // note that the simulation time is updated before calling all the
\r
122 // update methods. Thus it contains the time intervals *end* time
\r
123 double curtime = globals->get_sim_time_sec();
\r
125 // Get the last available time
\r
126 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
\r
127 double curentPkgTime = it->second.time;
\r
129 // Dynamically optimize the time offset between the feeder and the client
\r
130 // Well, 'dynamically' means that the dynamic of that update must be very
\r
131 // slow. You would otherwise notice huge jumps in the multiplayer models.
\r
132 // The reason is that we want to avoid huge extrapolation times since
\r
133 // extrapolation is highly error prone. For that we need something
\r
134 // approaching the average latency of the packets. This first order lag
\r
135 // component will provide this. We just take the error of the currently
\r
136 // requested time to the most recent available packet. This is the
\r
137 // target we want to reach in average.
\r
138 double lag = it->second.lag;
\r
139 if (!mTimeOffsetSet) {
\r
140 mTimeOffsetSet = true;
\r
141 mTimeOffset = curentPkgTime - curtime - lag;
\r
143 double offset = curentPkgTime - curtime - lag;
\r
144 if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
\r
145 || (offset - 10 > mTimeOffset)) {
\r
146 mTimeOffset = offset;
\r
147 SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
\r
148 "avoid extrapolation: time offset = " << mTimeOffset);
\r
150 // the error of the offset, respectively the negative error to avoid
\r
151 // a minus later ...
\r
152 double err = offset - mTimeOffset;
\r
153 // limit errors leading to shorter lag values somehow, that is late
\r
154 // arriving packets will pessimize the overall lag much more than
\r
155 // early packets will shorten the overall lag
\r
158 // Ok, we have some very late packets and nothing newer increase the
\r
159 // lag by the given speedadjust
\r
160 sysSpeed = mLagAdjustSystemSpeed*err;
\r
162 // We have a too pessimistic display delay shorten that a small bit
\r
163 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
\r
166 // simple euler integration for that first order system including some
\r
167 // overshooting guard to prevent to aggressive system speeds
\r
168 // (stiff systems) to explode the systems state
\r
169 double systemIncrement = dt*sysSpeed;
\r
170 if (fabs(err) < fabs(systemIncrement))
\r
171 systemIncrement = err;
\r
172 mTimeOffset += systemIncrement;
\r
174 SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
\r
175 << mTimeOffset << ", expected longitudinal position error due to "
\r
176 " current adjustment of the offset: "
\r
177 << fabs(norm(it->second.linearVel)*systemIncrement));
\r
182 // Compute the time in the feeders time scale which fits the current time
\r
184 double tInterp = curtime + mTimeOffset;
\r
189 if (tInterp <= curentPkgTime) {
\r
190 // Ok, we need a time prevous to the last available packet,
\r
191 // that is good ...
\r
193 // Find the first packet before the target time
\r
194 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
\r
195 if (nextIt == mMotionInfo.begin()) {
\r
196 SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
\r
197 // We have no packet before the target time, just use the first one
\r
198 MotionInfo::iterator firstIt = mMotionInfo.begin();
\r
199 ecPos = firstIt->second.position;
\r
200 ecOrient = firstIt->second.orientation;
\r
201 speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
\r
203 std::vector<FGPropertyData*>::const_iterator firstPropIt;
\r
204 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
\r
205 firstPropIt = firstIt->second.properties.begin();
\r
206 firstPropItEnd = firstIt->second.properties.end();
\r
207 while (firstPropIt != firstPropItEnd) {
\r
208 //cout << " Setting property..." << (*firstPropIt)->id;
\r
209 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
\r
210 if (pIt != mPropertyMap.end())
\r
212 //cout << "Found " << pIt->second->getPath() << ":";
\r
213 switch ((*firstPropIt)->type) {
\r
217 pIt->second->setIntValue((*firstPropIt)->int_value);
\r
218 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
\r
221 case props::DOUBLE:
\r
222 pIt->second->setFloatValue((*firstPropIt)->float_value);
\r
223 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
\r
225 case props::STRING:
\r
226 case props::UNSPECIFIED:
\r
227 pIt->second->setStringValue((*firstPropIt)->string_value);
\r
228 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
\r
231 // FIXME - currently defaults to float values
\r
232 pIt->second->setFloatValue((*firstPropIt)->float_value);
\r
233 //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
\r
239 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
\r
245 // Ok, we have really found something where our target time is in between
\r
246 // do interpolation here
\r
247 MotionInfo::iterator prevIt = nextIt;
\r
250 // Interpolation coefficient is between 0 and 1
\r
251 double intervalStart = prevIt->second.time;
\r
252 double intervalEnd = nextIt->second.time;
\r
253 double intervalLen = intervalEnd - intervalStart;
\r
254 double tau = (tInterp - intervalStart)/intervalLen;
\r
256 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
\r
257 << intervalStart << ", " << intervalEnd << "], intervalLen = "
\r
258 << intervalLen << ", interpolation parameter = " << tau);
\r
260 // Here we do just linear interpolation on the position
\r
261 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
\r
262 ecOrient = interpolate((float)tau, prevIt->second.orientation,
\r
263 nextIt->second.orientation);
\r
264 speed = norm((1-tau)*prevIt->second.linearVel
\r
265 + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
\r
267 if (prevIt->second.properties.size()
\r
268 == nextIt->second.properties.size()) {
\r
269 std::vector<FGPropertyData*>::const_iterator prevPropIt;
\r
270 std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
\r
271 std::vector<FGPropertyData*>::const_iterator nextPropIt;
\r
272 std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
\r
273 prevPropIt = prevIt->second.properties.begin();
\r
274 prevPropItEnd = prevIt->second.properties.end();
\r
275 nextPropIt = nextIt->second.properties.begin();
\r
276 nextPropItEnd = nextIt->second.properties.end();
\r
277 while (prevPropIt != prevPropItEnd) {
\r
278 PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
\r
279 //cout << " Setting property..." << (*prevPropIt)->id;
\r
281 if (pIt != mPropertyMap.end())
\r
283 //cout << "Found " << pIt->second->getPath() << ":";
\r
287 switch ((*prevPropIt)->type) {
\r
291 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
\r
292 tau*((double) (*nextPropIt)->int_value));
\r
293 pIt->second->setIntValue(ival);
\r
294 //cout << "Int: " << ival << "\n";
\r
297 case props::DOUBLE:
\r
298 val = (1-tau)*(*prevPropIt)->float_value +
\r
299 tau*(*nextPropIt)->float_value;
\r
300 //cout << "Flo: " << val << "\n";
\r
301 pIt->second->setFloatValue(val);
\r
303 case props::STRING:
\r
304 case props::UNSPECIFIED:
\r
305 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
\r
306 pIt->second->setStringValue((*nextPropIt)->string_value);
\r
309 // FIXME - currently defaults to float values
\r
310 val = (1-tau)*(*prevPropIt)->float_value +
\r
311 tau*(*nextPropIt)->float_value;
\r
312 //cout << "Unk: " << val << "\n";
\r
313 pIt->second->setFloatValue(val);
\r
319 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
\r
327 // Now throw away too old data
\r
328 if (prevIt != mMotionInfo.begin())
\r
332 MotionInfo::iterator delIt;
\r
333 delIt = mMotionInfo.begin();
\r
335 while (delIt != prevIt)
\r
337 std::vector<FGPropertyData*>::const_iterator propIt;
\r
338 std::vector<FGPropertyData*>::const_iterator propItEnd;
\r
339 propIt = delIt->second.properties.begin();
\r
340 propItEnd = delIt->second.properties.end();
\r
342 //cout << "Deleting data\n";
\r
344 while (propIt != propItEnd)
\r
353 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
\r
357 // Ok, we need to predict the future, so, take the best data we can have
\r
358 // and do some eom computation to guess that for now.
\r
359 FGExternalMotionData motionInfo = it->second;
\r
361 // The time to predict, limit to 5 seconds
\r
362 double t = tInterp - motionInfo.time;
\r
363 t = SGMisc<double>::min(t, 5);
\r
365 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
\r
366 "extrapolation time = " << t);
\r
368 // Do a few explicit euler steps with the constant acceleration's
\r
369 // This must be sufficient ...
\r
370 ecPos = motionInfo.position;
\r
371 ecOrient = motionInfo.orientation;
\r
372 SGVec3f linearVel = motionInfo.linearVel;
\r
373 SGVec3f angularVel = motionInfo.angularVel;
\r
379 SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
\r
381 ecOrient += h*ecOrient.derivative(angularVel);
\r
383 linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
\r
384 angularVel += h*motionInfo.angularAccel;
\r
389 std::vector<FGPropertyData*>::const_iterator firstPropIt;
\r
390 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
\r
391 speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
\r
392 firstPropIt = it->second.properties.begin();
\r
393 firstPropItEnd = it->second.properties.end();
\r
394 while (firstPropIt != firstPropItEnd) {
\r
395 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
\r
396 //cout << " Setting property..." << (*firstPropIt)->id;
\r
398 if (pIt != mPropertyMap.end())
\r
400 switch ((*firstPropIt)->type) {
\r
404 pIt->second->setIntValue((*firstPropIt)->int_value);
\r
405 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
\r
408 case props::DOUBLE:
\r
409 pIt->second->setFloatValue((*firstPropIt)->float_value);
\r
410 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
\r
412 case props::STRING:
\r
413 case props::UNSPECIFIED:
\r
414 pIt->second->setStringValue((*firstPropIt)->string_value);
\r
415 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
\r
418 // FIXME - currently defaults to float values
\r
419 pIt->second->setFloatValue((*firstPropIt)->float_value);
\r
420 //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
\r
426 SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
\r
433 // extract the position
\r
434 pos = SGGeod::fromCart(ecPos);
\r
435 altitude_ft = pos.getElevationFt();
\r
437 // The quaternion rotating from the earth centered frame to the
\r
438 // horizontal local frame
\r
439 SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
\r
440 (float)pos.getLatitudeRad());
\r
441 // The orientation wrt the horizontal local frame
\r
442 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
\r
443 float hDeg, pDeg, rDeg;
\r
444 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
\r
449 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
\r
450 << ecPos << ", " << hlOr);
\r
452 //###########################//
\r
453 // do calculations for radar //
\r
454 //###########################//
\r
455 double range_ft2 = UpdateRadar(manager);
\r
457 //************************************//
\r
459 //************************************//
\r
463 //cout << "IS tanker ";
\r
464 if ( (range_ft2 < 250.0 * 250.0) &&
\r
466 (elevation > 0.0) ){
\r
467 // refuel_node->setBoolValue(true);
\r
468 //cout << "in contact" << endl;
\r
471 // refuel_node->setBoolValue(false);
\r
472 //cout << "not in contact" << endl;
\r
476 //cout << "NOT tanker " << endl;
\r
484 FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
\r
487 mLastTimestamp = stamp;
\r
489 if (!mMotionInfo.empty()) {
\r
490 double diff = motionInfo.time - mMotionInfo.rbegin()->first;
\r
492 // packet is very old -- MP has probably reset (incl. his timebase)
\r
494 mMotionInfo.clear();
\r
496 // drop packets arriving out of order
\r
497 else if (diff < 0.0)
\r
500 mMotionInfo[motionInfo.time] = motionInfo;
\r
504 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
\r
506 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
\r
507 pNode->setDoubleValue(val);
\r