-// FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
-//
-// Based on FGAIAircraft
-// Written by David Culp, started October 2003.
-// Also by Gregor Richards, started December 2005.
-//
-// Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
-// Copyright (C) 2005 Gregor Richards
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of the
-// License, or (at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful, but
-// WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
-
-#ifdef HAVE_CONFIG_H
-# include <config.h>
-#endif
-
-#include <string>
-
-#include "AIMultiplayer.hxx"
-
-#include <simgear/scene/util/SGNodeMasks.hxx>
-
-// #define SG_DEBUG SG_ALERT
-
-FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
- no_roll = false;
-
- mTimeOffsetSet = false;
- mAllowExtrapolation = true;
- mLagAdjustSystemSpeed = 10;
-
- aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT);
-
-}
-
-
-FGAIMultiplayer::~FGAIMultiplayer() {
-}
-
-bool FGAIMultiplayer::init(bool search_in_AI_path) {
- props->setStringValue("sim/model/path", model_path.c_str());
- //refuel_node = fgGetNode("systems/refuel/contact", true);
- isTanker = false; // do this until this property is
- // passed over the net
-
- string str1 = _getCallsign();
- string str2 = "MOBIL";
-
- string::size_type loc1= str1.find( str2, 0 );
- if ( (loc1 != string::npos && str2 != "") ){
- // cout << " string found " << str2 << " in " << str1 << endl;
- isTanker = true;
- // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
- }
- return FGAIBase::init(search_in_AI_path);
-}
-
-void FGAIMultiplayer::bind() {
- FGAIBase::bind();
-
- props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));
- props->setBoolValue("tanker",isTanker);
-
-#define AIMPROProp(type, name) \
-SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
-
-#define AIMPRWProp(type, name) \
-SGRawValueMethods<FGAIMultiplayer, type>(*this, \
- &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
-
- //props->tie("callsign", AIMPROProp(const char *, CallSign));
-
- props->tie("controls/allow-extrapolation",
- AIMPRWProp(bool, AllowExtrapolation));
- props->tie("controls/lag-adjust-system-speed",
- AIMPRWProp(double, LagAdjustSystemSpeed));
-
-
-#undef AIMPROProp
-#undef AIMPRWProp
-}
-
-void FGAIMultiplayer::unbind() {
- FGAIBase::unbind();
-
- //props->untie("callsign");
- props->untie("controls/allow-extrapolation");
- props->untie("controls/lag-adjust-system-speed");
- props->untie("refuel/contact");
-}
-
-void FGAIMultiplayer::update(double dt)
-{
- using namespace simgear;
-
- if (dt <= 0)
- return;
-
- FGAIBase::update(dt);
-
- // Check if we already got data
- if (mMotionInfo.empty())
- return;
-
- // The current simulation time we need to update for,
- // note that the simulation time is updated before calling all the
- // update methods. Thus it contains the time intervals *end* time
- double curtime = globals->get_sim_time_sec();
-
- // Get the last available time
- MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
- double curentPkgTime = it->second.time;
-
- // Dynamically optimize the time offset between the feeder and the client
- // Well, 'dynamically' means that the dynamic of that update must be very
- // slow. You would otherwise notice huge jumps in the multiplayer models.
- // The reason is that we want to avoid huge extrapolation times since
- // extrapolation is highly error prone. For that we need something
- // approaching the average latency of the packets. This first order lag
- // component will provide this. We just take the error of the currently
- // requested time to the most recent available packet. This is the
- // target we want to reach in average.
- double lag = it->second.lag;
- if (!mTimeOffsetSet) {
- mTimeOffsetSet = true;
- mTimeOffset = curentPkgTime - curtime - lag;
- } else {
- double offset = curentPkgTime - curtime - lag;
- if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
- || (offset - 10 > mTimeOffset)) {
- mTimeOffset = offset;
- SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
- "avoid extrapolation: time offset = " << mTimeOffset);
- } else {
- // the error of the offset, respectively the negative error to avoid
- // a minus later ...
- double err = offset - mTimeOffset;
- // limit errors leading to shorter lag values somehow, that is late
- // arriving packets will pessimize the overall lag much more than
- // early packets will shorten the overall lag
- double sysSpeed;
- if (err < 0) {
- // Ok, we have some very late packets and nothing newer increase the
- // lag by the given speedadjust
- sysSpeed = mLagAdjustSystemSpeed*err;
- } else {
- // We have a too pessimistic display delay shorten that a small bit
- sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
- }
-
- // simple euler integration for that first order system including some
- // overshooting guard to prevent to aggressive system speeds
- // (stiff systems) to explode the systems state
- double systemIncrement = dt*sysSpeed;
- if (fabs(err) < fabs(systemIncrement))
- systemIncrement = err;
- mTimeOffset += systemIncrement;
-
- SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
- << mTimeOffset << ", expected longitudinal position error due to "
- " current adjustment of the offset: "
- << fabs(norm(it->second.linearVel)*systemIncrement));
- }
- }
-
-
- // Compute the time in the feeders time scale which fits the current time
- // we need to
- double tInterp = curtime + mTimeOffset;
-
- SGVec3d ecPos;
- SGQuatf ecOrient;
-
- if (tInterp <= curentPkgTime) {
- // Ok, we need a time prevous to the last available packet,
- // that is good ...
-
- // Find the first packet before the target time
- MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
- if (nextIt == mMotionInfo.begin()) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
- // We have no packet before the target time, just use the first one
- MotionInfo::iterator firstIt = mMotionInfo.begin();
- ecPos = firstIt->second.position;
- ecOrient = firstIt->second.orientation;
- speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
-
- std::vector<FGPropertyData*>::const_iterator firstPropIt;
- std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
- firstPropIt = firstIt->second.properties.begin();
- firstPropItEnd = firstIt->second.properties.end();
- while (firstPropIt != firstPropItEnd) {
- //cout << " Setting property..." << (*firstPropIt)->id;
- PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
- if (pIt != mPropertyMap.end())
- {
- //cout << "Found " << pIt->second->getPath() << ":";
- switch ((*firstPropIt)->type) {
- case props::INT:
- case props::BOOL:
- case props::LONG:
- pIt->second->setIntValue((*firstPropIt)->int_value);
- //cout << "Int: " << (*firstPropIt)->int_value << "\n";
- break;
- case props::FLOAT:
- case props::DOUBLE:
- pIt->second->setFloatValue((*firstPropIt)->float_value);
- //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
- break;
- case props::STRING:
- case props::UNSPECIFIED:
- pIt->second->setStringValue((*firstPropIt)->string_value);
- //cout << "Str: " << (*firstPropIt)->string_value << "\n";
- break;
- default:
- // FIXME - currently defaults to float values
- pIt->second->setFloatValue((*firstPropIt)->float_value);
- //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
- break;
- }
- }
- else
- {
- SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
- }
- ++firstPropIt;
- }
-
- } else {
- // Ok, we have really found something where our target time is in between
- // do interpolation here
- MotionInfo::iterator prevIt = nextIt;
- --prevIt;
-
- // Interpolation coefficient is between 0 and 1
- double intervalStart = prevIt->second.time;
- double intervalEnd = nextIt->second.time;
- double intervalLen = intervalEnd - intervalStart;
- double tau = (tInterp - intervalStart)/intervalLen;
-
- SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
- << intervalStart << ", " << intervalEnd << "], intervalLen = "
- << intervalLen << ", interpolation parameter = " << tau);
-
- // Here we do just linear interpolation on the position
- ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
- ecOrient = interpolate((float)tau, prevIt->second.orientation,
- nextIt->second.orientation);
- speed = norm((1-tau)*prevIt->second.linearVel
- + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
-
- if (prevIt->second.properties.size()
- == nextIt->second.properties.size()) {
- std::vector<FGPropertyData*>::const_iterator prevPropIt;
- std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
- std::vector<FGPropertyData*>::const_iterator nextPropIt;
- std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
- prevPropIt = prevIt->second.properties.begin();
- prevPropItEnd = prevIt->second.properties.end();
- nextPropIt = nextIt->second.properties.begin();
- nextPropItEnd = nextIt->second.properties.end();
- while (prevPropIt != prevPropItEnd) {
- PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
- //cout << " Setting property..." << (*prevPropIt)->id;
-
- if (pIt != mPropertyMap.end())
- {
- //cout << "Found " << pIt->second->getPath() << ":";
-
- int ival;
- float val;
- switch ((*prevPropIt)->type) {
- case props::INT:
- case props::BOOL:
- case props::LONG:
- ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
- tau*((double) (*nextPropIt)->int_value));
- pIt->second->setIntValue(ival);
- //cout << "Int: " << ival << "\n";
- break;
- case props::FLOAT:
- case props::DOUBLE:
- val = (1-tau)*(*prevPropIt)->float_value +
- tau*(*nextPropIt)->float_value;
- //cout << "Flo: " << val << "\n";
- pIt->second->setFloatValue(val);
- break;
- case props::STRING:
- case props::UNSPECIFIED:
- //cout << "Str: " << (*nextPropIt)->string_value << "\n";
- pIt->second->setStringValue((*nextPropIt)->string_value);
- break;
- default:
- // FIXME - currently defaults to float values
- val = (1-tau)*(*prevPropIt)->float_value +
- tau*(*nextPropIt)->float_value;
- //cout << "Unk: " << val << "\n";
- pIt->second->setFloatValue(val);
- break;
- }
- }
- else
- {
- SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
- }
-
- ++prevPropIt;
- ++nextPropIt;
- }
- }
-
- // Now throw away too old data
- if (prevIt != mMotionInfo.begin())
- {
- --prevIt;
-
- MotionInfo::iterator delIt;
- delIt = mMotionInfo.begin();
-
- while (delIt != prevIt)
- {
- std::vector<FGPropertyData*>::const_iterator propIt;
- std::vector<FGPropertyData*>::const_iterator propItEnd;
- propIt = delIt->second.properties.begin();
- propItEnd = delIt->second.properties.end();
-
- //cout << "Deleting data\n";
-
- while (propIt != propItEnd)
- {
- delete *propIt;
- propIt++;
- }
-
- delIt++;
- }
-
- mMotionInfo.erase(mMotionInfo.begin(), prevIt);
- }
- }
- } else {
- // Ok, we need to predict the future, so, take the best data we can have
- // and do some eom computation to guess that for now.
- FGExternalMotionData motionInfo = it->second;
-
- // The time to predict, limit to 5 seconds
- double t = tInterp - motionInfo.time;
- t = SGMisc<double>::min(t, 5);
-
- SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
- "extrapolation time = " << t);
-
- // Do a few explicit euler steps with the constant acceleration's
- // This must be sufficient ...
- ecPos = motionInfo.position;
- ecOrient = motionInfo.orientation;
- SGVec3f linearVel = motionInfo.linearVel;
- SGVec3f angularVel = motionInfo.angularVel;
- while (0 < t) {
- double h = 1e-1;
- if (t < h)
- h = t;
-
- SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
- ecPos += h*ecVel;
- ecOrient += h*ecOrient.derivative(angularVel);
-
- linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
- angularVel += h*motionInfo.angularAccel;
-
- t -= h;
- }
-
- std::vector<FGPropertyData*>::const_iterator firstPropIt;
- std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
- speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
- firstPropIt = it->second.properties.begin();
- firstPropItEnd = it->second.properties.end();
- while (firstPropIt != firstPropItEnd) {
- PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
- //cout << " Setting property..." << (*firstPropIt)->id;
-
- if (pIt != mPropertyMap.end())
- {
- switch ((*firstPropIt)->type) {
- case props::INT:
- case props::BOOL:
- case props::LONG:
- pIt->second->setIntValue((*firstPropIt)->int_value);
- //cout << "Int: " << (*firstPropIt)->int_value << "\n";
- break;
- case props::FLOAT:
- case props::DOUBLE:
- pIt->second->setFloatValue((*firstPropIt)->float_value);
- //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
- break;
- case props::STRING:
- case props::UNSPECIFIED:
- pIt->second->setStringValue((*firstPropIt)->string_value);
- //cout << "Str: " << (*firstPropIt)->string_value << "\n";
- break;
- default:
- // FIXME - currently defaults to float values
- pIt->second->setFloatValue((*firstPropIt)->float_value);
- //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
- break;
- }
- }
- else
- {
- SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
- }
-
- ++firstPropIt;
- }
- }
-
- // extract the position
- pos = SGGeod::fromCart(ecPos);
- altitude_ft = pos.getElevationFt();
-
- // The quaternion rotating from the earth centered frame to the
- // horizontal local frame
- SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
- (float)pos.getLatitudeRad());
- // The orientation wrt the horizontal local frame
- SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
- float hDeg, pDeg, rDeg;
- hlOr.getEulerDeg(hDeg, pDeg, rDeg);
- hdg = hDeg;
- roll = rDeg;
- pitch = pDeg;
-
- SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
- << ecPos << ", " << hlOr);
-
- //###########################//
- // do calculations for radar //
- //###########################//
- double range_ft2 = UpdateRadar(manager);
-
- //************************************//
- // Tanker code //
- //************************************//
-
-
- if ( isTanker) {
- if ( (range_ft2 < 250.0 * 250.0) &&
- (y_shift > 0.0) &&
- (elevation > 0.0) ){
- // refuel_node->setBoolValue(true);
- contact = true;
- } else {
- // refuel_node->setBoolValue(false);
- contact = false;
- }
- } else {
- contact = false;
- }
-
- Transform();
-}
-
-void
-FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
- long stamp)
-{
- mLastTimestamp = stamp;
-
- if (!mMotionInfo.empty()) {
- double diff = motionInfo.time - mMotionInfo.rbegin()->first;
-
- // packet is very old -- MP has probably reset (incl. his timebase)
- if (diff < -10.0)
- mMotionInfo.clear();
-
- // drop packets arriving out of order
- else if (diff < 0.0)
- return;
- }
- mMotionInfo[motionInfo.time] = motionInfo;
-}
-
-void
-FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
-{
- SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
- pNode->setDoubleValue(val);
-}
+// FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft\r
+//\r
+// Based on FGAIAircraft\r
+// Written by David Culp, started October 2003.\r
+// Also by Gregor Richards, started December 2005.\r
+//\r
+// Copyright (C) 2003 David P. Culp - davidculp2@comcast.net\r
+// Copyright (C) 2005 Gregor Richards\r
+//\r
+// This program is free software; you can redistribute it and/or\r
+// modify it under the terms of the GNU General Public License as\r
+// published by the Free Software Foundation; either version 2 of the\r
+// License, or (at your option) any later version.\r
+//\r
+// This program is distributed in the hope that it will be useful, but\r
+// WITHOUT ANY WARRANTY; without even the implied warranty of\r
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r
+// General Public License for more details.\r
+//\r
+// You should have received a copy of the GNU General Public License\r
+// along with this program; if not, write to the Free Software\r
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\r
+\r
+#ifdef HAVE_CONFIG_H\r
+# include <config.h>\r
+#endif\r
+\r
+#include <string>\r
+\r
+#include "AIMultiplayer.hxx"\r
+\r
+#include <simgear/scene/util/SGNodeMasks.hxx>\r
+\r
+// #define SG_DEBUG SG_ALERT\r
+\r
+FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {\r
+ no_roll = false;\r
+\r
+ mTimeOffsetSet = false;\r
+ mAllowExtrapolation = true;\r
+ mLagAdjustSystemSpeed = 10;\r
+\r
+ aip.getSceneGraph()->setNodeMask(~SG_NODEMASK_TERRAIN_BIT);\r
+\r
+}\r
+\r
+\r
+FGAIMultiplayer::~FGAIMultiplayer() {\r
+}\r
+\r
+bool FGAIMultiplayer::init(bool search_in_AI_path) {\r
+ props->setStringValue("sim/model/path", model_path.c_str());\r
+ //refuel_node = fgGetNode("systems/refuel/contact", true);\r
+ isTanker = false; // do this until this property is\r
+ // passed over the net\r
+\r
+ string str1 = _getCallsign();\r
+ string str2 = "MOBIL";\r
+\r
+ string::size_type loc1= str1.find( str2, 0 );\r
+ if ( (loc1 != string::npos && str2 != "") ){\r
+ // cout << " string found " << str2 << " in " << str1 << endl;\r
+ isTanker = true;\r
+ // cout << "isTanker " << isTanker << " " << mCallSign <<endl;\r
+ }\r
+ return FGAIBase::init(search_in_AI_path);\r
+}\r
+\r
+void FGAIMultiplayer::bind() {\r
+ FGAIBase::bind();\r
+\r
+ props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));\r
+ props->tie("tanker", SGRawValuePointer<bool>(&isTanker));\r
+\r
+ props->tie("controls/invisible",\r
+ SGRawValuePointer<bool>(&invisible));\r
+\r
+#define AIMPROProp(type, name) \\r
+SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)\r
+\r
+#define AIMPRWProp(type, name) \\r
+SGRawValueMethods<FGAIMultiplayer, type>(*this, \\r
+ &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)\r
+\r
+ //props->tie("callsign", AIMPROProp(const char *, CallSign));\r
+\r
+ props->tie("controls/allow-extrapolation",\r
+ AIMPRWProp(bool, AllowExtrapolation));\r
+ props->tie("controls/lag-adjust-system-speed",\r
+ AIMPRWProp(double, LagAdjustSystemSpeed));\r
+\r
+\r
+#undef AIMPROProp\r
+#undef AIMPRWProp\r
+}\r
+\r
+void FGAIMultiplayer::unbind() {\r
+ FGAIBase::unbind();\r
+\r
+ //props->untie("callsign");\r
+ props->untie("controls/allow-extrapolation");\r
+ props->untie("controls/lag-adjust-system-speed");\r
+ props->untie("controls/invisible");\r
+ props->untie("refuel/contact");\r
+}\r
+\r
+void FGAIMultiplayer::update(double dt)\r
+{\r
+ using namespace simgear;\r
+\r
+ if (dt <= 0)\r
+ return;\r
+\r
+ FGAIBase::update(dt);\r
+\r
+ // Check if we already got data\r
+ if (mMotionInfo.empty())\r
+ return;\r
+\r
+ // The current simulation time we need to update for,\r
+ // note that the simulation time is updated before calling all the\r
+ // update methods. Thus it contains the time intervals *end* time\r
+ double curtime = globals->get_sim_time_sec();\r
+\r
+ // Get the last available time\r
+ MotionInfo::reverse_iterator it = mMotionInfo.rbegin();\r
+ double curentPkgTime = it->second.time;\r
+\r
+ // Dynamically optimize the time offset between the feeder and the client\r
+ // Well, 'dynamically' means that the dynamic of that update must be very\r
+ // slow. You would otherwise notice huge jumps in the multiplayer models.\r
+ // The reason is that we want to avoid huge extrapolation times since\r
+ // extrapolation is highly error prone. For that we need something\r
+ // approaching the average latency of the packets. This first order lag\r
+ // component will provide this. We just take the error of the currently\r
+ // requested time to the most recent available packet. This is the\r
+ // target we want to reach in average.\r
+ double lag = it->second.lag;\r
+ if (!mTimeOffsetSet) {\r
+ mTimeOffsetSet = true;\r
+ mTimeOffset = curentPkgTime - curtime - lag;\r
+ } else {\r
+ double offset = curentPkgTime - curtime - lag;\r
+ if ((!mAllowExtrapolation && offset + lag < mTimeOffset)\r
+ || (offset - 10 > mTimeOffset)) {\r
+ mTimeOffset = offset;\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "\r
+ "avoid extrapolation: time offset = " << mTimeOffset);\r
+ } else {\r
+ // the error of the offset, respectively the negative error to avoid\r
+ // a minus later ...\r
+ double err = offset - mTimeOffset;\r
+ // limit errors leading to shorter lag values somehow, that is late\r
+ // arriving packets will pessimize the overall lag much more than\r
+ // early packets will shorten the overall lag\r
+ double sysSpeed;\r
+ if (err < 0) {\r
+ // Ok, we have some very late packets and nothing newer increase the\r
+ // lag by the given speedadjust\r
+ sysSpeed = mLagAdjustSystemSpeed*err;\r
+ } else {\r
+ // We have a too pessimistic display delay shorten that a small bit\r
+ sysSpeed = SGMiscd::min(0.1*err*err, 0.5);\r
+ }\r
+\r
+ // simple euler integration for that first order system including some\r
+ // overshooting guard to prevent to aggressive system speeds\r
+ // (stiff systems) to explode the systems state\r
+ double systemIncrement = dt*sysSpeed;\r
+ if (fabs(err) < fabs(systemIncrement))\r
+ systemIncrement = err;\r
+ mTimeOffset += systemIncrement;\r
+ \r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "\r
+ << mTimeOffset << ", expected longitudinal position error due to "\r
+ " current adjustment of the offset: "\r
+ << fabs(norm(it->second.linearVel)*systemIncrement));\r
+ }\r
+ }\r
+\r
+\r
+ // Compute the time in the feeders time scale which fits the current time\r
+ // we need to \r
+ double tInterp = curtime + mTimeOffset;\r
+\r
+ SGVec3d ecPos;\r
+ SGQuatf ecOrient;\r
+\r
+ if (tInterp <= curentPkgTime) {\r
+ // Ok, we need a time prevous to the last available packet,\r
+ // that is good ...\r
+\r
+ // Find the first packet before the target time\r
+ MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);\r
+ if (nextIt == mMotionInfo.begin()) {\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");\r
+ // We have no packet before the target time, just use the first one\r
+ MotionInfo::iterator firstIt = mMotionInfo.begin();\r
+ ecPos = firstIt->second.position;\r
+ ecOrient = firstIt->second.orientation;\r
+ speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;\r
+\r
+ std::vector<FGPropertyData*>::const_iterator firstPropIt;\r
+ std::vector<FGPropertyData*>::const_iterator firstPropItEnd;\r
+ firstPropIt = firstIt->second.properties.begin();\r
+ firstPropItEnd = firstIt->second.properties.end();\r
+ while (firstPropIt != firstPropItEnd) {\r
+ //cout << " Setting property..." << (*firstPropIt)->id;\r
+ PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);\r
+ if (pIt != mPropertyMap.end())\r
+ {\r
+ //cout << "Found " << pIt->second->getPath() << ":";\r
+ switch ((*firstPropIt)->type) {\r
+ case props::INT:\r
+ case props::BOOL:\r
+ case props::LONG:\r
+ pIt->second->setIntValue((*firstPropIt)->int_value);\r
+ //cout << "Int: " << (*firstPropIt)->int_value << "\n";\r
+ break;\r
+ case props::FLOAT:\r
+ case props::DOUBLE:\r
+ pIt->second->setFloatValue((*firstPropIt)->float_value);\r
+ //cout << "Flo: " << (*firstPropIt)->float_value << "\n";\r
+ break;\r
+ case props::STRING:\r
+ case props::UNSPECIFIED:\r
+ pIt->second->setStringValue((*firstPropIt)->string_value);\r
+ //cout << "Str: " << (*firstPropIt)->string_value << "\n"; \r
+ break;\r
+ default:\r
+ // FIXME - currently defaults to float values\r
+ pIt->second->setFloatValue((*firstPropIt)->float_value);\r
+ //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";\r
+ break;\r
+ } \r
+ }\r
+ else\r
+ {\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");\r
+ }\r
+ ++firstPropIt;\r
+ }\r
+\r
+ } else {\r
+ // Ok, we have really found something where our target time is in between\r
+ // do interpolation here\r
+ MotionInfo::iterator prevIt = nextIt;\r
+ --prevIt;\r
+\r
+ // Interpolation coefficient is between 0 and 1\r
+ double intervalStart = prevIt->second.time;\r
+ double intervalEnd = nextIt->second.time;\r
+ double intervalLen = intervalEnd - intervalStart;\r
+ double tau = (tInterp - intervalStart)/intervalLen;\r
+\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["\r
+ << intervalStart << ", " << intervalEnd << "], intervalLen = "\r
+ << intervalLen << ", interpolation parameter = " << tau);\r
+\r
+ // Here we do just linear interpolation on the position\r
+ ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);\r
+ ecOrient = interpolate((float)tau, prevIt->second.orientation,\r
+ nextIt->second.orientation);\r
+ speed = norm((1-tau)*prevIt->second.linearVel\r
+ + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;\r
+\r
+ if (prevIt->second.properties.size()\r
+ == nextIt->second.properties.size()) {\r
+ std::vector<FGPropertyData*>::const_iterator prevPropIt;\r
+ std::vector<FGPropertyData*>::const_iterator prevPropItEnd;\r
+ std::vector<FGPropertyData*>::const_iterator nextPropIt;\r
+ std::vector<FGPropertyData*>::const_iterator nextPropItEnd;\r
+ prevPropIt = prevIt->second.properties.begin();\r
+ prevPropItEnd = prevIt->second.properties.end();\r
+ nextPropIt = nextIt->second.properties.begin();\r
+ nextPropItEnd = nextIt->second.properties.end();\r
+ while (prevPropIt != prevPropItEnd) {\r
+ PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);\r
+ //cout << " Setting property..." << (*prevPropIt)->id;\r
+ \r
+ if (pIt != mPropertyMap.end())\r
+ {\r
+ //cout << "Found " << pIt->second->getPath() << ":";\r
+ \r
+ int ival;\r
+ float val;\r
+ switch ((*prevPropIt)->type) {\r
+ case props::INT:\r
+ case props::BOOL:\r
+ case props::LONG:\r
+ ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +\r
+ tau*((double) (*nextPropIt)->int_value));\r
+ pIt->second->setIntValue(ival);\r
+ //cout << "Int: " << ival << "\n";\r
+ break;\r
+ case props::FLOAT:\r
+ case props::DOUBLE:\r
+ val = (1-tau)*(*prevPropIt)->float_value +\r
+ tau*(*nextPropIt)->float_value;\r
+ //cout << "Flo: " << val << "\n";\r
+ pIt->second->setFloatValue(val);\r
+ break;\r
+ case props::STRING:\r
+ case props::UNSPECIFIED:\r
+ //cout << "Str: " << (*nextPropIt)->string_value << "\n";\r
+ pIt->second->setStringValue((*nextPropIt)->string_value);\r
+ break;\r
+ default:\r
+ // FIXME - currently defaults to float values\r
+ val = (1-tau)*(*prevPropIt)->float_value +\r
+ tau*(*nextPropIt)->float_value;\r
+ //cout << "Unk: " << val << "\n";\r
+ pIt->second->setFloatValue(val);\r
+ break;\r
+ } \r
+ }\r
+ else\r
+ {\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");\r
+ }\r
+ \r
+ ++prevPropIt;\r
+ ++nextPropIt;\r
+ }\r
+ }\r
+\r
+ // Now throw away too old data\r
+ if (prevIt != mMotionInfo.begin()) \r
+ {\r
+ --prevIt;\r
+ \r
+ MotionInfo::iterator delIt;\r
+ delIt = mMotionInfo.begin();\r
+ \r
+ while (delIt != prevIt) \r
+ {\r
+ std::vector<FGPropertyData*>::const_iterator propIt;\r
+ std::vector<FGPropertyData*>::const_iterator propItEnd;\r
+ propIt = delIt->second.properties.begin();\r
+ propItEnd = delIt->second.properties.end();\r
+\r
+ //cout << "Deleting data\n";\r
+ \r
+ while (propIt != propItEnd)\r
+ {\r
+ delete *propIt;\r
+ propIt++;\r
+ }\r
+ \r
+ delIt++;\r
+ }\r
+ \r
+ mMotionInfo.erase(mMotionInfo.begin(), prevIt);\r
+ }\r
+ }\r
+ } else {\r
+ // Ok, we need to predict the future, so, take the best data we can have\r
+ // and do some eom computation to guess that for now.\r
+ FGExternalMotionData motionInfo = it->second;\r
+\r
+ // The time to predict, limit to 5 seconds\r
+ double t = tInterp - motionInfo.time;\r
+ t = SGMisc<double>::min(t, 5);\r
+\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "\r
+ "extrapolation time = " << t);\r
+\r
+ // Do a few explicit euler steps with the constant acceleration's\r
+ // This must be sufficient ...\r
+ ecPos = motionInfo.position;\r
+ ecOrient = motionInfo.orientation;\r
+ SGVec3f linearVel = motionInfo.linearVel;\r
+ SGVec3f angularVel = motionInfo.angularVel;\r
+ while (0 < t) {\r
+ double h = 1e-1;\r
+ if (t < h)\r
+ h = t;\r
+\r
+ SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));\r
+ ecPos += h*ecVel;\r
+ ecOrient += h*ecOrient.derivative(angularVel);\r
+\r
+ linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);\r
+ angularVel += h*motionInfo.angularAccel;\r
+ \r
+ t -= h;\r
+ }\r
+\r
+ std::vector<FGPropertyData*>::const_iterator firstPropIt;\r
+ std::vector<FGPropertyData*>::const_iterator firstPropItEnd;\r
+ speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;\r
+ firstPropIt = it->second.properties.begin();\r
+ firstPropItEnd = it->second.properties.end();\r
+ while (firstPropIt != firstPropItEnd) {\r
+ PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);\r
+ //cout << " Setting property..." << (*firstPropIt)->id;\r
+ \r
+ if (pIt != mPropertyMap.end())\r
+ {\r
+ switch ((*firstPropIt)->type) {\r
+ case props::INT:\r
+ case props::BOOL:\r
+ case props::LONG:\r
+ pIt->second->setIntValue((*firstPropIt)->int_value);\r
+ //cout << "Int: " << (*firstPropIt)->int_value << "\n";\r
+ break;\r
+ case props::FLOAT:\r
+ case props::DOUBLE:\r
+ pIt->second->setFloatValue((*firstPropIt)->float_value);\r
+ //cout << "Flo: " << (*firstPropIt)->float_value << "\n";\r
+ break;\r
+ case props::STRING:\r
+ case props::UNSPECIFIED:\r
+ pIt->second->setStringValue((*firstPropIt)->string_value);\r
+ //cout << "Str: " << (*firstPropIt)->string_value << "\n";\r
+ break;\r
+ default:\r
+ // FIXME - currently defaults to float values\r
+ pIt->second->setFloatValue((*firstPropIt)->float_value);\r
+ //cout << "Unk: " << (*firstPropIt)->float_value << "\n";\r
+ break;\r
+ } \r
+ }\r
+ else\r
+ {\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");\r
+ }\r
+ \r
+ ++firstPropIt;\r
+ }\r
+ }\r
+ \r
+ // extract the position\r
+ pos = SGGeod::fromCart(ecPos);\r
+ altitude_ft = pos.getElevationFt();\r
+\r
+ // The quaternion rotating from the earth centered frame to the\r
+ // horizontal local frame\r
+ SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),\r
+ (float)pos.getLatitudeRad());\r
+ // The orientation wrt the horizontal local frame\r
+ SGQuatf hlOr = conj(qEc2Hl)*ecOrient;\r
+ float hDeg, pDeg, rDeg;\r
+ hlOr.getEulerDeg(hDeg, pDeg, rDeg);\r
+ hdg = hDeg;\r
+ roll = rDeg;\r
+ pitch = pDeg;\r
+\r
+ SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "\r
+ << ecPos << ", " << hlOr);\r
+\r
+ //###########################//\r
+ // do calculations for radar //\r
+ //###########################//\r
+ double range_ft2 = UpdateRadar(manager);\r
+\r
+ //************************************//\r
+ // Tanker code //\r
+ //************************************//\r
+\r
+\r
+ if ( isTanker) {\r
+ //cout << "IS tanker ";\r
+ if ( (range_ft2 < 250.0 * 250.0) &&\r
+ (y_shift > 0.0) &&\r
+ (elevation > 0.0) ){\r
+ // refuel_node->setBoolValue(true);\r
+ //cout << "in contact" << endl;\r
+ contact = true;\r
+ } else {\r
+ // refuel_node->setBoolValue(false);\r
+ //cout << "not in contact" << endl;\r
+ contact = false;\r
+ }\r
+ } else {\r
+ //cout << "NOT tanker " << endl;\r
+ contact = false;\r
+ }\r
+\r
+ Transform();\r
+}\r
+\r
+void\r
+FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,\r
+ long stamp)\r
+{\r
+ mLastTimestamp = stamp;\r
+\r
+ if (!mMotionInfo.empty()) {\r
+ double diff = motionInfo.time - mMotionInfo.rbegin()->first;\r
+\r
+ // packet is very old -- MP has probably reset (incl. his timebase)\r
+ if (diff < -10.0)\r
+ mMotionInfo.clear();\r
+\r
+ // drop packets arriving out of order\r
+ else if (diff < 0.0)\r
+ return;\r
+ }\r
+ mMotionInfo[motionInfo.time] = motionInfo;\r
+}\r
+\r
+void\r
+FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)\r
+{\r
+ SGPropertyNode* pNode = props->getChild(prop.c_str(), true);\r
+ pNode->setDoubleValue(val);\r
+}\r
projects / flightgear.git / blobdiff