1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
3 // Based on FGAIAircraft
4 // Written by David Culp, started October 2003.
5 // Also by Gregor Richards, started December 2005.
7 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
8 // Copyright (C) 2005 Gregor Richards
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // General Public License for more details.
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
30 #include "AIMultiplayer.hxx"
32 // #define SG_DEBUG SG_ALERT
34 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
37 mTimeOffsetSet = false;
38 mAllowExtrapolation = true;
39 mLagAdjustSystemSpeed = 10;
43 FGAIMultiplayer::~FGAIMultiplayer() {
46 bool FGAIMultiplayer::init() {
47 //refuel_node = fgGetNode("systems/refuel/contact", true);
48 isTanker = false; // do this until this property is
49 // passed over the net
51 string str1 = mCallSign;
52 string str2 = "MOBIL";
54 unsigned int loc1= str1.find( str2, 0 );
55 if ( (loc1 != string::npos && str2 != "") ){
56 // cout << " string found " << str2 << " in " << str1 << endl;
58 // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
60 return FGAIBase::init();
63 void FGAIMultiplayer::bind() {
66 props->tie("refuel/contact", SGRawValuePointer<bool>(&contact));
67 props->setBoolValue("tanker",isTanker);
69 #define AIMPROProp(type, name) \
70 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
72 #define AIMPRWProp(type, name) \
73 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
74 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
76 props->tie("callsign", AIMPROProp(const char *, CallSign));
78 props->tie("controls/allow-extrapolation",
79 AIMPRWProp(bool, AllowExtrapolation));
80 props->tie("controls/lag-adjust-system-speed",
81 AIMPRWProp(double, LagAdjustSystemSpeed));
88 void FGAIMultiplayer::unbind() {
91 props->untie("callsign");
92 props->untie("controls/allow-extrapolation");
93 props->untie("controls/lag-adjust-system-speed");
94 props->untie("refuel/contact");
97 void FGAIMultiplayer::update(double dt)
102 FGAIBase::update(dt);
104 // Check if we already got data
105 if (mMotionInfo.empty())
108 // The current simulation time we need to update for,
109 // note that the simulation time is updated before calling all the
110 // update methods. Thus it contains the time intervals *end* time
111 double curtime = globals->get_sim_time_sec();
113 // Get the last available time
114 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
115 double curentPkgTime = it->second.time;
117 // Dynamically optimize the time offset between the feeder and the client
118 // Well, 'dynamically' means that the dynamic of that update must be very
119 // slow. You would otherwise notice huge jumps in the multiplayer models.
120 // The reason is that we want to avoid huge extrapolation times since
121 // extrapolation is highly error prone. For that we need something
122 // approaching the average latency of the packets. This first order lag
123 // component will provide this. We just take the error of the currently
124 // requested time to the most recent available packet. This is the
125 // target we want to reach in average.
126 double lag = it->second.lag;
127 if (!mTimeOffsetSet) {
128 mTimeOffsetSet = true;
129 mTimeOffset = curentPkgTime - curtime - lag;
131 double offset = curentPkgTime - curtime - lag;
132 if (!mAllowExtrapolation && offset + lag < mTimeOffset) {
133 mTimeOffset = offset;
134 SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
135 "avoid extrapolation: time offset = " << mTimeOffset);
137 // the error of the offset, respectively the negative error to avoid
139 double err = offset - mTimeOffset;
140 // limit errors leading to shorter lag values somehow, that is late
141 // arriving packets will pessimize the overall lag much more than
142 // early packets will shorten the overall lag
145 // Ok, we have some very late packets and nothing newer increase the
146 // lag by the given speedadjust
147 sysSpeed = mLagAdjustSystemSpeed*err;
149 // We have a too pessimistic display delay shorten that a small bit
150 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
153 // simple euler integration for that first order system including some
154 // overshooting guard to prevent to aggressive system speeds
155 // (stiff systems) to explode the systems state
156 double systemIncrement = dt*sysSpeed;
157 if (fabs(err) < fabs(systemIncrement))
158 systemIncrement = err;
159 mTimeOffset += systemIncrement;
161 SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
162 << mTimeOffset << ", expected longitudinal position error due to "
163 " current adjustment of the offset: "
164 << fabs(norm(it->second.linearVel)*systemIncrement));
169 // Compute the time in the feeders time scale which fits the current time
171 double tInterp = curtime + mTimeOffset;
176 if (tInterp <= curentPkgTime) {
177 // Ok, we need a time prevous to the last available packet,
180 // Find the first packet before the target time
181 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
182 if (nextIt == mMotionInfo.begin()) {
183 SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
184 // We have no packet before the target time, just use the first one
185 MotionInfo::iterator firstIt = mMotionInfo.begin();
186 ecPos = firstIt->second.position;
187 ecOrient = firstIt->second.orientation;
188 speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
190 std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
191 std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
192 firstPropIt = firstIt->second.properties.begin();
193 firstPropItEnd = firstIt->second.properties.end();
194 while (firstPropIt != firstPropItEnd) {
195 float val = firstPropIt->value;
196 PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
197 if (pIt != mPropertyMap.end())
198 pIt->second->setFloatValue(val);
203 // Ok, we have really found something where our target time is in between
204 // do interpolation here
205 MotionInfo::iterator prevIt = nextIt;
208 // Interpolation coefficient is between 0 and 1
209 double intervalStart = prevIt->second.time;
210 double intervalEnd = nextIt->second.time;
211 double intervalLen = intervalEnd - intervalStart;
212 double tau = (tInterp - intervalStart)/intervalLen;
214 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
215 << intervalStart << ", " << intervalEnd << "], intervalLen = "
216 << intervalLen << ", interpolation parameter = " << tau);
218 // Here we do just linear interpolation on the position
219 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
220 ecOrient = interpolate((float)tau, prevIt->second.orientation,
221 nextIt->second.orientation);
222 speed = norm((1-tau)*prevIt->second.linearVel
223 + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
225 if (prevIt->second.properties.size()
226 == nextIt->second.properties.size()) {
227 std::vector<FGFloatPropertyData>::const_iterator prevPropIt;
228 std::vector<FGFloatPropertyData>::const_iterator prevPropItEnd;
229 std::vector<FGFloatPropertyData>::const_iterator nextPropIt;
230 std::vector<FGFloatPropertyData>::const_iterator nextPropItEnd;
231 prevPropIt = prevIt->second.properties.begin();
232 prevPropItEnd = prevIt->second.properties.end();
233 nextPropIt = nextIt->second.properties.begin();
234 nextPropItEnd = nextIt->second.properties.end();
235 while (prevPropIt != prevPropItEnd) {
236 float val = (1-tau)*prevPropIt->value + tau*nextPropIt->value;
237 PropertyMap::iterator pIt = mPropertyMap.find(prevPropIt->id);
238 if (pIt != mPropertyMap.end())
239 pIt->second->setFloatValue(val);
245 // Now throw away too old data
246 if (prevIt != mMotionInfo.begin()) {
248 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
252 // Ok, we need to predict the future, so, take the best data we can have
253 // and do some eom computation to guess that for now.
254 FGExternalMotionData motionInfo = it->second;
256 // The time to predict, limit to 5 seconds
257 double t = tInterp - motionInfo.time;
258 t = SGMisc<double>::min(t, 5);
260 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
261 "extrapolation time = " << t);
263 // Do a few explicit euler steps with the constant acceleration's
264 // This must be sufficient ...
265 ecPos = motionInfo.position;
266 ecOrient = motionInfo.orientation;
267 SGVec3f linearVel = motionInfo.linearVel;
268 SGVec3f angularVel = motionInfo.angularVel;
274 SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
276 ecOrient += h*ecOrient.derivative(angularVel);
278 linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
279 angularVel += h*motionInfo.angularAccel;
284 speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
286 std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
287 std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
288 firstPropIt = it->second.properties.begin();
289 firstPropItEnd = it->second.properties.end();
290 while (firstPropIt != firstPropItEnd) {
291 float val = firstPropIt->value;
292 PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
293 if (pIt != mPropertyMap.end())
294 pIt->second->setFloatValue(val);
299 // extract the position
300 pos = SGGeod::fromCart(ecPos);
301 altitude_ft = pos.getElevationFt();
303 // The quaternion rotating from the earth centered frame to the
304 // horizontal local frame
305 SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
306 (float)pos.getLatitudeRad());
307 // The orientation wrt the horizontal local frame
308 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
309 float hDeg, pDeg, rDeg;
310 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
315 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
316 << ecPos << ", " << hlOr);
318 //###########################//
319 // do calculations for radar //
320 //###########################//
321 double range_ft2 = UpdateRadar(manager);
323 //************************************//
325 //************************************//
329 if ( (range_ft2 < 250.0 * 250.0) &&
332 // refuel_node->setBoolValue(true);
335 // refuel_node->setBoolValue(false);
346 FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
349 mLastTimestamp = stamp;
350 // Drop packets arriving out of order
351 if (!mMotionInfo.empty() && motionInfo.time < mMotionInfo.rbegin()->first)
353 mMotionInfo[motionInfo.time] = motionInfo;
357 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
359 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
360 pNode->setDoubleValue(val);