1 // Copyright (C) 2009 - 2012 Mathias Froehlich
3 // This program is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU General Public License as
5 // published by the Free Software Foundation; either version 2 of the
6 // License, or (at your option) any later version.
8 // This program is distributed in the hope that it will be useful, but
9 // WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // General Public License for more details.
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
23 #include <simgear/misc/sg_path.hxx>
24 #include <simgear/debug/logstream.hxx>
26 #include "AIObject.hxx"
27 #include "AIManager.hxx"
29 #include "HLAMPAircraft.hxx"
30 #include "HLAMPAircraftClass.hxx"
31 #include "AIPhysics.hxx"
36 class AIVehiclePhysics : public AIPhysics {
38 AIVehiclePhysics(const SGLocationd& location, const SGVec3d& linearBodyVelocity = SGVec3d::zeros(),
39 const SGVec3d& angularBodyVelocity = SGVec3d::zeros()) :
40 AIPhysics(location, linearBodyVelocity, angularBodyVelocity)
45 virtual ~AIVehiclePhysics()
48 double getMass() const
50 void setMass(double mass)
53 void setInertia(double ixx, double iyy, double izz, double ixy = 0, double ixz = 0, double iyz = 0)
55 _inertia = SGMatrixd(ixx, ixy, ixz, 0,
59 invert(_inertiaInverse, _inertia);
63 void advanceByBodyForce(const double& dt,
65 const SGVec3d& torque)
67 SGVec3d linearVelocity = getLinearBodyVelocity();
68 SGVec3d angularVelocity = getAngularBodyVelocity();
70 SGVec3d linearAcceleration = (1/_mass)*(force - cross(angularVelocity, linearVelocity));
71 SGVec3d angularAcceleration = _inertiaInverse.xformVec(torque - cross(angularVelocity, _inertia.xformVec(angularVelocity)));
73 advanceByBodyAcceleration(dt, linearAcceleration, angularAcceleration);
76 SGVec3d getGravityAcceleration() const
78 return SGQuatd::fromLonLat(getGeodPosition()).backTransform(SGVec3d(0, 0, 9.81));
82 // unsimulated motion, hide this for this kind of class here
83 using AIPhysics::advanceByBodyAcceleration;
84 using AIPhysics::advanceByBodyVelocity;
85 using AIPhysics::advanceToCartPosition;
88 // FIXME this is a symmetric 3x3 matrix ...
90 SGMatrixd _inertiaInverse;
93 // FIXME Totally unfinished simple aerodynamics model for an ai aircraft
94 // also just here for a sketch of an idea
95 class AIAircraftPhysics : public AIVehiclePhysics {
97 AIAircraftPhysics(const SGLocationd& location, const SGVec3d& linearBodyVelocity = SGVec3d::zeros(),
98 const SGVec3d& angularBodyVelocity = SGVec3d::zeros()) :
99 AIVehiclePhysics(location, linearBodyVelocity, angularBodyVelocity)
101 virtual ~AIAircraftPhysics()
104 double getElevatorPosition() const
105 { return _elevatorPosition; }
106 void setElevatorPosition(double elevatorPosition)
107 { _elevatorPosition = elevatorPosition; }
109 double getAileronPosition() const
110 { return _aileronPosition; }
111 void setAileronPosition(double aileronPosition)
112 { _aileronPosition = aileronPosition; }
114 double getRudderPosition() const
115 { return _rudderPosition; }
116 void setRudderPosition(double rudderPosition)
117 { _rudderPosition = rudderPosition; }
119 // double getFlapsPosition() const
120 // { return _flapsPosition; }
121 // void setFlapsPosition(double flapsPosition)
122 // { _flapsPosition = flapsPosition; }
124 double getThrust() const
126 void setThrust(double thrust)
127 { _thrust = thrust; }
129 virtual void update(AIObject& object, const SGTimeStamp& dt)
131 // const AIEnvironment& environment = object.getEnvironment();
132 const AIEnvironment environment;
134 /// Compute the forces on the aircraft. This is a very simple fdm.
136 // The velocity of the aircraft wrt the surrounding fluid
137 SGVec3d windVelocity = getLinearBodyVelocity();
138 windVelocity -= getOrientation().transform(environment.getWindVelocity());
140 // The true airspeed of the bird
141 double airSpeed = norm(windVelocity);
142 // simple density with(out FIXME) altitude
143 double density = environment.getDensity();
144 // The dynaimc pressure - most important value in aerodynamics
145 double dynamicPressure = 0.5*density*airSpeed*airSpeed;
147 // The angle of attack and sideslip angle
148 double alpha = 0, beta = 0;
149 if (1e-3 < SGMiscd::max(fabs(windVelocity[0]), fabs(windVelocity[2])))
150 alpha = atan2(windVelocity[2], windVelocity[0]);
151 double uw = sqrt(windVelocity[0]*windVelocity[0] + windVelocity[2]*windVelocity[2]);
152 if (1e-3 < SGMiscd::max(fabs(windVelocity[1]), fabs(uw)))
153 beta = atan2(windVelocity[1], uw);
154 // Transform from the stability axis to body axis
155 SGQuatd stabilityToBody = SGQuatd::fromEulerRad(beta, alpha, 0);
157 // We assume a simple angular dependency for the
158 // lift, drag and side force coefficients.
160 // lift for alpha = 0
162 // lift at stall angle of attack
164 // stall angle of attack
165 double _alphaStall = 18;
166 // Drag for alpha = 0
168 // Drag for alpha = 90
170 // Side force coefficient for maximum side angle
175 SGVec3d _aerodynamicReferencePoint(0, 0, 0);
176 SGVec3d _centerOfGravity(0, 0, 0);
178 // So compute the lift drag and side force coefficient for the
179 // current stream conditions.
180 double Cl = _Cl0 + (_ClStall - _Cl0)*sin(SGMiscd::clip(90/_alphaStall*alpha, -0.5*SGMiscd::pi(), SGMiscd::pi()));
181 double Cd = _Cd0 + (_Cd90 - _Cd0)*(0.5 - 0.5*cos(2*alpha));
182 double Cs = _Cs90*sin(beta);
184 // Forces in the stability axes
185 double lift = Cl*dynamicPressure*_area;
186 double drag = Cd*dynamicPressure*_area;
187 double side = Cs*dynamicPressure*_area;
189 // Torque in body axes
194 // Compute the force in stability coordinates ...
195 SGVec3d stabilityForce(-drag, side, -lift);
196 // ... and torque in body coordinates
197 SGVec3d torque(p, q, r);
199 SGVec3d force = stabilityToBody.transform(stabilityForce);
200 torque += cross(force, _aerodynamicReferencePoint);
202 std::pair<SGVec3d, SGVec3d> velocity;
203 for (_GearVector::iterator i = _gearVector.begin(); i != _gearVector.end(); ++i) {
204 std::pair<SGVec3d, SGVec3d> torqueForce;
205 torqueForce = i->force(getLocation(), velocity, object);
206 torque += torqueForce.first;
207 force += torqueForce.second;
210 // Transform the torque back to the center of gravity
211 torque -= cross(force, _centerOfGravity);
213 // Advance the equations of motion.
214 advanceByBodyForce(dt.toSecs(), force, torque);
217 /// The normalized elevator position
218 double _elevatorPosition;
219 /// The normalized aileron position
220 double _aileronPosition;
221 /// The normalized rudder position
222 double _rudderPosition;
223 // /// The normalized flaps position
224 // double _flapsPosition;
225 /// Normalized thrust
234 std::pair<SGVec3d, SGVec3d>
235 force(const SGLocationd& location, const std::pair<SGVec3d, SGVec3d>& velocity, AIObject& object)
237 SGVec3d start = location.getAbsolutePosition(_position - _direction);
238 SGVec3d end = location.getAbsolutePosition(_position);
239 SGLineSegmentd lineSegment(start, end);
243 // if (!object.getGroundIntersection(point, normal, lineSegment))
244 // return std::pair<SGVec3d, SGVec3d>(SGVec3d(0, 0, 0), SGVec3d(0, 0, 0));
246 // FIXME just now only the spring force ...
248 // The compression length
249 double compressLength = norm(point - start);
250 SGVec3d springForce = -_spring*compressLength*_direction;
252 SGVec3d dampingForce(0, 0, 0);
254 SGVec3d force = springForce + dampingForce;
255 SGVec3d torque(0, 0, 0);
257 return std::pair<SGVec3d, SGVec3d>(torque, force);
261 typedef std::vector<_Gear> _GearVector;
262 _GearVector _gearVector;
264 /// The total mass of the bird in kg. No fluel is burned.
265 /// Some sensible inertia values are derived from the mass.
267 /// The thrust to mass ratio which tells us someting about
268 /// the possible accelerations.
269 // double _thrustMassRatio;
272 // double _stallSpeed;
273 // double _maximumSpeed;
274 // // double _approachSpeed;
275 // // double _takeoffSpeed;
276 // // double _cruiseSpeed;
277 /// The maximum density altitude this aircraft can fly
278 // double _densityAltitudeLimit;
280 /// statistical evaluation shows:
281 /// wingarea = C*wingspan^2, C in [0.1, 0.4], say 0.2
282 /// ixx = C*wingarea*mass, C in [1e-3, 1e-2]
283 /// iyy = C*wingarea*mass, C in [1e-2, 0.02]
284 /// izz = C*wingarea*mass, C in [1e-2, 0.02]
285 /// Hmm, let's say, weight relates to wingarea?
286 /// probably, since lift is linear dependent on wingarea
288 /// So, define a 'size' in meters.
291 /// wingarea = 0.2*size*size
292 /// mass = C*wingarea
293 /// ixx = 0.005*wingarea*mass
294 /// iyy = 0.05*wingarea*mass
295 /// izz = 0.05*wingarea*mass
299 /// define a bird of some weight class. That means mass given.
301 /// i* ??? must be mass^2 accodring to the thoughts above
302 /// Then do Cl, Cd, Cs.
303 /// according to approach speed at sea level with 5 deg aoa and 2,5 deg glideslope and 25 % thrust.
304 /// according to cruise altitude and cruise speed at 75% thrust compute this at altitude
305 /// interpolate between these two sets of C*'s based on altitude.
309 /// An automated lego aircraft, constant linear and angular speed
310 class AIOgel : public AIObject {
312 AIOgel(const SGGeod& geod) :
315 _turnaroundTime(3*60),
321 virtual void init(AIManager& manager)
323 AIObject::init(manager);
325 SGLocationd location(SGVec3d::fromGeod(_geod), SGQuatd::fromLonLat(_geod));
326 SGVec3d linearVelocity(_velocity, 0, 0);
327 SGVec3d angularVelocity(0, 0, SGMiscd::twopi()/_turnaroundTime);
328 setPhysics(new AIPhysics(location, linearVelocity, angularVelocity));
330 HLAMPAircraftClass* objectClass = dynamic_cast<HLAMPAircraftClass*>(manager.getObjectClass("MPAircraft"));
333 _objectInstance = new HLAMPAircraft(objectClass);
334 if (!_objectInstance.valid())
336 _objectInstance->registerInstance();
337 _objectInstance->setModelPath("Aircraft/ogel/Models/SinglePiston.xml");
339 manager.schedule(*this, getSimTime() + SGTimeStamp::fromSecMSec(0, 1));
342 virtual void update(AIManager& manager, const SGTimeStamp& simTime)
344 SGTimeStamp dt = simTime - getSimTime();
346 setGroundCache(getPhysics(), manager.getPager(), dt);
347 getEnvironment().update(*this, dt);
348 getSubsystemGroup().update(*this, dt);
349 getPhysics().update(*this, dt);
351 AIObject::update(manager, simTime);
353 if (!_objectInstance.valid())
356 _objectInstance->setLocation(getPhysics());
357 _objectInstance->setSimTime(getSimTime().toSecs());
358 _objectInstance->updateAttributeValues(getSimTime(), simgear::RTIData("update"));
360 manager.schedule(*this, getSimTime() + SGTimeStamp::fromSecMSec(0, 100));
363 virtual void shutdown(AIManager& manager)
365 if (_objectInstance.valid())
366 _objectInstance->removeInstance(simgear::RTIData("shutdown"));
368 AIObject::shutdown(manager);
374 double _turnaroundTime;
376 SGSharedPtr<HLAMPAircraft> _objectInstance;
379 /// an ogle in a traffic circuit at lowi
380 class AIOgelInTrafficCircuit : public AIObject {
382 /// Also nothing to really use for a long time, but to demonstrate how it basically works
383 class Physics : public AIPhysics {
385 Physics(const SGLocationd& location, const SGVec3d& linearBodyVelocity = SGVec3d::zeros(),
386 const SGVec3d& angularBodyVelocity = SGVec3d::zeros()) :
387 AIPhysics(location, linearBodyVelocity, angularBodyVelocity),
394 virtual void update(AIObject& object, const SGTimeStamp& dt)
396 SGVec3d down = getHorizontalLocalOrientation().backTransform(SGVec3d(0, 0, 1));
397 SGVec3d distToAimingPoint = getAimingPoint() - getPosition();
398 if (norm(distToAimingPoint - down*dot(down, distToAimingPoint)) <= 10*dt.toSecs()*norm(getLinearVelocity()))
401 SGVec3d aimingVector = normalize(getAimingPoint() - getPosition());
402 SGVec3d bodyAimingVector = getLocation().getOrientation().transform(aimingVector);
404 SGVec3d angularVelocity = 0.2*cross(SGVec3d(1, 0, 0), bodyAimingVector);
405 SGVec3d bodyDownVector = getLocation().getOrientation().transform(down);
406 // keep an upward orientation
407 angularVelocity += cross(SGVec3d(0, 0, 1), SGVec3d(0, bodyDownVector[1], bodyDownVector[2]));
409 SGVec3d linearVelocity(_targetVelocity, 0, 0);
411 SGVec3d gearPosition = getPosition() + _gearOffset*down;
412 SGLineSegmentd lineSegment(gearPosition - 10*down, gearPosition + 10*down);
413 SGVec3d point, normal;
414 if (object.getGroundIntersection(point, normal, lineSegment)) {
415 double agl = dot(down, point - gearPosition);
417 linearVelocity -= down*(0.5*agl/dt.toSecs());
420 advanceByBodyVelocity(dt.toSecs(), linearVelocity, angularVelocity);
423 const SGVec3d& getAimingPoint() const
424 { return _waypoints.front(); }
425 void rotateAimingPoint()
426 { _waypoints.splice(_waypoints.end(), _waypoints, _waypoints.begin()); }
428 std::list<SGVec3d> _waypoints;
429 double _targetVelocity;
433 AIOgelInTrafficCircuit()
435 virtual ~AIOgelInTrafficCircuit()
438 virtual void init(AIManager& manager)
440 AIObject::init(manager);
442 /// Put together somw waypoints
443 /// This needs to be replaced by something generic
444 SGGeod rwyStart = SGGeod::fromDegM(11.35203755, 47.26109606, 574);
445 SGGeod rwyEnd = SGGeod::fromDegM(11.33741688, 47.25951967, 576);
446 SGQuatd hl = SGQuatd::fromLonLat(rwyStart);
447 SGVec3d down = hl.backTransform(SGVec3d(0, 0, 1));
449 SGVec3d cartRwyStart = SGVec3d::fromGeod(rwyStart);
450 SGVec3d cartRwyEnd = SGVec3d::fromGeod(rwyEnd);
452 SGVec3d centerline = normalize(cartRwyEnd - cartRwyStart);
453 SGVec3d left = normalize(cross(centerline, down));
455 SGGeod endClimb = SGGeod::fromGeodM(SGGeod::fromCart(cartRwyEnd + 500*centerline), 700);
456 SGGeod startDescend = SGGeod::fromGeodM(SGGeod::fromCart(cartRwyStart - 500*centerline + 150*left), 650);
458 SGGeod startDownwind = SGGeod::fromGeodM(SGGeod::fromCart(cartRwyEnd + 500*centerline + 800*left), 750);
459 SGGeod endDownwind = SGGeod::fromGeodM(SGGeod::fromCart(cartRwyStart - 500*centerline + 800*left), 750);
461 SGLocationd location(SGVec3d::fromGeod(rwyStart), SGQuatd::fromLonLat(rwyStart)*SGQuatd::fromEulerDeg(-100, 0, 0));
462 Physics* physics = new Physics(location, SGVec3d(0, 0, 0), SGVec3d(0, 0, 0));
463 physics->_waypoints.push_back(SGVec3d::fromGeod(rwyStart));
464 physics->_waypoints.push_back(SGVec3d::fromGeod(rwyEnd));
465 physics->_waypoints.push_back(SGVec3d::fromGeod(endClimb));
466 physics->_waypoints.push_back(SGVec3d::fromGeod(startDownwind));
467 physics->_waypoints.push_back(SGVec3d::fromGeod(endDownwind));
468 physics->_waypoints.push_back(SGVec3d::fromGeod(startDescend));
471 /// Ok, this is part of the official sketch
472 HLAMPAircraftClass* objectClass = dynamic_cast<HLAMPAircraftClass*>(manager.getObjectClass("MPAircraft"));
475 _objectInstance = new HLAMPAircraft(objectClass);
476 if (!_objectInstance.valid())
478 _objectInstance->registerInstance();
479 _objectInstance->setModelPath("Aircraft/ogel/Models/SinglePiston.xml");
481 /// Need to schedule something else we get deleted
482 manager.schedule(*this, getSimTime() + SGTimeStamp::fromSecMSec(0, 100));
485 virtual void update(AIManager& manager, const SGTimeStamp& simTime)
487 SGTimeStamp dt = simTime - getSimTime();
489 setGroundCache(getPhysics(), manager.getPager(), dt);
490 getEnvironment().update(*this, dt);
491 getSubsystemGroup().update(*this, dt);
492 getPhysics().update(*this, dt);
494 AIObject::update(manager, simTime);
496 if (!_objectInstance.valid())
499 _objectInstance->setLocation(getPhysics());
500 _objectInstance->setSimTime(getSimTime().toSecs());
501 _objectInstance->updateAttributeValues(getSimTime(), simgear::RTIData("update"));
503 /// Need to schedule something else we get deleted
504 manager.schedule(*this, getSimTime() + SGTimeStamp::fromSecMSec(0, 100));
507 virtual void shutdown(AIManager& manager)
509 if (_objectInstance.valid())
510 _objectInstance->removeInstance(simgear::RTIData("shutdown"));
513 AIObject::shutdown(manager);
517 SGSharedPtr<HLAMPAircraft> _objectInstance;
526 main(int argc, char* argv[])
528 SGSharedPtr<fgai::AIManager> manager = new fgai::AIManager;
530 /// FIXME include some argument parsing stuff
532 std::string fg_scenery;
535 while ((c = getopt(argc, argv, "cCf:F:n:O:p:RsS")) != EOF) {
538 manager->setCreateFederationExecution(true);
541 manager->setTimeConstrained(true);
544 manager->setFederateType(optarg);
547 manager->setFederationExecutionName(optarg);
550 manager->setFederationObjectModel(optarg);
553 sglog().set_log_classes(SG_ALL);
554 sglog().set_log_priority(sgDebugPriority(atoi(optarg)));
557 manager->setTimeRegulating(true);
560 manager->setTimeConstrainedByLocalClock(false);
563 manager->setTimeConstrainedByLocalClock(true);
574 if (fg_root.empty()) {
575 if (const char *fg_root_env = std::getenv("FG_ROOT")) {
576 fg_root = fg_root_env;
581 if (fg_scenery.empty()) {
582 if (const char *fg_scenery_env = std::getenv("FG_SCENERY")) {
583 fg_scenery = fg_scenery_env;
584 } else if (!fg_root.empty()) {
585 SGPath path(fg_root);
586 path.append("Scenery");
587 fg_scenery = path.str();
591 manager->getPager().setScenery(fg_root, fg_scenery);
593 if (manager->getFederationObjectModel().empty()) {
594 SGPath path(fg_root);
596 path.append("fg-local-fom.xml");
597 manager->setFederationObjectModel(path.str());
601 manager->insert(new fgai::AIOgel(SGGeod::fromDegFt(9.19298, 48.6895, 2000)));
603 manager->insert(new fgai::AIOgel(SGGeod::fromDegFt(11.344, 47.260, 2500)));
604 /// LOWI traffic circuit
605 manager->insert(new fgai::AIOgelInTrafficCircuit);
607 return manager->exec();