+ }
+
+
+ // Compute the time in the feeders time scale which fits the current time
+ // we need to
+ double tInterp = curtime + mTimeOffset;
+
+ SGVec3d ecPos;
+ SGQuatf ecOrient;
+ SGVec3f ecLinearVel;
+
+ if (tInterp <= curentPkgTime) {
+ // Ok, we need a time prevous to the last available packet,
+ // that is good ...
+
+ // Find the first packet before the target time
+ MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
+ if (nextIt == mMotionInfo.begin()) {
+ SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!");
+ // We have no packet before the target time, just use the first one
+ MotionInfo::iterator firstIt = mMotionInfo.begin();
+ ecPos = firstIt->second.position;
+ ecOrient = firstIt->second.orientation;
+ ecLinearVel = firstIt->second.linearVel;
+ speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
+
+ std::vector<FGPropertyData*>::const_iterator firstPropIt;
+ std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
+ firstPropIt = firstIt->second.properties.begin();
+ firstPropItEnd = firstIt->second.properties.end();
+ while (firstPropIt != firstPropItEnd) {
+ //cout << " Setting property..." << (*firstPropIt)->id;
+ PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
+ if (pIt != mPropertyMap.end())
+ {
+ //cout << "Found " << pIt->second->getPath() << ":";
+ switch ((*firstPropIt)->type) {
+ case props::INT:
+ case props::BOOL:
+ case props::LONG:
+ pIt->second->setIntValue((*firstPropIt)->int_value);
+ //cout << "Int: " << (*firstPropIt)->int_value << "\n";
+ break;
+ case props::FLOAT:
+ case props::DOUBLE:
+ pIt->second->setFloatValue((*firstPropIt)->float_value);
+ //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
+ break;
+ case props::STRING:
+ case props::UNSPECIFIED:
+ pIt->second->setStringValue((*firstPropIt)->string_value);
+ //cout << "Str: " << (*firstPropIt)->string_value << "\n";
+ break;
+ default:
+ // FIXME - currently defaults to float values
+ pIt->second->setFloatValue((*firstPropIt)->float_value);
+ //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
+ break;
+ }
+ }
+ else
+ {
+ SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
+ }
+ ++firstPropIt;
+ }
+
+ } else {
+ // Ok, we have really found something where our target time is in between
+ // do interpolation here
+ MotionInfo::iterator prevIt = nextIt;
+ --prevIt;
+
+ // 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_AI, 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);
+ ecLinearVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel);
+ speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
+
+ if (prevIt->second.properties.size()
+ == nextIt->second.properties.size()) {
+ std::vector<FGPropertyData*>::const_iterator prevPropIt;
+ std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
+ std::vector<FGPropertyData*>::const_iterator nextPropIt;
+ std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
+ prevPropIt = prevIt->second.properties.begin();
+ prevPropItEnd = prevIt->second.properties.end();
+ nextPropIt = nextIt->second.properties.begin();
+ nextPropItEnd = nextIt->second.properties.end();
+ while (prevPropIt != prevPropItEnd) {
+ PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
+ //cout << " Setting property..." << (*prevPropIt)->id;
+
+ if (pIt != mPropertyMap.end())
+ {
+ //cout << "Found " << pIt->second->getPath() << ":";
+
+ int ival;
+ float val;
+ switch ((*prevPropIt)->type) {
+ case props::INT:
+ case props::BOOL:
+ case props::LONG:
+ ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
+ tau*((double) (*nextPropIt)->int_value));
+ pIt->second->setIntValue(ival);
+ //cout << "Int: " << ival << "\n";
+ break;
+ case props::FLOAT:
+ case props::DOUBLE:
+ val = (1-tau)*(*prevPropIt)->float_value +
+ tau*(*nextPropIt)->float_value;
+ //cout << "Flo: " << val << "\n";
+ pIt->second->setFloatValue(val);
+ break;
+ case props::STRING:
+ case props::UNSPECIFIED:
+ //cout << "Str: " << (*nextPropIt)->string_value << "\n";
+ pIt->second->setStringValue((*nextPropIt)->string_value);
+ break;
+ default:
+ // FIXME - currently defaults to float values
+ val = (1-tau)*(*prevPropIt)->float_value +
+ tau*(*nextPropIt)->float_value;
+ //cout << "Unk: " << val << "\n";
+ pIt->second->setFloatValue(val);
+ break;
+ }
+ }
+ else
+ {
+ SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
+ }
+
+ ++prevPropIt;
+ ++nextPropIt;
+ }
+ }
+
+ // Now throw away too old data
+ if (prevIt != mMotionInfo.begin())
+ {
+ --prevIt;
+ mMotionInfo.erase(mMotionInfo.begin(), prevIt);
+ }
+ }
+ } else {
+ // Ok, we need to predict the future, so, take the best data we can have
+ // and do some eom computation to guess that for now.
+ FGExternalMotionData& motionInfo = it->second;
+
+ // The time to predict, limit to 5 seconds
+ double t = tInterp - motionInfo.time;
+ t = SGMisc<double>::min(t, 5);
+
+ SG_LOG(SG_AI, 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;
+ ecLinearVel = motionInfo.linearVel;
+ SGVec3f angularVel = motionInfo.angularVel;
+ while (0 < t) {
+ double h = 1e-1;
+ if (t < h)
+ h = t;
+
+ SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel));
+ ecPos += h*ecVel;
+ ecOrient += h*ecOrient.derivative(angularVel);
+
+ ecLinearVel += h*(cross(ecLinearVel, angularVel) + motionInfo.linearAccel);
+ angularVel += h*motionInfo.angularAccel;
+
+ t -= h;