From ded4ec5ae30783097c0cb7235cec61b38a89206b Mon Sep 17 00:00:00 2001 From: Richard Harrison <rjh@zaretto.com> Date: Tue, 7 Feb 2017 00:37:14 +0100 Subject: [PATCH] merged --- src/AIModel/AIMultiplayer.cxx | 871 +++++++++++++++++----------------- 1 file changed, 443 insertions(+), 428 deletions(-) diff --git a/src/AIModel/AIMultiplayer.cxx b/src/AIModel/AIMultiplayer.cxx index 0e563ad19..32d6e8fb6 100644 --- a/src/AIModel/AIMultiplayer.cxx +++ b/src/AIModel/AIMultiplayer.cxx @@ -38,58 +38,58 @@ using std::string; // #define SG_DEBUG SG_ALERT FGAIMultiplayer::FGAIMultiplayer() : - FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false)) + FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false)) { - no_roll = false; + no_roll = false; - mTimeOffsetSet = false; - mAllowExtrapolation = true; - mLagAdjustSystemSpeed = 10; - mLastTimestamp = 0; - lastUpdateTime = 0; - playerLag = 0.03; - compensateLag = 1; + mTimeOffsetSet = false; + mAllowExtrapolation = true; + mLagAdjustSystemSpeed = 10; + mLastTimestamp = 0; + lastUpdateTime = 0; + playerLag = 0.03; + compensateLag = 1; -} +} 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 + 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 - const string& str1 = _getCallsign(); - const string str2 = "MOBIL"; + const string& str1 = _getCallsign(); + const 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; - } + 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; + } - // load model - bool result = FGAIBase::init(search_in_AI_path); - // propagate installation state (used by MP pilot list) - props->setBoolValue("model-installed", _installed); - return result; + // load model + bool result = FGAIBase::init(search_in_AI_path); + // propagate installation state (used by MP pilot list) + props->setBoolValue("model-installed", _installed); + return result; } void FGAIMultiplayer::bind() { - FGAIBase::bind(); + FGAIBase::bind(); - tie("refuel/contact", SGRawValuePointer<bool>(&contact)); - tie("tanker", SGRawValuePointer<bool>(&isTanker)); + tie("refuel/contact", SGRawValuePointer<bool>(&contact)); + tie("tanker", SGRawValuePointer<bool>(&isTanker)); - tie("controls/invisible", - SGRawValuePointer<bool>(&invisible)); + tie("controls/invisible", + SGRawValuePointer<bool>(&invisible)); _uBodyNode = props->getNode("velocities/uBody-fps", true); _vBodyNode = props->getNode("velocities/vBody-fps", true); _wBodyNode = props->getNode("velocities/wBody-fps", true); - + #define AIMPROProp(type, name) \ SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name) @@ -97,16 +97,16 @@ SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name) SGRawValueMethods<FGAIMultiplayer, type>(*this, \ &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name) - //tie("callsign", AIMPROProp(const char *, CallSign)); + //tie("callsign", AIMPROProp(const char *, CallSign)); - tie("controls/allow-extrapolation", - AIMPRWProp(bool, AllowExtrapolation)); - tie("controls/lag-adjust-system-speed", - AIMPRWProp(double, LagAdjustSystemSpeed)); - tie("controls/player-lag", - AIMPRWProp(double, playerLag)); - tie("controls/compensate-lag", - AIMPRWProp(int, compensateLag)); + tie("controls/allow-extrapolation", + AIMPRWProp(bool, AllowExtrapolation)); + tie("controls/lag-adjust-system-speed", + AIMPRWProp(double, LagAdjustSystemSpeed)); + tie("controls/player-lag", + AIMPRWProp(double, playerLag)); + tie("controls/compensate-lag", + AIMPRWProp(int, compensateLag)); #undef AIMPROProp #undef AIMPRWProp @@ -114,450 +114,465 @@ SGRawValueMethods<FGAIMultiplayer, type>(*this, \ void FGAIMultiplayer::update(double dt) { - using namespace simgear; + using namespace simgear; - if (dt <= 0) - return; + if (dt <= 0) + return; - FGAIBase::update(dt); + FGAIBase::update(dt); - // Check if we already got data - if (mMotionInfo.empty()) - return; + // 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(); + // 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; + // 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 = 0.0; + // 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 = 0.0; //spectator mode, more late to be in the interpolation zone - if (compensateLag == 3) { offset = curentPkgTime -curtime -lag + playerLag; + if (compensateLag == 3) { + offset = curentPkgTime - curtime - lag + playerLag; - // old behaviour - } else if (compensateLag == 1) { offset = curentPkgTime - curtime - lag; - - // using the prediction mode to display the mpaircraft in the futur/past with given playerlag value - //currently compensatelag = 2 - } else { offset = curentPkgTime - curtime + 0.48*lag + playerLag; - } - if ((!mAllowExtrapolation && offset + lag < mTimeOffset) - || (offset - 10 > mTimeOffset)) { - mTimeOffset = offset; - SG_LOG(SG_AI, 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; - - //trying to slow the rudderlag phenomenon thus using more the prediction system - //if we are off by less than 1.5s, do a little correction, and bigger step above 1.5s - if (fabs(err) < 1.5) { - if (err < 0) { - sysSpeed = mLagAdjustSystemSpeed*err*0.01; - } else { - sysSpeed = SGMiscd::min(0.5*err*err, 0.05); + // old behaviour } - } else { - if (err < 0) { + else if (compensateLag == 1) { + offset = curentPkgTime - curtime - lag; - // 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); - } - } + // using the prediction mode to display the mpaircraft in the futur/past with given playerlag value + //currently compensatelag = 2 + } + else { + offset = curentPkgTime - curtime + 0.48*lag + playerLag; + } + if ((!mAllowExtrapolation && offset + lag < mTimeOffset) + || (offset - 10 > mTimeOffset)) { + mTimeOffset = offset; + SG_LOG(SG_AI, 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; + + //trying to slow the rudderlag phenomenon thus using more the prediction system + //if we are off by less than 1.5s, do a little correction, and bigger step above 1.5s + if (fabs(err) < 1.5) { + if (err < 0) { + sysSpeed = mLagAdjustSystemSpeed*err*0.01; + } + else { + sysSpeed = SGMiscd::min(0.5*err*err, 0.05); + } + } + else { + 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_AI, 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)); - } - } + // 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_AI, 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; + // Compute the time in the feeders time scale which fits the current time + // we need to + double tInterp = curtime + mTimeOffset; - SGVec3d ecPos; - SGQuatf ecOrient; - SGVec3f ecLinearVel; + SGVec3d ecPos; + SGQuatf ecOrient; + SGVec3f ecLinearVel; - if (tInterp <= curentPkgTime) { - // Ok, we need a time prevous to the last available packet, - // that is good ... + 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_AI, 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; - ecLinearVel = firstIt->second.linearVel; - speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0; + // Find the first packet before the target time + MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp); + if (nextIt == mMotionInfo.begin()) { + SG_LOG(SG_AI, 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; + ecLinearVel = firstIt->second.linearVel; + speed = norm(ecLinearVel) * 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_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); - } - ++firstPropIt; - } + 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_AI, 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; + } + 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 = 0.0; - if (intervalLen != 0.0) tau = (tInterp - intervalStart)/intervalLen; + // Interpolation coefficient is between 0 and 1 + double intervalStart = prevIt->second.time; + double intervalEnd = nextIt->second.time; + double intervalLen = intervalEnd - intervalStart; + double tau = 0.0; + if (intervalLen != 0.0) tau = (tInterp - intervalStart) / intervalLen; - SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: [" - << intervalStart << ", " << intervalEnd << "], intervalLen = " - << intervalLen << ", interpolation parameter = " << tau); + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: [" + << intervalStart << ", " << intervalEnd << "], intervalLen = " + << intervalLen << ", interpolation parameter = " << tau); - // Here we do just linear interpolation on the position - ecPos = interpolate(tau, prevIt->second.position, nextIt->second.position); - ecOrient = interpolate((float)tau, prevIt->second.orientation, - nextIt->second.orientation); - ecLinearVel = interpolate((float)tau, prevIt->second.linearVel, nextIt->second.linearVel); - speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0; + // 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); + ecLinearVel = ((1 - tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel); + speed = norm(ecLinearVel) * 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; - /* - * RJH - 2017-01-25 - * During multiplayer operations a series of crashes were encountered that affected all players - * within range of each other and resulting in an exception being thrown at exactly the same moment in time - * (within case props::STRING: ref http://i.imgur.com/y6MBoXq.png) - * Investigation showed that the nextPropIt and prevPropIt were pointing to different properties - * which may be caused due to certain models that have overloaded mp property transmission and - * these craft have their properties truncated due to packet size. However the result of this - * will be different contents in the previous and current packets, so here we protect against - * this by only considering properties where the previous and next id are the same. - * It might be a better solution to search the previous and next lists to locate the matching id's - */ - if (*nextPropIt && (*nextPropIt)->id == (*prevPropIt)->id ) { - switch ((*prevPropIt)->type) { + 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; + /* + * RJH - 2017-01-25 + * During multiplayer operations a series of crashes were encountered that affected all players + * within range of each other and resulting in an exception being thrown at exactly the same moment in time + * (within case props::STRING: ref http://i.imgur.com/y6MBoXq.png) + * Investigation showed that the nextPropIt and prevPropIt were pointing to different properties + * which may be caused due to certain models that have overloaded mp property transmission and + * these craft have their properties truncated due to packet size. However the result of this + * will be different contents in the previous and current packets, so here we protect against + * this by only considering properties where the previous and next id are the same. + * It might be a better solution to search the previous and next lists to locate the matching id's + */ + if (*nextPropIt && (*nextPropIt)->id == (*prevPropIt)->id) { + 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_AI, SG_WARN, "MP packet mismatch during lag interpolation: " << (*prevPropIt)->id << " != " << (*nextPropIt)->id << "\n"); + } + } + else + { + SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); + } + + ++prevPropIt; + ++nextPropIt; + } + } + + // Now throw away too old data + if (prevIt != mMotionInfo.begin()) + { + --prevIt; + 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 3 seconds + double t = tInterp - motionInfo.time; + t = SGMisc<double>::min(t, 3); + + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: " + "extrapolation time = " << t); + + // using velocity and acceleration to guess a parabolic position... + ecPos = motionInfo.position; + ecOrient = motionInfo.orientation; + ecLinearVel = motionInfo.linearVel; + SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel)); + SGVec3f angularVel = motionInfo.angularVel; + SGVec3d ecAcc = toVec3d(ecOrient.backTransform(motionInfo.linearAccel)); + + double normVel = norm(ecVel); + + // not doing rotationnal prediction for small speed or rotation rate, + // to avoid agitated parked plane + if ((norm(angularVel) > 0.05) || (normVel > 1.0)) { + ecOrient += t*ecOrient.derivative(angularVel); + } + + // not using acceleration for small speed, to have better parked planes + // note that anyway acceleration is not transmit yet by mp + if (normVel > 1.0) { + ecPos += t*(ecVel + 0.5*t*ecAcc); + } + else { + ecPos += t*(ecVel); + } + + std::vector<FGPropertyData*>::const_iterator firstPropIt; + std::vector<FGPropertyData*>::const_iterator firstPropItEnd; + speed = norm(ecLinearVel) * 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: - ival = (int)(0.5 + (1 - tau)*((double)(*prevPropIt)->int_value) + - tau*((double)(*nextPropIt)->int_value)); - pIt->second->setIntValue(ival); - //cout << "Int: " << ival << "\n"; + pIt->second->setIntValue((*firstPropIt)->int_value); + //cout << "Int: " << (*firstPropIt)->int_value << "\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); + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; break; case props::STRING: case props::UNSPECIFIED: - //cout << "Str: " << (*nextPropIt)->string_value << "\n"; - pIt->second->setStringValue((*nextPropIt)->string_value); + pIt->second->setStringValue((*firstPropIt)->string_value); + //cout << "Str: " << (*firstPropIt)->string_value << "\n"; 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); + pIt->second->setFloatValue((*firstPropIt)->float_value); + //cout << "Unk: " << (*firstPropIt)->float_value << "\n"; break; } } else { - SG_LOG(SG_AI, SG_WARN, "MP packet mismatch during lag interpolation: " << (*prevPropIt)->id << " != " << (*nextPropIt)->id << "\n"); + SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); } - } - else - { - SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); - } - - ++prevPropIt; - ++nextPropIt; - } - } - // Now throw away too old data - if (prevIt != mMotionInfo.begin()) - { - --prevIt; - 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 3 seconds - double t = tInterp - motionInfo.time; - t = SGMisc<double>::min(t, 3); - - SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: " - "extrapolation time = " << t); - - // using velocity and acceleration to guess a parabolic position... - ecPos = motionInfo.position; - ecOrient = motionInfo.orientation; - ecLinearVel = motionInfo.linearVel; - SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel)); - SGVec3f angularVel = motionInfo.angularVel; - SGVec3d ecAcc = toVec3d(ecOrient.backTransform(motionInfo.linearAccel)); - - double normVel = norm(ecVel); - - // not doing rotationnal prediction for small speed or rotation rate, - // to avoid agitated parked plane - if (( norm(angularVel) > 0.05 ) || ( normVel > 1.0 )) { - ecOrient += t*ecOrient.derivative(angularVel); + ++firstPropIt; + } } - // not using acceleration for small speed, to have better parked planes - // note that anyway acceleration is not transmit yet by mp - if ( normVel > 1.0 ) { - ecPos += t*(ecVel + 0.5*t*ecAcc); - } else { - ecPos += t*(ecVel); + // extract the position + pos = SGGeod::fromCart(ecPos); + double recent_alt_ft = altitude_ft; + altitude_ft = pos.getElevationFt(); + + // expose a valid vertical speed + if (lastUpdateTime != 0) + { + double dT = curtime - lastUpdateTime; + double Weighting = 1; + if (dt < 1.0) + Weighting = dt; + // simple smoothing over 1 second + vs = (1.0 - Weighting)*vs + Weighting * (altitude_ft - recent_alt_ft) / dT * 60; + } + lastUpdateTime = curtime; + + // 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; + + // expose velocities/u,v,wbody-fps in the mp tree + _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET); + _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET); + _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET); + + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: " + << ecPos << ", " << hlOr); + + //###########################// + // do calculations for radar // + //###########################// + double range_ft2 = UpdateRadar(manager); + + //************************************// + // Tanker code // + //************************************// + + + if (isTanker) { + //cout << "IS tanker "; + if ((range_ft2 < 250.0 * 250.0) && + (y_shift > 0.0) && + (elevation > 0.0)) { + // refuel_node->setBoolValue(true); + //cout << "in contact" << endl; + contact = true; + } + else { + // refuel_node->setBoolValue(false); + //cout << "not in contact" << endl; + contact = false; + } + } + else { + //cout << "NOT tanker " << endl; + contact = false; } - std::vector<FGPropertyData*>::const_iterator firstPropIt; - std::vector<FGPropertyData*>::const_iterator firstPropItEnd; - speed = norm(ecLinearVel) * 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_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); - } - - ++firstPropIt; - } - } - - // extract the position - pos = SGGeod::fromCart(ecPos); - double recent_alt_ft = altitude_ft; - altitude_ft = pos.getElevationFt(); - - // expose a valid vertical speed - if (lastUpdateTime != 0) - { - double dT = curtime - lastUpdateTime; - double Weighting=1; - if (dt < 1.0) - Weighting = dt; - // simple smoothing over 1 second - vs = (1.0-Weighting)*vs + Weighting * (altitude_ft - recent_alt_ft) / dT * 60; - } - lastUpdateTime = curtime; - - // 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; - - // expose velocities/u,v,wbody-fps in the mp tree - _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET); - _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET); - _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET); - - SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: " - << ecPos << ", " << hlOr); - - //###########################// - // do calculations for radar // - //###########################// - double range_ft2 = UpdateRadar(manager); - - //************************************// - // Tanker code // - //************************************// - - - if ( isTanker) { - //cout << "IS tanker "; - if ( (range_ft2 < 250.0 * 250.0) && - (y_shift > 0.0) && - (elevation > 0.0) ){ - // refuel_node->setBoolValue(true); - //cout << "in contact" << endl; - contact = true; - } else { - // refuel_node->setBoolValue(false); - //cout << "not in contact" << endl; - contact = false; - } - } else { - //cout << "NOT tanker " << endl; - contact = false; - } - - Transform(); + Transform(); } void FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo, - long stamp) + long stamp) { - mLastTimestamp = stamp; + mLastTimestamp = stamp; - if (!mMotionInfo.empty()) { - double diff = motionInfo.time - mMotionInfo.rbegin()->first; + 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(); + // 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; - // We just copied the property (pointer) list - they are ours now. Clear the - // properties list in given/returned object, so former owner won't deallocate them. - motionInfo.properties.clear(); + // drop packets arriving out of order + else if (diff < 0.0) + return; + } + mMotionInfo[motionInfo.time] = motionInfo; + // We just copied the property (pointer) list - they are ours now. Clear the + // properties list in given/returned object, so former owner won't deallocate them. + motionInfo.properties.clear(); } void FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val) { - SGPropertyNode* pNode = props->getChild(prop.c_str(), true); - pNode->setDoubleValue(val); + SGPropertyNode* pNode = props->getChild(prop.c_str(), true); + pNode->setDoubleValue(val); }