utils/fgai/AIPhysics.hxx

   1 // Copyright (C) 2009 - 2012  Mathias Froehlich
   2 //
   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.
   7 //
   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.
  12 //
  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.
  16
  17 #ifndef AIPhysics_hxx
  18 #define AIPhysics_hxx
  19
  20 #include <simgear/math/SGMath.hxx>
  21 #include "AISubsystem.hxx"
  22
  23 namespace fgai {
  24
  25 class AIPhysics : public AISubsystem {
  26 public:
  27     /// Initial conditions need to be set at creation time.
  28     /// Just setting the position underway will result in unphysical motion.
  29     AIPhysics(const AIPhysics& physics);
  30     AIPhysics(const SGLocationd& location, const SGVec3d& linearBodyVelocity = SGVec3d::zeros(),
  31                  const SGVec3d& angularBodyVelocity = SGVec3d::zeros());
  32     virtual ~AIPhysics();
  33
  34     /// The default is unaccelerated movement
  35     virtual void update(AIObject& object, const SGTimeStamp& dt);
  36
  37     /// The current state
  38     const SGLocationd& getLocation() const
  39     { return _location; }
  40     const SGVec3d& getPosition() const
  41     { return _location.getPosition(); }
  42     const SGQuatd& getOrientation() const
  43     { return _location.getOrientation(); }
  44     const SGGeod& getGeodPosition() const
  45     { return _geodPosition; }
  46     const SGQuatd& getHorizontalLocalOrientation() const
  47     { return _horizontalLocalOrientation; }
  48     /// The orientation of the body wrt the geodetic ned coordinate system
  49     SGQuatd getGeodOrientation() const
  50     { return inverse(_horizontalLocalOrientation)*_location.getOrientation(); }
  51     const SGVec3d& getLinearBodyVelocity() const
  52     { return _linearBodyVelocity; }
  53     const SGVec3d& getAngularBodyVelocity() const
  54     { return _angularBodyVelocity; }
  55     /// The velocity in global cartesian coordinates
  56     SGVec3d getLinearVelocity() const
  57     { return _location.getOrientation().backTransform(_linearBodyVelocity); }
  58     /// The velocity in the north east down coordinate system
  59     /// Note that this gets undefined at the poles.
  60     SGVec3d getGeodVelocity() const
  61     { return getGeodOrientation().backTransform(getLinearBodyVelocity()); }
  62     double getDownVelocity() const
  63     { return getGeodVelocity()[2]; }
  64     SGVec2d getHorizontalVelocity() const
  65     { SGVec3d v = getGeodVelocity(); return SGVec2d(v[0], v[1]); }
  66
  67 protected:
  68     /// The below methods change the position and velocity of the vehicle
  69     /// in a way that is consistent in that it matches position, velocity
  70     /// values to keep the velocity a numerical derivative of the position.
  71
  72     /// Given the accelerations at the current simulation time mSimTime,
  73     /// update the position and velocity to mSimTime + dt.
  74     /// This is the primary advance mode for physically simulated motion.
  75     /// Compute the forces on the single body, compute the accelerations
  76     /// from the forces by newtons law and accelerate by this amount.
  77     void advanceByBodyAcceleration(const double& dt,
  78                                    const SGVec3d& linearAcceleration,
  79                                    const SGVec3d& angularAcceleration);
  80
  81     /// Given the velocities at the next simulation time mSimTime,
  82     /// update the position and velocity to mSimTime + dt.
  83     void advanceByBodyVelocity(const double& dt,
  84                                const SGVec3d& linearVelocity,
  85                                const SGVec3d& angularVelocity);
  86
  87     /// Given the desired position and orientation, a pair of velocities
  88     /// is computed to reach that position and orientation in the
  89     /// next advance step. This one advance step latency is important
  90     /// for other participants to correctly extrapolate the position
  91     /// and orientation based on the velocities.
  92     /// Note that this only works when the next update is called with
  93     /// the same time increment.
  94     void advanceToLocation(const double& dt, const SGLocationd& location);
  95
  96 private:
  97     AIPhysics& operator=(const AIPhysics& physics);
  98
  99     SGLocationd _location;
 100     SGVec3d _linearBodyVelocity;
 101     SGVec3d _angularBodyVelocity;
 102     SGGeod _geodPosition;
 103     SGQuatd _horizontalLocalOrientation;
 104 };
 105
 106 } // namespace fgai
 107
 108 #endif