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., 675 Mass Ave, Cambridge, MA 02139, 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 return FGAIBase::init();
50 void FGAIMultiplayer::bind() {
53 #define AIMPROProp(type, name) \
54 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
56 #define AIMPRWProp(type, name) \
57 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
58 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
60 props->tie("callsign", AIMPROProp(const char *, CallSign));
62 props->tie("controls/allow-extrapolation",
63 AIMPRWProp(bool, AllowExtrapolation));
64 props->tie("controls/lag-adjust-system-speed",
65 AIMPRWProp(double, LagAdjustSystemSpeed));
71 void FGAIMultiplayer::unbind() {
74 props->untie("callsign");
75 props->untie("controls/allow-extrapolation");
76 props->untie("controls/lag-adjust-system-speed");
79 void FGAIMultiplayer::update(double dt)
86 // Check if we already got data
87 if (mMotionInfo.empty())
90 // The current simulation time we need to update for,
91 // note that the simulation time is updated before calling all the
92 // update methods. Thus it contains the time intervals *end* time
93 double curtime = globals->get_sim_time_sec();
95 // Get the last available time
96 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
97 double curentPkgTime = it->second.time;
99 // Dynamically optimize the time offset between the feeder and the client
100 // Well, 'dynamically' means that the dynamic of that update must be very
101 // slow. You would otherwise notice huge jumps in the multiplayer models.
102 // The reason is that we want to avoid huge extrapolation times since
103 // extrapolation is highly error prone. For that we need something
104 // approaching the average latency of the packets. This first order lag
105 // component will provide this. We just take the error of the currently
106 // requested time to the most recent available packet. This is the
107 // target we want to reach in average.
108 double lag = it->second.lag;
109 if (!mTimeOffsetSet) {
110 mTimeOffsetSet = true;
111 mTimeOffset = curentPkgTime - curtime - lag;
113 double offset = curentPkgTime - curtime - lag;
114 if (!mAllowExtrapolation && offset + lag < mTimeOffset) {
115 mTimeOffset = offset;
116 SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to "
117 "avoid extrapolation: time offset = " << mTimeOffset);
119 // the error of the offset, respectively the negative error to avoid
121 double err = offset - mTimeOffset;
122 // limit errors leading to shorter lag values somehow, that is late
123 // arriving packets will pessimize the overall lag much more than
124 // early packets will shorten the overall lag
127 // Ok, we have some very late packets and nothing newer increase the
128 // lag by the given speedadjust
129 sysSpeed = mLagAdjustSystemSpeed*err;
131 // We have a too pessimistic display delay shorten that a small bit
132 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
135 // simple euler integration for that first order system including some
136 // overshooting guard to prevent to aggressive system speeds
137 // (stiff systems) to explode the systems state
138 double systemIncrement = dt*sysSpeed;
139 if (fabs(err) < fabs(systemIncrement))
140 systemIncrement = err;
141 mTimeOffset += systemIncrement;
143 SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = "
144 << mTimeOffset << ", expected longitudinal position error due to "
145 " current adjustment of the offset: "
146 << fabs(norm(it->second.linearVel)*systemIncrement));
151 // Compute the time in the feeders time scale which fits the current time
153 double tInterp = curtime + mTimeOffset;
157 if (tInterp <= curentPkgTime) {
158 // Ok, we need a time prevous to the last available packet,
161 // Find the first packet before the target time
162 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
163 if (nextIt == mMotionInfo.begin()) {
164 SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!");
165 // We have no packet before the target time, just use the first one
166 MotionInfo::iterator firstIt = mMotionInfo.begin();
167 ecPos = firstIt->second.position;
168 ecOrient = firstIt->second.orientation;
170 std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
171 std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
172 firstPropIt = firstIt->second.properties.begin();
173 firstPropItEnd = firstIt->second.properties.end();
174 while (firstPropIt != firstPropItEnd) {
175 float val = firstPropIt->value;
176 PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
177 if (pIt != mPropertyMap.end())
178 pIt->second->setFloatValue(val);
183 // Ok, we have really found something where our target time is in between
184 // do interpolation here
185 MotionInfo::iterator prevIt = nextIt;
188 // Interpolation coefficient is between 0 and 1
189 double intervalStart = prevIt->second.time;
190 double intervalEnd = nextIt->second.time;
191 double intervalLen = intervalEnd - intervalStart;
192 double tau = (tInterp - intervalStart)/intervalLen;
194 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: ["
195 << intervalStart << ", " << intervalEnd << "], intervalLen = "
196 << intervalLen << ", interpolation parameter = " << tau);
198 // Here we do just linear interpolation on the position
199 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
200 ecOrient = interpolate((float)tau, prevIt->second.orientation,
201 nextIt->second.orientation);
203 if (prevIt->second.properties.size()
204 == nextIt->second.properties.size()) {
205 std::vector<FGFloatPropertyData>::const_iterator prevPropIt;
206 std::vector<FGFloatPropertyData>::const_iterator prevPropItEnd;
207 std::vector<FGFloatPropertyData>::const_iterator nextPropIt;
208 std::vector<FGFloatPropertyData>::const_iterator nextPropItEnd;
209 prevPropIt = prevIt->second.properties.begin();
210 prevPropItEnd = prevIt->second.properties.end();
211 nextPropIt = nextIt->second.properties.begin();
212 nextPropItEnd = nextIt->second.properties.end();
213 while (prevPropIt != prevPropItEnd) {
214 float val = (1-tau)*prevPropIt->value + tau*nextPropIt->value;
215 PropertyMap::iterator pIt = mPropertyMap.find(prevPropIt->id);
216 if (pIt != mPropertyMap.end())
217 pIt->second->setFloatValue(val);
223 // Now throw away too old data
224 if (prevIt != mMotionInfo.begin()) {
226 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
230 // Ok, we need to predict the future, so, take the best data we can have
231 // and do some eom computation to guess that for now.
232 FGExternalMotionData motionInfo = it->second;
234 // The time to predict, limit to 5 seconds
235 double t = tInterp - motionInfo.time;
236 t = SGMisc<double>::min(t, 5);
238 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: "
239 "extrapolation time = " << t);
241 // Do a few explicit euler steps with the constant acceleration's
242 // This must be sufficient ...
243 ecPos = motionInfo.position;
244 ecOrient = motionInfo.orientation;
245 SGVec3f linearVel = motionInfo.linearVel;
246 SGVec3f angularVel = motionInfo.angularVel;
252 SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
254 ecOrient += h*ecOrient.derivative(angularVel);
256 linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
257 angularVel += h*motionInfo.angularAccel;
262 std::vector<FGFloatPropertyData>::const_iterator firstPropIt;
263 std::vector<FGFloatPropertyData>::const_iterator firstPropItEnd;
264 firstPropIt = it->second.properties.begin();
265 firstPropItEnd = it->second.properties.end();
266 while (firstPropIt != firstPropItEnd) {
267 float val = firstPropIt->value;
268 PropertyMap::iterator pIt = mPropertyMap.find(firstPropIt->id);
269 if (pIt != mPropertyMap.end())
270 pIt->second->setFloatValue(val);
275 // extract the position
277 pos.setlat(geod.getLatitudeDeg());
278 pos.setlon(geod.getLongitudeDeg());
279 pos.setelev(geod.getElevationM());
281 // The quaternion rotating from the earth centered frame to the
282 // horizontal local frame
283 SGQuatf qEc2Hl = SGQuatf::fromLonLat((float)geod.getLongitudeRad(),
284 (float)geod.getLatitudeRad());
285 // The orientation wrt the horizontal local frame
286 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
287 float hDeg, pDeg, rDeg;
288 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
293 SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: "
294 << geod << ", " << hlOr);
296 //###########################//
297 // do calculations for radar //
298 //###########################//
299 UpdateRadar(manager);
305 FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo,
308 mLastTimestamp = stamp;
309 // Drop packets arriving out of order
310 if (!mMotionInfo.empty() && motionInfo.time < mMotionInfo.rbegin()->first)
312 mMotionInfo[motionInfo.time] = motionInfo;
316 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
318 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
319 pNode->setDoubleValue(val);