diff --git a/src/AIModel/AIMultiplayer.cxx b/src/AIModel/AIMultiplayer.cxx index bd5cc0a90..f268b1822 100644 --- a/src/AIModel/AIMultiplayer.cxx +++ b/src/AIModel/AIMultiplayer.cxx @@ -1,508 +1,508 @@ -// 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 -#endif - -#include - -#include "AIMultiplayer.hxx" - -#include - -// #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 <tie("refuel/contact", SGRawValuePointer(&contact)); - props->tie("tanker", SGRawValuePointer(&isTanker)); - - props->tie("controls/invisible", - SGRawValuePointer(&invisible)); - -#define AIMPROProp(type, name) \ -SGRawValueMethods(*this, &FGAIMultiplayer::get##name) - -#define AIMPRWProp(type, name) \ -SGRawValueMethods(*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("controls/invisible"); - 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::const_iterator firstPropIt; - std::vector::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::const_iterator prevPropIt; - std::vector::const_iterator prevPropItEnd; - std::vector::const_iterator nextPropIt; - std::vector::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::const_iterator propIt; - std::vector::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::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::const_iterator firstPropIt; - std::vector::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) { - //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(); -} - -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 +// +// 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 +#endif + +#include + +#include "AIMultiplayer.hxx" + +#include + +// #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 <tie("refuel/contact", SGRawValuePointer(&contact)); + props->tie("tanker", SGRawValuePointer(&isTanker)); + + props->tie("controls/invisible", + SGRawValuePointer(&invisible)); + +#define AIMPROProp(type, name) \ +SGRawValueMethods(*this, &FGAIMultiplayer::get##name) + +#define AIMPRWProp(type, name) \ +SGRawValueMethods(*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("controls/invisible"); + 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::const_iterator firstPropIt; + std::vector::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::const_iterator prevPropIt; + std::vector::const_iterator prevPropItEnd; + std::vector::const_iterator nextPropIt; + std::vector::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::const_iterator propIt; + std::vector::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::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::const_iterator firstPropIt; + std::vector::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) { + //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(); +} + +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); +} diff --git a/src/MultiPlayer/multiplaymgr.cxx b/src/MultiPlayer/multiplaymgr.cxx index 8d4783315..f20777714 100644 --- a/src/MultiPlayer/multiplaymgr.cxx +++ b/src/MultiPlayer/multiplaymgr.cxx @@ -1,1045 +1,1045 @@ -////////////////////////////////////////////////////////////////////// -// -// multiplaymgr.hpp -// -// Written by Duncan McCreanor, started February 2003. -// duncan.mccreanor@airservicesaustralia.com -// -// Copyright (C) 2003 Airservices Australia -// Copyright (C) 2005 Oliver Schroeder -// Copyright (C) 2006 Mathias Froehlich -// -// 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. -// -// $Id$ -// -////////////////////////////////////////////////////////////////////// - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include -#include -#include -#include // isNaN -#include - -#include -#include -#include -#include - -#include -#include
-#include "multiplaymgr.hxx" -#include "mpmessages.hxx" - -using namespace std; - -#define MAX_PACKET_SIZE 1200 -#define MAX_TEXT_SIZE 128 - -// These constants are provided so that the ident -// command can list file versions -const char sMULTIPLAYMGR_BID[] = "$Id$"; -const char sMULTIPLAYMGR_HID[] = MULTIPLAYTXMGR_HID; - -// A static map of protocol property id values to property paths, -// This should be extendable dynamically for every specific aircraft ... -// For now only that static list -const FGMultiplayMgr::IdPropertyList -FGMultiplayMgr::sIdPropertyList[] = { - {100, "surface-positions/left-aileron-pos-norm", simgear::props::FLOAT}, - {101, "surface-positions/right-aileron-pos-norm", simgear::props::FLOAT}, - {102, "surface-positions/elevator-pos-norm", simgear::props::FLOAT}, - {103, "surface-positions/rudder-pos-norm", simgear::props::FLOAT}, - {104, "surface-positions/flap-pos-norm", simgear::props::FLOAT}, - {105, "surface-positions/speedbrake-pos-norm", simgear::props::FLOAT}, - {106, "gear/tailhook/position-norm", simgear::props::FLOAT}, - {107, "gear/launchbar/position-norm", simgear::props::FLOAT}, - {108, "gear/launchbar/state", simgear::props::STRING}, - {109, "gear/launchbar/holdback-position-norm", simgear::props::FLOAT}, - {110, "canopy/position-norm", simgear::props::FLOAT}, - {111, "surface-positions/wing-pos-norm", simgear::props::FLOAT}, - {112, "surface-positions/wing-fold-pos-norm", simgear::props::FLOAT}, - - {200, "gear/gear[0]/compression-norm", simgear::props::FLOAT}, - {201, "gear/gear[0]/position-norm", simgear::props::FLOAT}, - {210, "gear/gear[1]/compression-norm", simgear::props::FLOAT}, - {211, "gear/gear[1]/position-norm", simgear::props::FLOAT}, - {220, "gear/gear[2]/compression-norm", simgear::props::FLOAT}, - {221, "gear/gear[2]/position-norm", simgear::props::FLOAT}, - {230, "gear/gear[3]/compression-norm", simgear::props::FLOAT}, - {231, "gear/gear[3]/position-norm", simgear::props::FLOAT}, - {240, "gear/gear[4]/compression-norm", simgear::props::FLOAT}, - {241, "gear/gear[4]/position-norm", simgear::props::FLOAT}, - - {300, "engines/engine[0]/n1", simgear::props::FLOAT}, - {301, "engines/engine[0]/n2", simgear::props::FLOAT}, - {302, "engines/engine[0]/rpm", simgear::props::FLOAT}, - {310, "engines/engine[1]/n1", simgear::props::FLOAT}, - {311, "engines/engine[1]/n2", simgear::props::FLOAT}, - {312, "engines/engine[1]/rpm", simgear::props::FLOAT}, - {320, "engines/engine[2]/n1", simgear::props::FLOAT}, - {321, "engines/engine[2]/n2", simgear::props::FLOAT}, - {322, "engines/engine[2]/rpm", simgear::props::FLOAT}, - {330, "engines/engine[3]/n1", simgear::props::FLOAT}, - {331, "engines/engine[3]/n2", simgear::props::FLOAT}, - {332, "engines/engine[3]/rpm", simgear::props::FLOAT}, - {340, "engines/engine[4]/n1", simgear::props::FLOAT}, - {341, "engines/engine[4]/n2", simgear::props::FLOAT}, - {342, "engines/engine[4]/rpm", simgear::props::FLOAT}, - {350, "engines/engine[5]/n1", simgear::props::FLOAT}, - {351, "engines/engine[5]/n2", simgear::props::FLOAT}, - {352, "engines/engine[5]/rpm", simgear::props::FLOAT}, - {360, "engines/engine[6]/n1", simgear::props::FLOAT}, - {361, "engines/engine[6]/n2", simgear::props::FLOAT}, - {362, "engines/engine[6]/rpm", simgear::props::FLOAT}, - {370, "engines/engine[7]/n1", simgear::props::FLOAT}, - {371, "engines/engine[7]/n2", simgear::props::FLOAT}, - {372, "engines/engine[7]/rpm", simgear::props::FLOAT}, - {380, "engines/engine[8]/n1", simgear::props::FLOAT}, - {381, "engines/engine[8]/n2", simgear::props::FLOAT}, - {382, "engines/engine[8]/rpm", simgear::props::FLOAT}, - {390, "engines/engine[9]/n1", simgear::props::FLOAT}, - {391, "engines/engine[9]/n2", simgear::props::FLOAT}, - {392, "engines/engine[9]/rpm", simgear::props::FLOAT}, - - {800, "rotors/main/rpm", simgear::props::FLOAT}, - {801, "rotors/tail/rpm", simgear::props::FLOAT}, - {810, "rotors/main/blade[0]/position-deg", simgear::props::FLOAT}, - {811, "rotors/main/blade[1]/position-deg", simgear::props::FLOAT}, - {812, "rotors/main/blade[2]/position-deg", simgear::props::FLOAT}, - {813, "rotors/main/blade[3]/position-deg", simgear::props::FLOAT}, - {820, "rotors/main/blade[0]/flap-deg", simgear::props::FLOAT}, - {821, "rotors/main/blade[1]/flap-deg", simgear::props::FLOAT}, - {822, "rotors/main/blade[2]/flap-deg", simgear::props::FLOAT}, - {823, "rotors/main/blade[3]/flap-deg", simgear::props::FLOAT}, - {830, "rotors/tail/blade[0]/position-deg", simgear::props::FLOAT}, - {831, "rotors/tail/blade[1]/position-deg", simgear::props::FLOAT}, - - {900, "sim/hitches/aerotow/tow/length", simgear::props::FLOAT}, - {901, "sim/hitches/aerotow/tow/elastic-constant", simgear::props::FLOAT}, - {902, "sim/hitches/aerotow/tow/weight-per-m-kg-m", simgear::props::FLOAT}, - {903, "sim/hitches/aerotow/tow/dist", simgear::props::FLOAT}, - {904, "sim/hitches/aerotow/tow/connected-to-property-node", simgear::props::BOOL}, - {905, "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", simgear::props::STRING}, - {906, "sim/hitches/aerotow/tow/brake-force", simgear::props::FLOAT}, - {907, "sim/hitches/aerotow/tow/end-force-x", simgear::props::FLOAT}, - {908, "sim/hitches/aerotow/tow/end-force-y", simgear::props::FLOAT}, - {909, "sim/hitches/aerotow/tow/end-force-z", simgear::props::FLOAT}, - {930, "sim/hitches/aerotow/is-slave", simgear::props::BOOL}, - {931, "sim/hitches/aerotow/speed-in-tow-direction", simgear::props::FLOAT}, - {932, "sim/hitches/aerotow/open", simgear::props::BOOL}, - {933, "sim/hitches/aerotow/local-pos-x", simgear::props::FLOAT}, - {934, "sim/hitches/aerotow/local-pos-y", simgear::props::FLOAT}, - {935, "sim/hitches/aerotow/local-pos-z", simgear::props::FLOAT}, - - {1001, "controls/flight/slats", simgear::props::FLOAT}, - {1002, "controls/flight/speedbrake", simgear::props::FLOAT}, - {1003, "controls/flight/spoilers", simgear::props::FLOAT}, - {1004, "controls/gear/gear-down", simgear::props::FLOAT}, - {1005, "controls/lighting/nav-lights", simgear::props::FLOAT}, - {1006, "controls/armament/station[0]/jettison-all", simgear::props::BOOL}, - - {1100, "sim/model/variant", simgear::props::INT}, - {1101, "sim/model/livery/file", simgear::props::STRING}, - - {1200, "environment/wildfire/data", simgear::props::STRING}, - - {1300, "tanker", simgear::props::INT}, - - {10001, "sim/multiplay/transmission-freq-hz", simgear::props::STRING}, - {10002, "sim/multiplay/chat", simgear::props::STRING}, - - {10100, "sim/multiplay/generic/string[0]", simgear::props::STRING}, - {10101, "sim/multiplay/generic/string[1]", simgear::props::STRING}, - {10102, "sim/multiplay/generic/string[2]", simgear::props::STRING}, - {10103, "sim/multiplay/generic/string[3]", simgear::props::STRING}, - {10104, "sim/multiplay/generic/string[4]", simgear::props::STRING}, - {10105, "sim/multiplay/generic/string[5]", simgear::props::STRING}, - {10106, "sim/multiplay/generic/string[6]", simgear::props::STRING}, - {10107, "sim/multiplay/generic/string[7]", simgear::props::STRING}, - {10108, "sim/multiplay/generic/string[8]", simgear::props::STRING}, - {10109, "sim/multiplay/generic/string[9]", simgear::props::STRING}, - {10110, "sim/multiplay/generic/string[10]", simgear::props::STRING}, - {10111, "sim/multiplay/generic/string[11]", simgear::props::STRING}, - {10112, "sim/multiplay/generic/string[12]", simgear::props::STRING}, - {10113, "sim/multiplay/generic/string[13]", simgear::props::STRING}, - {10114, "sim/multiplay/generic/string[14]", simgear::props::STRING}, - {10115, "sim/multiplay/generic/string[15]", simgear::props::STRING}, - {10116, "sim/multiplay/generic/string[16]", simgear::props::STRING}, - {10117, "sim/multiplay/generic/string[17]", simgear::props::STRING}, - {10118, "sim/multiplay/generic/string[18]", simgear::props::STRING}, - {10119, "sim/multiplay/generic/string[19]", simgear::props::STRING}, - - {10200, "sim/multiplay/generic/float[0]", simgear::props::FLOAT}, - {10201, "sim/multiplay/generic/float[1]", simgear::props::FLOAT}, - {10202, "sim/multiplay/generic/float[2]", simgear::props::FLOAT}, - {10203, "sim/multiplay/generic/float[3]", simgear::props::FLOAT}, - {10204, "sim/multiplay/generic/float[4]", simgear::props::FLOAT}, - {10205, "sim/multiplay/generic/float[5]", simgear::props::FLOAT}, - {10206, "sim/multiplay/generic/float[6]", simgear::props::FLOAT}, - {10207, "sim/multiplay/generic/float[7]", simgear::props::FLOAT}, - {10208, "sim/multiplay/generic/float[8]", simgear::props::FLOAT}, - {10209, "sim/multiplay/generic/float[9]", simgear::props::FLOAT}, - {10210, "sim/multiplay/generic/float[10]", simgear::props::FLOAT}, - {10211, "sim/multiplay/generic/float[11]", simgear::props::FLOAT}, - {10212, "sim/multiplay/generic/float[12]", simgear::props::FLOAT}, - {10213, "sim/multiplay/generic/float[13]", simgear::props::FLOAT}, - {10214, "sim/multiplay/generic/float[14]", simgear::props::FLOAT}, - {10215, "sim/multiplay/generic/float[15]", simgear::props::FLOAT}, - {10216, "sim/multiplay/generic/float[16]", simgear::props::FLOAT}, - {10217, "sim/multiplay/generic/float[17]", simgear::props::FLOAT}, - {10218, "sim/multiplay/generic/float[18]", simgear::props::FLOAT}, - {10219, "sim/multiplay/generic/float[19]", simgear::props::FLOAT}, - - {10300, "sim/multiplay/generic/int[0]", simgear::props::INT}, - {10301, "sim/multiplay/generic/int[1]", simgear::props::INT}, - {10302, "sim/multiplay/generic/int[2]", simgear::props::INT}, - {10303, "sim/multiplay/generic/int[3]", simgear::props::INT}, - {10304, "sim/multiplay/generic/int[4]", simgear::props::INT}, - {10305, "sim/multiplay/generic/int[5]", simgear::props::INT}, - {10306, "sim/multiplay/generic/int[6]", simgear::props::INT}, - {10307, "sim/multiplay/generic/int[7]", simgear::props::INT}, - {10308, "sim/multiplay/generic/int[8]", simgear::props::INT}, - {10309, "sim/multiplay/generic/int[9]", simgear::props::INT}, - {10310, "sim/multiplay/generic/int[10]", simgear::props::INT}, - {10311, "sim/multiplay/generic/int[11]", simgear::props::INT}, - {10312, "sim/multiplay/generic/int[12]", simgear::props::INT}, - {10313, "sim/multiplay/generic/int[13]", simgear::props::INT}, - {10314, "sim/multiplay/generic/int[14]", simgear::props::INT}, - {10315, "sim/multiplay/generic/int[15]", simgear::props::INT}, - {10316, "sim/multiplay/generic/int[16]", simgear::props::INT}, - {10317, "sim/multiplay/generic/int[17]", simgear::props::INT}, - {10318, "sim/multiplay/generic/int[18]", simgear::props::INT}, - {10319, "sim/multiplay/generic/int[19]", simgear::props::INT} -}; - -const unsigned -FGMultiplayMgr::numProperties = (sizeof(FGMultiplayMgr::sIdPropertyList) - / sizeof(FGMultiplayMgr::sIdPropertyList[0])); - -// Look up a property ID using binary search. -namespace -{ - struct ComparePropertyId - { - bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, - const FGMultiplayMgr::IdPropertyList& rhs) - { - return lhs.id < rhs.id; - } - bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, - unsigned id) - { - return lhs.id < id; - } - bool operator()(unsigned id, - const FGMultiplayMgr::IdPropertyList& rhs) - { - return id < rhs.id; - } - }; - -} -const FGMultiplayMgr::IdPropertyList* FGMultiplayMgr::findProperty(unsigned id) -{ - std::pair result - = std::equal_range(sIdPropertyList, sIdPropertyList + numProperties, id, - ComparePropertyId()); - if (result.first == result.second) { - return 0; - } else { - return result.first; - } -} - -namespace -{ - bool verifyProperties(const xdr_data_t* data, const xdr_data_t* end) - { - using namespace simgear; - const xdr_data_t* xdr = data; - while (xdr < end) { - unsigned id = XDR_decode_uint32(*xdr); - const FGMultiplayMgr::IdPropertyList* plist - = FGMultiplayMgr::findProperty(id); - - if (plist) { - xdr++; - // How we decode the remainder of the property depends on the type - switch (plist->type) { - case props::INT: - case props::BOOL: - case props::LONG: - xdr++; - break; - case props::FLOAT: - case props::DOUBLE: - { - float val = XDR_decode_float(*xdr); - if (osg::isNaN(val)) - return false; - xdr++; - break; - } - case props::STRING: - case props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - // XXX Yes, each byte is padded out to a word! Too late - // to change... - uint32_t length = XDR_decode_uint32(*xdr); - xdr++; - // Old versions truncated the string but left the length - // unadjusted. - if (length > MAX_TEXT_SIZE) - length = MAX_TEXT_SIZE; - xdr += length; - // Now handle the padding - while ((length % 4) != 0) - { - xdr++; - length++; - //cout << "0"; - } - } - break; - default: - // cerr << "Unknown Prop type " << id << " " << type << "\n"; - xdr++; - break; - } - } - else { - // give up; this is a malformed property list. - return false; - } - } - return true; - } -} -////////////////////////////////////////////////////////////////////// -// -// MultiplayMgr constructor -// -////////////////////////////////////////////////////////////////////// -FGMultiplayMgr::FGMultiplayMgr() -{ - mSocket = 0; - mInitialised = false; - mHaveServer = false; -} // FGMultiplayMgr::FGMultiplayMgr() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// MultiplayMgr destructor -// -////////////////////////////////////////////////////////////////////// -FGMultiplayMgr::~FGMultiplayMgr() -{ - Close(); -} // FGMultiplayMgr::~FGMultiplayMgr() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Initialise object -// -////////////////////////////////////////////////////////////////////// -bool -FGMultiplayMgr::init (void) -{ - ////////////////////////////////////////////////// - // Initialise object if not already done - ////////////////////////////////////////////////// - if (mInitialised) { - SG_LOG(SG_NETWORK, SG_WARN, "FGMultiplayMgr::init - already initialised"); - return false; - } - ////////////////////////////////////////////////// - // Set members from property values - ////////////////////////////////////////////////// - short rxPort = fgGetInt("/sim/multiplay/rxport"); - string rxAddress = fgGetString("/sim/multiplay/rxhost"); - short txPort = fgGetInt("/sim/multiplay/txport"); - string txAddress = fgGetString("/sim/multiplay/txhost"); - mCallsign = fgGetString("/sim/multiplay/callsign"); - if (txPort > 0 && !txAddress.empty()) { - mServer.set(txAddress.c_str(), txPort); - if (strncmp (mServer.getHost(), "0.0.0.0", 8) == 0) { - mHaveServer = false; - SG_LOG(SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr - could not resolve '" - << txAddress << "', Multiplayermode disabled"); - } else { - mHaveServer = true; - } - if (rxPort <= 0) - rxPort = txPort; - } - if (rxPort <= 0) { - SG_LOG(SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr - No receiver port, Multiplayermode disabled"); - return (false); - } - if (mCallsign.empty()) - mCallsign = "JohnDoe"; // FIXME: use getpwuid - SG_LOG(SG_NETWORK,SG_INFO,"FGMultiplayMgr::init-txaddress= "<setBlocking(false); - if (mSocket->bind(rxAddress.c_str(), rxPort) != 0) { - perror("bind"); - SG_LOG( SG_NETWORK, SG_DEBUG, - "FGMultiplayMgr::Open - Failed to bind receive socket" ); - return false; - } - mInitialised = true; - return true; -} // FGMultiplayMgr::init() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Closes and deletes the local player object. Closes -// and deletes the tx socket. Resets the object state to unitialised. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::Close (void) -{ - mMultiPlayerMap.clear(); - - if (mSocket) { - mSocket->close(); - delete mSocket; - mSocket = 0; - } - mInitialised = false; -} // FGMultiplayMgr::Close(void) -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Description: Sends the position data for the local position. -// -////////////////////////////////////////////////////////////////////// - -/** - * The buffer that holds a multi-player message, suitably aligned. - */ -union FGMultiplayMgr::MsgBuf -{ - MsgBuf() - { - memset(&Msg, 0, sizeof(Msg)); - } - - T_MsgHdr* msgHdr() - { - return reinterpret_cast(Msg); - } - - const T_MsgHdr* msgHdr() const - { - return reinterpret_cast(Msg); - } - - T_PositionMsg* posMsg() - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr)); - } - - const T_PositionMsg* posMsg() const - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr)); - } - - xdr_data_t* properties() - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr) - + sizeof(T_PositionMsg)); - } - - const xdr_data_t* properties() const - { - return reinterpret_cast(Msg + sizeof(T_MsgHdr) - + sizeof(T_PositionMsg)); - } - /** - * The end of the properties buffer. - */ - xdr_data_t* propsEnd() - { - return reinterpret_cast(Msg + MAX_PACKET_SIZE); - }; - - const xdr_data_t* propsEnd() const - { - return reinterpret_cast(Msg + MAX_PACKET_SIZE); - }; - /** - * The end of properties actually in the buffer. This assumes that - * the message header is valid. - */ - xdr_data_t* propsRecvdEnd() - { - return reinterpret_cast(Msg + msgHdr()->MsgLen); - } - - const xdr_data_t* propsRecvdEnd() const - { - return reinterpret_cast(Msg + msgHdr()->MsgLen); - } - - xdr_data2_t double_val; - char Msg[MAX_PACKET_SIZE]; -}; - -void -FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) -{ - if ((! mInitialised) || (! mHaveServer)) - return; - if (! mHaveServer) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); - return; - } - - MsgBuf msgBuf; - T_PositionMsg* PosMsg = msgBuf.posMsg(); - - strncpy(PosMsg->Model, fgGetString("/sim/model/path"), MAX_MODEL_NAME_LEN); - PosMsg->Model[MAX_MODEL_NAME_LEN - 1] = '\0'; - - PosMsg->time = XDR_encode_double (motionInfo.time); - PosMsg->lag = XDR_encode_double (motionInfo.lag); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->position[i] = XDR_encode_double (motionInfo.position(i)); - SGVec3f angleAxis; - motionInfo.orientation.getAngleAxis(angleAxis); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->orientation[i] = XDR_encode_float (angleAxis(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->linearVel[i] = XDR_encode_float (motionInfo.linearVel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->angularVel[i] = XDR_encode_float (motionInfo.angularVel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->linearAccel[i] = XDR_encode_float (motionInfo.linearAccel(i)); - for (unsigned i = 0 ; i < 3; ++i) - PosMsg->angularAccel[i] = XDR_encode_float (motionInfo.angularAccel(i)); - - xdr_data_t* ptr = msgBuf.properties(); - std::vector::const_iterator it; - it = motionInfo.properties.begin(); - //cout << "OUTPUT PROPERTIES\n"; - xdr_data_t* msgEnd = msgBuf.propsEnd(); - while (it != motionInfo.properties.end() && ptr + 2 < msgEnd) { - - // First element is the ID. Write it out when we know we have room for - // the whole property. - xdr_data_t id = XDR_encode_uint32((*it)->id); - // The actual data representation depends on the type - switch ((*it)->type) { - case simgear::props::INT: - case simgear::props::BOOL: - case simgear::props::LONG: - *ptr++ = id; - *ptr++ = XDR_encode_uint32((*it)->int_value); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->int_value << "\n"; - break; - case simgear::props::FLOAT: - case simgear::props::DOUBLE: - *ptr++ = id; - *ptr++ = XDR_encode_float((*it)->float_value); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; - break; - case simgear::props::STRING: - case simgear::props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - const char* lcharptr = (*it)->string_value; - - if (lcharptr != 0) - { - // Add the length - ////cout << "String length: " << strlen(lcharptr) << "\n"; - uint32_t len = strlen(lcharptr); - if (len > MAX_TEXT_SIZE) - len = MAX_TEXT_SIZE; - // XXX This should not be using 4 bytes per character! - // If there's not enough room for this property, drop it - // on the floor. - if (ptr + 2 + ((len + 3) & ~3) > msgEnd) - goto escape; - //cout << "String length unint32: " << len << "\n"; - *ptr++ = id; - *ptr++ = XDR_encode_uint32(len); - if (len != 0) - { - // Now the text itself - // XXX This should not be using 4 bytes per character! - int lcount = 0; - while ((*lcharptr != '\0') && (lcount < MAX_TEXT_SIZE)) - { - *ptr++ = XDR_encode_int8(*lcharptr); - lcharptr++; - lcount++; - } - - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " " << len << " " << (*it)->string_value; - - // Now pad if required - while ((lcount % 4) != 0) - { - *ptr++ = XDR_encode_int8(0); - lcount++; - //cout << "0"; - } - - //cout << "\n"; - } - } - else - { - // Nothing to encode - *ptr++ = id; - *ptr++ = XDR_encode_uint32(0); - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " 0\n"; - } - } - break; - - default: - //cout << " Unknown Type: " << (*it)->type << "\n"; - *ptr++ = id; - *ptr++ = XDR_encode_float((*it)->float_value);; - //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; - break; - } - - ++it; - } -escape: - unsigned msgLen = reinterpret_cast(ptr) - msgBuf.Msg; - FillMsgHdr(msgBuf.msgHdr(), POS_DATA_ID, msgLen); - mSocket->sendto(msgBuf.Msg, msgLen, 0, &mServer); - SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition"); -} // FGMultiplayMgr::SendMyPosition() - -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Name: SendTextMessage -// Description: Sends a message to the player. The message must -// contain a valid and correctly filled out header and optional -// message body. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::SendTextMessage(const string &MsgText) -{ - if (!mInitialised || !mHaveServer) - return; - - T_MsgHdr MsgHdr; - FillMsgHdr(&MsgHdr, CHAT_MSG_ID); - ////////////////////////////////////////////////// - // Divide the text string into blocks that fit - // in the message and send the blocks. - ////////////////////////////////////////////////// - unsigned iNextBlockPosition = 0; - T_ChatMsg ChatMsg; - - char Msg[sizeof(T_MsgHdr) + sizeof(T_ChatMsg)]; - while (iNextBlockPosition < MsgText.length()) { - strncpy (ChatMsg.Text, - MsgText.substr(iNextBlockPosition, MAX_CHAT_MSG_LEN - 1).c_str(), - MAX_CHAT_MSG_LEN); - ChatMsg.Text[MAX_CHAT_MSG_LEN - 1] = '\0'; - memcpy (Msg, &MsgHdr, sizeof(T_MsgHdr)); - memcpy (Msg + sizeof(T_MsgHdr), &ChatMsg, sizeof(T_ChatMsg)); - mSocket->sendto (Msg, sizeof(T_MsgHdr) + sizeof(T_ChatMsg), 0, &mServer); - iNextBlockPosition += MAX_CHAT_MSG_LEN - 1; - - } - - -} // FGMultiplayMgr::SendTextMessage () -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// Name: ProcessData -// Description: Processes data waiting at the receive socket. The -// processing ends when there is no more data at the socket. -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::Update(void) -{ - if (!mInitialised) - return; - - /// Just for expiry - long stamp = SGTimeStamp::now().getSeconds(); - - ////////////////////////////////////////////////// - // Read the receive socket and process any data - ////////////////////////////////////////////////// - ssize_t bytes; - do { - MsgBuf msgBuf; - ////////////////////////////////////////////////// - // Although the recv call asks for - // MAX_PACKET_SIZE of data, the number of bytes - // returned will only be that of the next - // packet waiting to be processed. - ////////////////////////////////////////////////// - netAddress SenderAddress; - bytes = mSocket->recvfrom(msgBuf.Msg, sizeof(msgBuf.Msg), 0, - &SenderAddress); - ////////////////////////////////////////////////// - // no Data received - ////////////////////////////////////////////////// - if (bytes <= 0) { - if (errno != EAGAIN && errno != 0) // MSVC output "NoError" otherwise - perror("FGMultiplayMgr::MP_ProcessData"); - break; - } - if (bytes <= static_cast(sizeof(T_MsgHdr))) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "received message with insufficient data" ); - break; - } - ////////////////////////////////////////////////// - // Read header - ////////////////////////////////////////////////// - T_MsgHdr* MsgHdr = msgBuf.msgHdr(); - MsgHdr->Magic = XDR_decode_uint32 (MsgHdr->Magic); - MsgHdr->Version = XDR_decode_uint32 (MsgHdr->Version); - MsgHdr->MsgId = XDR_decode_uint32 (MsgHdr->MsgId); - MsgHdr->MsgLen = XDR_decode_uint32 (MsgHdr->MsgLen); - MsgHdr->ReplyPort = XDR_decode_uint32 (MsgHdr->ReplyPort); - MsgHdr->Callsign[MAX_CALLSIGN_LEN -1] = '\0'; - if (MsgHdr->Magic != MSG_MAGIC) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message has invalid magic number!" ); - break; - } - if (MsgHdr->Version != PROTO_VER) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message has invalid protocoll number!" ); - break; - } - if (MsgHdr->MsgLen != bytes) { - SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message from " << MsgHdr->Callsign << " has invalid length!"); - break; - } - ////////////////////////////////////////////////// - // Process messages - ////////////////////////////////////////////////// - switch (MsgHdr->MsgId) { - case CHAT_MSG_ID: - ProcessChatMsg(msgBuf, SenderAddress); - break; - case POS_DATA_ID: - ProcessPosMsg(msgBuf, SenderAddress, stamp); - break; - case UNUSABLE_POS_DATA_ID: - case OLD_OLD_POS_DATA_ID: - case OLD_PROP_MSG_ID: - case OLD_POS_DATA_ID: - break; - default: - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Unknown message Id received: " << MsgHdr->MsgId ); - break; - } - } while (bytes > 0); - - // check for expiry - MultiPlayerMap::iterator it = mMultiPlayerMap.begin(); - while (it != mMultiPlayerMap.end()) { - if (it->second->getLastTimestamp() + 10 < stamp) { - std::string name = it->first; - it->second->setDie(true); - mMultiPlayerMap.erase(it); - it = mMultiPlayerMap.upper_bound(name); - } else - ++it; - } -} // FGMultiplayMgr::ProcessData(void) -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// handle a position message -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, - const netAddress& SenderAddress, long stamp) -{ - const T_MsgHdr* MsgHdr = Msg.msgHdr(); - if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + sizeof(T_PositionMsg)) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Position message received with insufficient data" ); - return; - } - const T_PositionMsg* PosMsg = Msg.posMsg(); - FGExternalMotionData motionInfo; - motionInfo.time = XDR_decode_double(PosMsg->time); - motionInfo.lag = XDR_decode_double(PosMsg->lag); - for (unsigned i = 0; i < 3; ++i) - motionInfo.position(i) = XDR_decode_double(PosMsg->position[i]); - SGVec3f angleAxis; - for (unsigned i = 0; i < 3; ++i) - angleAxis(i) = XDR_decode_float(PosMsg->orientation[i]); - motionInfo.orientation = SGQuatf::fromAngleAxis(angleAxis); - for (unsigned i = 0; i < 3; ++i) - motionInfo.linearVel(i) = XDR_decode_float(PosMsg->linearVel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.angularVel(i) = XDR_decode_float(PosMsg->angularVel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.linearAccel(i) = XDR_decode_float(PosMsg->linearAccel[i]); - for (unsigned i = 0; i < 3; ++i) - motionInfo.angularAccel(i) = XDR_decode_float(PosMsg->angularAccel[i]); - - - //cout << "INPUT MESSAGE\n"; - - // There was a bug in 1.9.0 and before: T_PositionMsg was 196 bytes - // on 32 bit architectures and 200 bytes on 64 bit, and this - // structure is put directly on the wire. By looking at the padding, - // we can sort through the mess, mostly: - // If padding is 0 (which is not a valid property type), then the - // message was produced by a new client or an old 64 bit client that - // happened to have 0 on the stack; - // Else if the property list starting with the padding word is - // well-formed, then the client is probably an old 32 bit client and - // we'll go with that; - // Else it is an old 64-bit client and properties start after the - // padding. - // There is a chance that we could be fooled by garbage in the - // padding looking like a valid property, so verifyProperties() is - // strict about the validity of the property values. - const xdr_data_t* xdr = Msg.properties(); - if (PosMsg->pad != 0) { - if (verifyProperties(&PosMsg->pad, Msg.propsRecvdEnd())) - xdr = &PosMsg->pad; - else if (!verifyProperties(xdr, Msg.propsRecvdEnd())) - goto noprops; - } - while (xdr < Msg.propsRecvdEnd()) { - FGPropertyData* pData = new FGPropertyData; - // simgear::props::Type type = simgear::props::UNSPECIFIED; - - // First element is always the ID - pData->id = XDR_decode_uint32(*xdr); - //cout << pData->id << " "; - xdr++; - - // Check the ID actually exists and get the type - const IdPropertyList* plist = findProperty(pData->id); - - if (plist) - { - pData->type = plist->type; - // How we decode the remainder of the property depends on the type - switch (pData->type) { - case simgear::props::INT: - case simgear::props::BOOL: - case simgear::props::LONG: - pData->int_value = XDR_decode_uint32(*xdr); - xdr++; - //cout << pData->int_value << "\n"; - break; - case simgear::props::FLOAT: - case simgear::props::DOUBLE: - pData->float_value = XDR_decode_float(*xdr); - xdr++; - //cout << pData->float_value << "\n"; - break; - case simgear::props::STRING: - case simgear::props::UNSPECIFIED: - { - // String is complicated. It consists of - // The length of the string - // The string itself - // Padding to the nearest 4-bytes. - uint32_t length = XDR_decode_uint32(*xdr); - xdr++; - //cout << length << " "; - // Old versions truncated the string but left the length unadjusted. - if (length > MAX_TEXT_SIZE) - length = MAX_TEXT_SIZE; - pData->string_value = new char[length + 1]; - //cout << " String: "; - for (unsigned i = 0; i < length; i++) - { - pData->string_value[i] = (char) XDR_decode_int8(*xdr); - xdr++; - //cout << pData->string_value[i]; - } - - pData->string_value[length] = '\0'; - - // Now handle the padding - while ((length % 4) != 0) - { - xdr++; - length++; - //cout << "0"; - } - //cout << "\n"; - } - break; - - default: - pData->float_value = XDR_decode_float(*xdr); - SG_LOG(SG_NETWORK, SG_DEBUG, "Unknown Prop type " << pData->id << " " << pData->type); - xdr++; - break; - } - - motionInfo.properties.push_back(pData); - } - else - { - // We failed to find the property. We'll try the next packet immediately. - SG_LOG(SG_NETWORK, SG_INFO, "FGMultiplayMgr::ProcessPosMsg - " - "message from " << MsgHdr->Callsign << " has unknown property id " - << pData->id); - } - } - noprops: - FGAIMultiplayer* mp = getMultiplayer(MsgHdr->Callsign); - if (!mp) - mp = addMultiplayer(MsgHdr->Callsign, PosMsg->Model); - mp->addMotionInfo(motionInfo, stamp); -} // FGMultiplayMgr::ProcessPosMsg() -////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////// -// -// handle a chat message -// FIXME: display chat message withi flightgear -// -////////////////////////////////////////////////////////////////////// -void -FGMultiplayMgr::ProcessChatMsg(const MsgBuf& Msg, - const netAddress& SenderAddress) -{ - const T_MsgHdr* MsgHdr = Msg.msgHdr(); - if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + 1) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "Chat message received with insufficient data" ); - return; - } - - char *chatStr = new char[MsgHdr->MsgLen - sizeof(T_MsgHdr)]; - const T_ChatMsg* ChatMsg - = reinterpret_cast(Msg.Msg + sizeof(T_MsgHdr)); - strncpy(chatStr, ChatMsg->Text, - MsgHdr->MsgLen - sizeof(T_MsgHdr)); - chatStr[MsgHdr->MsgLen - sizeof(T_MsgHdr) - 1] = '\0'; - - SG_LOG (SG_NETWORK, SG_WARN, "Chat [" << MsgHdr->Callsign << "]" - << " " << chatStr); - - delete [] chatStr; -} // FGMultiplayMgr::ProcessChatMsg () -////////////////////////////////////////////////////////////////////// - -void -FGMultiplayMgr::FillMsgHdr(T_MsgHdr *MsgHdr, int MsgId, unsigned _len) -{ - uint32_t len; - switch (MsgId) { - case CHAT_MSG_ID: - len = sizeof(T_MsgHdr) + sizeof(T_ChatMsg); - break; - case POS_DATA_ID: - len = _len; - break; - default: - len = sizeof(T_MsgHdr); - break; - } - MsgHdr->Magic = XDR_encode_uint32(MSG_MAGIC); - MsgHdr->Version = XDR_encode_uint32(PROTO_VER); - MsgHdr->MsgId = XDR_encode_uint32(MsgId); - MsgHdr->MsgLen = XDR_encode_uint32(len); - MsgHdr->ReplyAddress = 0; // Are obsolete, keep them for the server for - MsgHdr->ReplyPort = 0; // now - strncpy(MsgHdr->Callsign, mCallsign.c_str(), MAX_CALLSIGN_LEN); - MsgHdr->Callsign[MAX_CALLSIGN_LEN - 1] = '\0'; -} - -FGAIMultiplayer* -FGMultiplayMgr::addMultiplayer(const std::string& callsign, - const std::string& modelName) -{ - if (0 < mMultiPlayerMap.count(callsign)) - return mMultiPlayerMap[callsign].get(); - - FGAIMultiplayer* mp = new FGAIMultiplayer; - mp->setPath(modelName.c_str()); - mp->setCallSign(callsign); - mMultiPlayerMap[callsign] = mp; - - FGAIManager *aiMgr = (FGAIManager*)globals->get_subsystem("ai_model"); - if (aiMgr) { - aiMgr->attach(mp); - - /// FIXME: that must follow the attach ATM ... - for (unsigned i = 0; i < numProperties; ++i) - mp->addPropertyId(sIdPropertyList[i].id, sIdPropertyList[i].name); - } - - return mp; -} - -FGAIMultiplayer* -FGMultiplayMgr::getMultiplayer(const std::string& callsign) -{ - if (0 < mMultiPlayerMap.count(callsign)) - return mMultiPlayerMap[callsign].get(); - else - return 0; -} +////////////////////////////////////////////////////////////////////// +// +// multiplaymgr.hpp +// +// Written by Duncan McCreanor, started February 2003. +// duncan.mccreanor@airservicesaustralia.com +// +// Copyright (C) 2003 Airservices Australia +// Copyright (C) 2005 Oliver Schroeder +// Copyright (C) 2006 Mathias Froehlich +// +// 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. +// +// $Id$ +// +////////////////////////////////////////////////////////////////////// + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include // isNaN +#include + +#include +#include +#include +#include + +#include +#include
+#include "multiplaymgr.hxx" +#include "mpmessages.hxx" + +using namespace std; + +#define MAX_PACKET_SIZE 1200 +#define MAX_TEXT_SIZE 128 + +// These constants are provided so that the ident +// command can list file versions +const char sMULTIPLAYMGR_BID[] = "$Id$"; +const char sMULTIPLAYMGR_HID[] = MULTIPLAYTXMGR_HID; + +// A static map of protocol property id values to property paths, +// This should be extendable dynamically for every specific aircraft ... +// For now only that static list +const FGMultiplayMgr::IdPropertyList +FGMultiplayMgr::sIdPropertyList[] = { + {100, "surface-positions/left-aileron-pos-norm", simgear::props::FLOAT}, + {101, "surface-positions/right-aileron-pos-norm", simgear::props::FLOAT}, + {102, "surface-positions/elevator-pos-norm", simgear::props::FLOAT}, + {103, "surface-positions/rudder-pos-norm", simgear::props::FLOAT}, + {104, "surface-positions/flap-pos-norm", simgear::props::FLOAT}, + {105, "surface-positions/speedbrake-pos-norm", simgear::props::FLOAT}, + {106, "gear/tailhook/position-norm", simgear::props::FLOAT}, + {107, "gear/launchbar/position-norm", simgear::props::FLOAT}, + {108, "gear/launchbar/state", simgear::props::STRING}, + {109, "gear/launchbar/holdback-position-norm", simgear::props::FLOAT}, + {110, "canopy/position-norm", simgear::props::FLOAT}, + {111, "surface-positions/wing-pos-norm", simgear::props::FLOAT}, + {112, "surface-positions/wing-fold-pos-norm", simgear::props::FLOAT}, + + {200, "gear/gear[0]/compression-norm", simgear::props::FLOAT}, + {201, "gear/gear[0]/position-norm", simgear::props::FLOAT}, + {210, "gear/gear[1]/compression-norm", simgear::props::FLOAT}, + {211, "gear/gear[1]/position-norm", simgear::props::FLOAT}, + {220, "gear/gear[2]/compression-norm", simgear::props::FLOAT}, + {221, "gear/gear[2]/position-norm", simgear::props::FLOAT}, + {230, "gear/gear[3]/compression-norm", simgear::props::FLOAT}, + {231, "gear/gear[3]/position-norm", simgear::props::FLOAT}, + {240, "gear/gear[4]/compression-norm", simgear::props::FLOAT}, + {241, "gear/gear[4]/position-norm", simgear::props::FLOAT}, + + {300, "engines/engine[0]/n1", simgear::props::FLOAT}, + {301, "engines/engine[0]/n2", simgear::props::FLOAT}, + {302, "engines/engine[0]/rpm", simgear::props::FLOAT}, + {310, "engines/engine[1]/n1", simgear::props::FLOAT}, + {311, "engines/engine[1]/n2", simgear::props::FLOAT}, + {312, "engines/engine[1]/rpm", simgear::props::FLOAT}, + {320, "engines/engine[2]/n1", simgear::props::FLOAT}, + {321, "engines/engine[2]/n2", simgear::props::FLOAT}, + {322, "engines/engine[2]/rpm", simgear::props::FLOAT}, + {330, "engines/engine[3]/n1", simgear::props::FLOAT}, + {331, "engines/engine[3]/n2", simgear::props::FLOAT}, + {332, "engines/engine[3]/rpm", simgear::props::FLOAT}, + {340, "engines/engine[4]/n1", simgear::props::FLOAT}, + {341, "engines/engine[4]/n2", simgear::props::FLOAT}, + {342, "engines/engine[4]/rpm", simgear::props::FLOAT}, + {350, "engines/engine[5]/n1", simgear::props::FLOAT}, + {351, "engines/engine[5]/n2", simgear::props::FLOAT}, + {352, "engines/engine[5]/rpm", simgear::props::FLOAT}, + {360, "engines/engine[6]/n1", simgear::props::FLOAT}, + {361, "engines/engine[6]/n2", simgear::props::FLOAT}, + {362, "engines/engine[6]/rpm", simgear::props::FLOAT}, + {370, "engines/engine[7]/n1", simgear::props::FLOAT}, + {371, "engines/engine[7]/n2", simgear::props::FLOAT}, + {372, "engines/engine[7]/rpm", simgear::props::FLOAT}, + {380, "engines/engine[8]/n1", simgear::props::FLOAT}, + {381, "engines/engine[8]/n2", simgear::props::FLOAT}, + {382, "engines/engine[8]/rpm", simgear::props::FLOAT}, + {390, "engines/engine[9]/n1", simgear::props::FLOAT}, + {391, "engines/engine[9]/n2", simgear::props::FLOAT}, + {392, "engines/engine[9]/rpm", simgear::props::FLOAT}, + + {800, "rotors/main/rpm", simgear::props::FLOAT}, + {801, "rotors/tail/rpm", simgear::props::FLOAT}, + {810, "rotors/main/blade[0]/position-deg", simgear::props::FLOAT}, + {811, "rotors/main/blade[1]/position-deg", simgear::props::FLOAT}, + {812, "rotors/main/blade[2]/position-deg", simgear::props::FLOAT}, + {813, "rotors/main/blade[3]/position-deg", simgear::props::FLOAT}, + {820, "rotors/main/blade[0]/flap-deg", simgear::props::FLOAT}, + {821, "rotors/main/blade[1]/flap-deg", simgear::props::FLOAT}, + {822, "rotors/main/blade[2]/flap-deg", simgear::props::FLOAT}, + {823, "rotors/main/blade[3]/flap-deg", simgear::props::FLOAT}, + {830, "rotors/tail/blade[0]/position-deg", simgear::props::FLOAT}, + {831, "rotors/tail/blade[1]/position-deg", simgear::props::FLOAT}, + + {900, "sim/hitches/aerotow/tow/length", simgear::props::FLOAT}, + {901, "sim/hitches/aerotow/tow/elastic-constant", simgear::props::FLOAT}, + {902, "sim/hitches/aerotow/tow/weight-per-m-kg-m", simgear::props::FLOAT}, + {903, "sim/hitches/aerotow/tow/dist", simgear::props::FLOAT}, + {904, "sim/hitches/aerotow/tow/connected-to-property-node", simgear::props::BOOL}, + {905, "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", simgear::props::STRING}, + {906, "sim/hitches/aerotow/tow/brake-force", simgear::props::FLOAT}, + {907, "sim/hitches/aerotow/tow/end-force-x", simgear::props::FLOAT}, + {908, "sim/hitches/aerotow/tow/end-force-y", simgear::props::FLOAT}, + {909, "sim/hitches/aerotow/tow/end-force-z", simgear::props::FLOAT}, + {930, "sim/hitches/aerotow/is-slave", simgear::props::BOOL}, + {931, "sim/hitches/aerotow/speed-in-tow-direction", simgear::props::FLOAT}, + {932, "sim/hitches/aerotow/open", simgear::props::BOOL}, + {933, "sim/hitches/aerotow/local-pos-x", simgear::props::FLOAT}, + {934, "sim/hitches/aerotow/local-pos-y", simgear::props::FLOAT}, + {935, "sim/hitches/aerotow/local-pos-z", simgear::props::FLOAT}, + + {1001, "controls/flight/slats", simgear::props::FLOAT}, + {1002, "controls/flight/speedbrake", simgear::props::FLOAT}, + {1003, "controls/flight/spoilers", simgear::props::FLOAT}, + {1004, "controls/gear/gear-down", simgear::props::FLOAT}, + {1005, "controls/lighting/nav-lights", simgear::props::FLOAT}, + {1006, "controls/armament/station[0]/jettison-all", simgear::props::BOOL}, + + {1100, "sim/model/variant", simgear::props::INT}, + {1101, "sim/model/livery/file", simgear::props::STRING}, + + {1200, "environment/wildfire/data", simgear::props::STRING}, + + {1300, "tanker", simgear::props::INT}, + + {10001, "sim/multiplay/transmission-freq-hz", simgear::props::STRING}, + {10002, "sim/multiplay/chat", simgear::props::STRING}, + + {10100, "sim/multiplay/generic/string[0]", simgear::props::STRING}, + {10101, "sim/multiplay/generic/string[1]", simgear::props::STRING}, + {10102, "sim/multiplay/generic/string[2]", simgear::props::STRING}, + {10103, "sim/multiplay/generic/string[3]", simgear::props::STRING}, + {10104, "sim/multiplay/generic/string[4]", simgear::props::STRING}, + {10105, "sim/multiplay/generic/string[5]", simgear::props::STRING}, + {10106, "sim/multiplay/generic/string[6]", simgear::props::STRING}, + {10107, "sim/multiplay/generic/string[7]", simgear::props::STRING}, + {10108, "sim/multiplay/generic/string[8]", simgear::props::STRING}, + {10109, "sim/multiplay/generic/string[9]", simgear::props::STRING}, + {10110, "sim/multiplay/generic/string[10]", simgear::props::STRING}, + {10111, "sim/multiplay/generic/string[11]", simgear::props::STRING}, + {10112, "sim/multiplay/generic/string[12]", simgear::props::STRING}, + {10113, "sim/multiplay/generic/string[13]", simgear::props::STRING}, + {10114, "sim/multiplay/generic/string[14]", simgear::props::STRING}, + {10115, "sim/multiplay/generic/string[15]", simgear::props::STRING}, + {10116, "sim/multiplay/generic/string[16]", simgear::props::STRING}, + {10117, "sim/multiplay/generic/string[17]", simgear::props::STRING}, + {10118, "sim/multiplay/generic/string[18]", simgear::props::STRING}, + {10119, "sim/multiplay/generic/string[19]", simgear::props::STRING}, + + {10200, "sim/multiplay/generic/float[0]", simgear::props::FLOAT}, + {10201, "sim/multiplay/generic/float[1]", simgear::props::FLOAT}, + {10202, "sim/multiplay/generic/float[2]", simgear::props::FLOAT}, + {10203, "sim/multiplay/generic/float[3]", simgear::props::FLOAT}, + {10204, "sim/multiplay/generic/float[4]", simgear::props::FLOAT}, + {10205, "sim/multiplay/generic/float[5]", simgear::props::FLOAT}, + {10206, "sim/multiplay/generic/float[6]", simgear::props::FLOAT}, + {10207, "sim/multiplay/generic/float[7]", simgear::props::FLOAT}, + {10208, "sim/multiplay/generic/float[8]", simgear::props::FLOAT}, + {10209, "sim/multiplay/generic/float[9]", simgear::props::FLOAT}, + {10210, "sim/multiplay/generic/float[10]", simgear::props::FLOAT}, + {10211, "sim/multiplay/generic/float[11]", simgear::props::FLOAT}, + {10212, "sim/multiplay/generic/float[12]", simgear::props::FLOAT}, + {10213, "sim/multiplay/generic/float[13]", simgear::props::FLOAT}, + {10214, "sim/multiplay/generic/float[14]", simgear::props::FLOAT}, + {10215, "sim/multiplay/generic/float[15]", simgear::props::FLOAT}, + {10216, "sim/multiplay/generic/float[16]", simgear::props::FLOAT}, + {10217, "sim/multiplay/generic/float[17]", simgear::props::FLOAT}, + {10218, "sim/multiplay/generic/float[18]", simgear::props::FLOAT}, + {10219, "sim/multiplay/generic/float[19]", simgear::props::FLOAT}, + + {10300, "sim/multiplay/generic/int[0]", simgear::props::INT}, + {10301, "sim/multiplay/generic/int[1]", simgear::props::INT}, + {10302, "sim/multiplay/generic/int[2]", simgear::props::INT}, + {10303, "sim/multiplay/generic/int[3]", simgear::props::INT}, + {10304, "sim/multiplay/generic/int[4]", simgear::props::INT}, + {10305, "sim/multiplay/generic/int[5]", simgear::props::INT}, + {10306, "sim/multiplay/generic/int[6]", simgear::props::INT}, + {10307, "sim/multiplay/generic/int[7]", simgear::props::INT}, + {10308, "sim/multiplay/generic/int[8]", simgear::props::INT}, + {10309, "sim/multiplay/generic/int[9]", simgear::props::INT}, + {10310, "sim/multiplay/generic/int[10]", simgear::props::INT}, + {10311, "sim/multiplay/generic/int[11]", simgear::props::INT}, + {10312, "sim/multiplay/generic/int[12]", simgear::props::INT}, + {10313, "sim/multiplay/generic/int[13]", simgear::props::INT}, + {10314, "sim/multiplay/generic/int[14]", simgear::props::INT}, + {10315, "sim/multiplay/generic/int[15]", simgear::props::INT}, + {10316, "sim/multiplay/generic/int[16]", simgear::props::INT}, + {10317, "sim/multiplay/generic/int[17]", simgear::props::INT}, + {10318, "sim/multiplay/generic/int[18]", simgear::props::INT}, + {10319, "sim/multiplay/generic/int[19]", simgear::props::INT} +}; + +const unsigned +FGMultiplayMgr::numProperties = (sizeof(FGMultiplayMgr::sIdPropertyList) + / sizeof(FGMultiplayMgr::sIdPropertyList[0])); + +// Look up a property ID using binary search. +namespace +{ + struct ComparePropertyId + { + bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, + const FGMultiplayMgr::IdPropertyList& rhs) + { + return lhs.id < rhs.id; + } + bool operator()(const FGMultiplayMgr::IdPropertyList& lhs, + unsigned id) + { + return lhs.id < id; + } + bool operator()(unsigned id, + const FGMultiplayMgr::IdPropertyList& rhs) + { + return id < rhs.id; + } + }; + +} +const FGMultiplayMgr::IdPropertyList* FGMultiplayMgr::findProperty(unsigned id) +{ + std::pair result + = std::equal_range(sIdPropertyList, sIdPropertyList + numProperties, id, + ComparePropertyId()); + if (result.first == result.second) { + return 0; + } else { + return result.first; + } +} + +namespace +{ + bool verifyProperties(const xdr_data_t* data, const xdr_data_t* end) + { + using namespace simgear; + const xdr_data_t* xdr = data; + while (xdr < end) { + unsigned id = XDR_decode_uint32(*xdr); + const FGMultiplayMgr::IdPropertyList* plist + = FGMultiplayMgr::findProperty(id); + + if (plist) { + xdr++; + // How we decode the remainder of the property depends on the type + switch (plist->type) { + case props::INT: + case props::BOOL: + case props::LONG: + xdr++; + break; + case props::FLOAT: + case props::DOUBLE: + { + float val = XDR_decode_float(*xdr); + if (osg::isNaN(val)) + return false; + xdr++; + break; + } + case props::STRING: + case props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + // XXX Yes, each byte is padded out to a word! Too late + // to change... + uint32_t length = XDR_decode_uint32(*xdr); + xdr++; + // Old versions truncated the string but left the length + // unadjusted. + if (length > MAX_TEXT_SIZE) + length = MAX_TEXT_SIZE; + xdr += length; + // Now handle the padding + while ((length % 4) != 0) + { + xdr++; + length++; + //cout << "0"; + } + } + break; + default: + // cerr << "Unknown Prop type " << id << " " << type << "\n"; + xdr++; + break; + } + } + else { + // give up; this is a malformed property list. + return false; + } + } + return true; + } +} +////////////////////////////////////////////////////////////////////// +// +// MultiplayMgr constructor +// +////////////////////////////////////////////////////////////////////// +FGMultiplayMgr::FGMultiplayMgr() +{ + mSocket = 0; + mInitialised = false; + mHaveServer = false; +} // FGMultiplayMgr::FGMultiplayMgr() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// MultiplayMgr destructor +// +////////////////////////////////////////////////////////////////////// +FGMultiplayMgr::~FGMultiplayMgr() +{ + Close(); +} // FGMultiplayMgr::~FGMultiplayMgr() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Initialise object +// +////////////////////////////////////////////////////////////////////// +bool +FGMultiplayMgr::init (void) +{ + ////////////////////////////////////////////////// + // Initialise object if not already done + ////////////////////////////////////////////////// + if (mInitialised) { + SG_LOG(SG_NETWORK, SG_WARN, "FGMultiplayMgr::init - already initialised"); + return false; + } + ////////////////////////////////////////////////// + // Set members from property values + ////////////////////////////////////////////////// + short rxPort = fgGetInt("/sim/multiplay/rxport"); + string rxAddress = fgGetString("/sim/multiplay/rxhost"); + short txPort = fgGetInt("/sim/multiplay/txport"); + string txAddress = fgGetString("/sim/multiplay/txhost"); + mCallsign = fgGetString("/sim/multiplay/callsign"); + if (txPort > 0 && !txAddress.empty()) { + mServer.set(txAddress.c_str(), txPort); + if (strncmp (mServer.getHost(), "0.0.0.0", 8) == 0) { + mHaveServer = false; + SG_LOG(SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr - could not resolve '" + << txAddress << "', Multiplayermode disabled"); + } else { + mHaveServer = true; + } + if (rxPort <= 0) + rxPort = txPort; + } + if (rxPort <= 0) { + SG_LOG(SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr - No receiver port, Multiplayermode disabled"); + return (false); + } + if (mCallsign.empty()) + mCallsign = "JohnDoe"; // FIXME: use getpwuid + SG_LOG(SG_NETWORK,SG_INFO,"FGMultiplayMgr::init-txaddress= "<setBlocking(false); + if (mSocket->bind(rxAddress.c_str(), rxPort) != 0) { + perror("bind"); + SG_LOG( SG_NETWORK, SG_DEBUG, + "FGMultiplayMgr::Open - Failed to bind receive socket" ); + return false; + } + mInitialised = true; + return true; +} // FGMultiplayMgr::init() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Closes and deletes the local player object. Closes +// and deletes the tx socket. Resets the object state to unitialised. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::Close (void) +{ + mMultiPlayerMap.clear(); + + if (mSocket) { + mSocket->close(); + delete mSocket; + mSocket = 0; + } + mInitialised = false; +} // FGMultiplayMgr::Close(void) +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Description: Sends the position data for the local position. +// +////////////////////////////////////////////////////////////////////// + +/** + * The buffer that holds a multi-player message, suitably aligned. + */ +union FGMultiplayMgr::MsgBuf +{ + MsgBuf() + { + memset(&Msg, 0, sizeof(Msg)); + } + + T_MsgHdr* msgHdr() + { + return reinterpret_cast(Msg); + } + + const T_MsgHdr* msgHdr() const + { + return reinterpret_cast(Msg); + } + + T_PositionMsg* posMsg() + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr)); + } + + const T_PositionMsg* posMsg() const + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr)); + } + + xdr_data_t* properties() + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr) + + sizeof(T_PositionMsg)); + } + + const xdr_data_t* properties() const + { + return reinterpret_cast(Msg + sizeof(T_MsgHdr) + + sizeof(T_PositionMsg)); + } + /** + * The end of the properties buffer. + */ + xdr_data_t* propsEnd() + { + return reinterpret_cast(Msg + MAX_PACKET_SIZE); + }; + + const xdr_data_t* propsEnd() const + { + return reinterpret_cast(Msg + MAX_PACKET_SIZE); + }; + /** + * The end of properties actually in the buffer. This assumes that + * the message header is valid. + */ + xdr_data_t* propsRecvdEnd() + { + return reinterpret_cast(Msg + msgHdr()->MsgLen); + } + + const xdr_data_t* propsRecvdEnd() const + { + return reinterpret_cast(Msg + msgHdr()->MsgLen); + } + + xdr_data2_t double_val; + char Msg[MAX_PACKET_SIZE]; +}; + +void +FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) +{ + if ((! mInitialised) || (! mHaveServer)) + return; + if (! mHaveServer) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); + return; + } + + MsgBuf msgBuf; + T_PositionMsg* PosMsg = msgBuf.posMsg(); + + strncpy(PosMsg->Model, fgGetString("/sim/model/path"), MAX_MODEL_NAME_LEN); + PosMsg->Model[MAX_MODEL_NAME_LEN - 1] = '\0'; + + PosMsg->time = XDR_encode_double (motionInfo.time); + PosMsg->lag = XDR_encode_double (motionInfo.lag); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->position[i] = XDR_encode_double (motionInfo.position(i)); + SGVec3f angleAxis; + motionInfo.orientation.getAngleAxis(angleAxis); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->orientation[i] = XDR_encode_float (angleAxis(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->linearVel[i] = XDR_encode_float (motionInfo.linearVel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->angularVel[i] = XDR_encode_float (motionInfo.angularVel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->linearAccel[i] = XDR_encode_float (motionInfo.linearAccel(i)); + for (unsigned i = 0 ; i < 3; ++i) + PosMsg->angularAccel[i] = XDR_encode_float (motionInfo.angularAccel(i)); + + xdr_data_t* ptr = msgBuf.properties(); + std::vector::const_iterator it; + it = motionInfo.properties.begin(); + //cout << "OUTPUT PROPERTIES\n"; + xdr_data_t* msgEnd = msgBuf.propsEnd(); + while (it != motionInfo.properties.end() && ptr + 2 < msgEnd) { + + // First element is the ID. Write it out when we know we have room for + // the whole property. + xdr_data_t id = XDR_encode_uint32((*it)->id); + // The actual data representation depends on the type + switch ((*it)->type) { + case simgear::props::INT: + case simgear::props::BOOL: + case simgear::props::LONG: + *ptr++ = id; + *ptr++ = XDR_encode_uint32((*it)->int_value); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->int_value << "\n"; + break; + case simgear::props::FLOAT: + case simgear::props::DOUBLE: + *ptr++ = id; + *ptr++ = XDR_encode_float((*it)->float_value); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; + break; + case simgear::props::STRING: + case simgear::props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + const char* lcharptr = (*it)->string_value; + + if (lcharptr != 0) + { + // Add the length + ////cout << "String length: " << strlen(lcharptr) << "\n"; + uint32_t len = strlen(lcharptr); + if (len > MAX_TEXT_SIZE) + len = MAX_TEXT_SIZE; + // XXX This should not be using 4 bytes per character! + // If there's not enough room for this property, drop it + // on the floor. + if (ptr + 2 + ((len + 3) & ~3) > msgEnd) + goto escape; + //cout << "String length unint32: " << len << "\n"; + *ptr++ = id; + *ptr++ = XDR_encode_uint32(len); + if (len != 0) + { + // Now the text itself + // XXX This should not be using 4 bytes per character! + int lcount = 0; + while ((*lcharptr != '\0') && (lcount < MAX_TEXT_SIZE)) + { + *ptr++ = XDR_encode_int8(*lcharptr); + lcharptr++; + lcount++; + } + + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " " << len << " " << (*it)->string_value; + + // Now pad if required + while ((lcount % 4) != 0) + { + *ptr++ = XDR_encode_int8(0); + lcount++; + //cout << "0"; + } + + //cout << "\n"; + } + } + else + { + // Nothing to encode + *ptr++ = id; + *ptr++ = XDR_encode_uint32(0); + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " 0\n"; + } + } + break; + + default: + //cout << " Unknown Type: " << (*it)->type << "\n"; + *ptr++ = id; + *ptr++ = XDR_encode_float((*it)->float_value);; + //cout << "Prop:" << (*it)->id << " " << (*it)->type << " "<< (*it)->float_value << "\n"; + break; + } + + ++it; + } +escape: + unsigned msgLen = reinterpret_cast(ptr) - msgBuf.Msg; + FillMsgHdr(msgBuf.msgHdr(), POS_DATA_ID, msgLen); + mSocket->sendto(msgBuf.Msg, msgLen, 0, &mServer); + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition"); +} // FGMultiplayMgr::SendMyPosition() + +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Name: SendTextMessage +// Description: Sends a message to the player. The message must +// contain a valid and correctly filled out header and optional +// message body. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::SendTextMessage(const string &MsgText) +{ + if (!mInitialised || !mHaveServer) + return; + + T_MsgHdr MsgHdr; + FillMsgHdr(&MsgHdr, CHAT_MSG_ID); + ////////////////////////////////////////////////// + // Divide the text string into blocks that fit + // in the message and send the blocks. + ////////////////////////////////////////////////// + unsigned iNextBlockPosition = 0; + T_ChatMsg ChatMsg; + + char Msg[sizeof(T_MsgHdr) + sizeof(T_ChatMsg)]; + while (iNextBlockPosition < MsgText.length()) { + strncpy (ChatMsg.Text, + MsgText.substr(iNextBlockPosition, MAX_CHAT_MSG_LEN - 1).c_str(), + MAX_CHAT_MSG_LEN); + ChatMsg.Text[MAX_CHAT_MSG_LEN - 1] = '\0'; + memcpy (Msg, &MsgHdr, sizeof(T_MsgHdr)); + memcpy (Msg + sizeof(T_MsgHdr), &ChatMsg, sizeof(T_ChatMsg)); + mSocket->sendto (Msg, sizeof(T_MsgHdr) + sizeof(T_ChatMsg), 0, &mServer); + iNextBlockPosition += MAX_CHAT_MSG_LEN - 1; + + } + + +} // FGMultiplayMgr::SendTextMessage () +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// Name: ProcessData +// Description: Processes data waiting at the receive socket. The +// processing ends when there is no more data at the socket. +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::Update(void) +{ + if (!mInitialised) + return; + + /// Just for expiry + long stamp = SGTimeStamp::now().getSeconds(); + + ////////////////////////////////////////////////// + // Read the receive socket and process any data + ////////////////////////////////////////////////// + ssize_t bytes; + do { + MsgBuf msgBuf; + ////////////////////////////////////////////////// + // Although the recv call asks for + // MAX_PACKET_SIZE of data, the number of bytes + // returned will only be that of the next + // packet waiting to be processed. + ////////////////////////////////////////////////// + netAddress SenderAddress; + bytes = mSocket->recvfrom(msgBuf.Msg, sizeof(msgBuf.Msg), 0, + &SenderAddress); + ////////////////////////////////////////////////// + // no Data received + ////////////////////////////////////////////////// + if (bytes <= 0) { + if (errno != EAGAIN && errno != 0) // MSVC output "NoError" otherwise + perror("FGMultiplayMgr::MP_ProcessData"); + break; + } + if (bytes <= static_cast(sizeof(T_MsgHdr))) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "received message with insufficient data" ); + break; + } + ////////////////////////////////////////////////// + // Read header + ////////////////////////////////////////////////// + T_MsgHdr* MsgHdr = msgBuf.msgHdr(); + MsgHdr->Magic = XDR_decode_uint32 (MsgHdr->Magic); + MsgHdr->Version = XDR_decode_uint32 (MsgHdr->Version); + MsgHdr->MsgId = XDR_decode_uint32 (MsgHdr->MsgId); + MsgHdr->MsgLen = XDR_decode_uint32 (MsgHdr->MsgLen); + MsgHdr->ReplyPort = XDR_decode_uint32 (MsgHdr->ReplyPort); + MsgHdr->Callsign[MAX_CALLSIGN_LEN -1] = '\0'; + if (MsgHdr->Magic != MSG_MAGIC) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message has invalid magic number!" ); + break; + } + if (MsgHdr->Version != PROTO_VER) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message has invalid protocoll number!" ); + break; + } + if (MsgHdr->MsgLen != bytes) { + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "message from " << MsgHdr->Callsign << " has invalid length!"); + break; + } + ////////////////////////////////////////////////// + // Process messages + ////////////////////////////////////////////////// + switch (MsgHdr->MsgId) { + case CHAT_MSG_ID: + ProcessChatMsg(msgBuf, SenderAddress); + break; + case POS_DATA_ID: + ProcessPosMsg(msgBuf, SenderAddress, stamp); + break; + case UNUSABLE_POS_DATA_ID: + case OLD_OLD_POS_DATA_ID: + case OLD_PROP_MSG_ID: + case OLD_POS_DATA_ID: + break; + default: + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Unknown message Id received: " << MsgHdr->MsgId ); + break; + } + } while (bytes > 0); + + // check for expiry + MultiPlayerMap::iterator it = mMultiPlayerMap.begin(); + while (it != mMultiPlayerMap.end()) { + if (it->second->getLastTimestamp() + 10 < stamp) { + std::string name = it->first; + it->second->setDie(true); + mMultiPlayerMap.erase(it); + it = mMultiPlayerMap.upper_bound(name); + } else + ++it; + } +} // FGMultiplayMgr::ProcessData(void) +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// handle a position message +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, + const netAddress& SenderAddress, long stamp) +{ + const T_MsgHdr* MsgHdr = Msg.msgHdr(); + if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + sizeof(T_PositionMsg)) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Position message received with insufficient data" ); + return; + } + const T_PositionMsg* PosMsg = Msg.posMsg(); + FGExternalMotionData motionInfo; + motionInfo.time = XDR_decode_double(PosMsg->time); + motionInfo.lag = XDR_decode_double(PosMsg->lag); + for (unsigned i = 0; i < 3; ++i) + motionInfo.position(i) = XDR_decode_double(PosMsg->position[i]); + SGVec3f angleAxis; + for (unsigned i = 0; i < 3; ++i) + angleAxis(i) = XDR_decode_float(PosMsg->orientation[i]); + motionInfo.orientation = SGQuatf::fromAngleAxis(angleAxis); + for (unsigned i = 0; i < 3; ++i) + motionInfo.linearVel(i) = XDR_decode_float(PosMsg->linearVel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.angularVel(i) = XDR_decode_float(PosMsg->angularVel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.linearAccel(i) = XDR_decode_float(PosMsg->linearAccel[i]); + for (unsigned i = 0; i < 3; ++i) + motionInfo.angularAccel(i) = XDR_decode_float(PosMsg->angularAccel[i]); + + + //cout << "INPUT MESSAGE\n"; + + // There was a bug in 1.9.0 and before: T_PositionMsg was 196 bytes + // on 32 bit architectures and 200 bytes on 64 bit, and this + // structure is put directly on the wire. By looking at the padding, + // we can sort through the mess, mostly: + // If padding is 0 (which is not a valid property type), then the + // message was produced by a new client or an old 64 bit client that + // happened to have 0 on the stack; + // Else if the property list starting with the padding word is + // well-formed, then the client is probably an old 32 bit client and + // we'll go with that; + // Else it is an old 64-bit client and properties start after the + // padding. + // There is a chance that we could be fooled by garbage in the + // padding looking like a valid property, so verifyProperties() is + // strict about the validity of the property values. + const xdr_data_t* xdr = Msg.properties(); + if (PosMsg->pad != 0) { + if (verifyProperties(&PosMsg->pad, Msg.propsRecvdEnd())) + xdr = &PosMsg->pad; + else if (!verifyProperties(xdr, Msg.propsRecvdEnd())) + goto noprops; + } + while (xdr < Msg.propsRecvdEnd()) { + FGPropertyData* pData = new FGPropertyData; + // simgear::props::Type type = simgear::props::UNSPECIFIED; + + // First element is always the ID + pData->id = XDR_decode_uint32(*xdr); + //cout << pData->id << " "; + xdr++; + + // Check the ID actually exists and get the type + const IdPropertyList* plist = findProperty(pData->id); + + if (plist) + { + pData->type = plist->type; + // How we decode the remainder of the property depends on the type + switch (pData->type) { + case simgear::props::INT: + case simgear::props::BOOL: + case simgear::props::LONG: + pData->int_value = XDR_decode_uint32(*xdr); + xdr++; + //cout << pData->int_value << "\n"; + break; + case simgear::props::FLOAT: + case simgear::props::DOUBLE: + pData->float_value = XDR_decode_float(*xdr); + xdr++; + //cout << pData->float_value << "\n"; + break; + case simgear::props::STRING: + case simgear::props::UNSPECIFIED: + { + // String is complicated. It consists of + // The length of the string + // The string itself + // Padding to the nearest 4-bytes. + uint32_t length = XDR_decode_uint32(*xdr); + xdr++; + //cout << length << " "; + // Old versions truncated the string but left the length unadjusted. + if (length > MAX_TEXT_SIZE) + length = MAX_TEXT_SIZE; + pData->string_value = new char[length + 1]; + //cout << " String: "; + for (unsigned i = 0; i < length; i++) + { + pData->string_value[i] = (char) XDR_decode_int8(*xdr); + xdr++; + //cout << pData->string_value[i]; + } + + pData->string_value[length] = '\0'; + + // Now handle the padding + while ((length % 4) != 0) + { + xdr++; + length++; + //cout << "0"; + } + //cout << "\n"; + } + break; + + default: + pData->float_value = XDR_decode_float(*xdr); + SG_LOG(SG_NETWORK, SG_DEBUG, "Unknown Prop type " << pData->id << " " << pData->type); + xdr++; + break; + } + + motionInfo.properties.push_back(pData); + } + else + { + // We failed to find the property. We'll try the next packet immediately. + SG_LOG(SG_NETWORK, SG_INFO, "FGMultiplayMgr::ProcessPosMsg - " + "message from " << MsgHdr->Callsign << " has unknown property id " + << pData->id); + } + } + noprops: + FGAIMultiplayer* mp = getMultiplayer(MsgHdr->Callsign); + if (!mp) + mp = addMultiplayer(MsgHdr->Callsign, PosMsg->Model); + mp->addMotionInfo(motionInfo, stamp); +} // FGMultiplayMgr::ProcessPosMsg() +////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////// +// +// handle a chat message +// FIXME: display chat message withi flightgear +// +////////////////////////////////////////////////////////////////////// +void +FGMultiplayMgr::ProcessChatMsg(const MsgBuf& Msg, + const netAddress& SenderAddress) +{ + const T_MsgHdr* MsgHdr = Msg.msgHdr(); + if (MsgHdr->MsgLen < sizeof(T_MsgHdr) + 1) { + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " + << "Chat message received with insufficient data" ); + return; + } + + char *chatStr = new char[MsgHdr->MsgLen - sizeof(T_MsgHdr)]; + const T_ChatMsg* ChatMsg + = reinterpret_cast(Msg.Msg + sizeof(T_MsgHdr)); + strncpy(chatStr, ChatMsg->Text, + MsgHdr->MsgLen - sizeof(T_MsgHdr)); + chatStr[MsgHdr->MsgLen - sizeof(T_MsgHdr) - 1] = '\0'; + + SG_LOG (SG_NETWORK, SG_WARN, "Chat [" << MsgHdr->Callsign << "]" + << " " << chatStr); + + delete [] chatStr; +} // FGMultiplayMgr::ProcessChatMsg () +////////////////////////////////////////////////////////////////////// + +void +FGMultiplayMgr::FillMsgHdr(T_MsgHdr *MsgHdr, int MsgId, unsigned _len) +{ + uint32_t len; + switch (MsgId) { + case CHAT_MSG_ID: + len = sizeof(T_MsgHdr) + sizeof(T_ChatMsg); + break; + case POS_DATA_ID: + len = _len; + break; + default: + len = sizeof(T_MsgHdr); + break; + } + MsgHdr->Magic = XDR_encode_uint32(MSG_MAGIC); + MsgHdr->Version = XDR_encode_uint32(PROTO_VER); + MsgHdr->MsgId = XDR_encode_uint32(MsgId); + MsgHdr->MsgLen = XDR_encode_uint32(len); + MsgHdr->ReplyAddress = 0; // Are obsolete, keep them for the server for + MsgHdr->ReplyPort = 0; // now + strncpy(MsgHdr->Callsign, mCallsign.c_str(), MAX_CALLSIGN_LEN); + MsgHdr->Callsign[MAX_CALLSIGN_LEN - 1] = '\0'; +} + +FGAIMultiplayer* +FGMultiplayMgr::addMultiplayer(const std::string& callsign, + const std::string& modelName) +{ + if (0 < mMultiPlayerMap.count(callsign)) + return mMultiPlayerMap[callsign].get(); + + FGAIMultiplayer* mp = new FGAIMultiplayer; + mp->setPath(modelName.c_str()); + mp->setCallSign(callsign); + mMultiPlayerMap[callsign] = mp; + + FGAIManager *aiMgr = (FGAIManager*)globals->get_subsystem("ai_model"); + if (aiMgr) { + aiMgr->attach(mp); + + /// FIXME: that must follow the attach ATM ... + for (unsigned i = 0; i < numProperties; ++i) + mp->addPropertyId(sIdPropertyList[i].id, sIdPropertyList[i].name); + } + + return mp; +} + +FGAIMultiplayer* +FGMultiplayMgr::getMultiplayer(const std::string& callsign) +{ + if (0 < mMultiPlayerMap.count(callsign)) + return mMultiPlayerMap[callsign].get(); + else + return 0; +}