utils/fgai/AIPhysics.cxx

   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 #ifdef HAVE_CONFIG_H
  18 #include <config.h>
  19 #endif
  20
  21 #include "AIPhysics.hxx"
  22
  23 #include <simgear/math/SGGeometry.hxx>
  24 #include "AIEnvironment.hxx"
  25
  26 namespace fgai {
  27
  28 AIPhysics::AIPhysics(const AIPhysics& physics) :
  29     _location(physics._location),
  30     _linearBodyVelocity(physics._linearBodyVelocity),
  31     _angularBodyVelocity(physics._angularBodyVelocity),
  32     _geodPosition(physics._geodPosition),
  33     _horizontalLocalOrientation(physics._horizontalLocalOrientation)
  34 {
  35 }
  36
  37 AIPhysics::AIPhysics(const SGLocationd& location, const SGVec3d& linearBodyVelocity,
  38                            const SGVec3d& angularBodyVelocity) :
  39     _location(location),
  40     _linearBodyVelocity(linearBodyVelocity),
  41     _angularBodyVelocity(angularBodyVelocity)
  42 {
  43     _geodPosition = SGGeod::fromCart(_location.getPosition());
  44     _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
  45 }
  46
  47 AIPhysics::~AIPhysics()
  48 {
  49 }
  50
  51 void
  52 AIPhysics::update(AIObject& object, const SGTimeStamp& dt)
  53 {
  54     advanceByBodyVelocity(dt.toSecs(), _linearBodyVelocity, _angularBodyVelocity);
  55 }
  56
  57 void
  58 AIPhysics::advanceByBodyAcceleration(const double& dt,
  59                                         const SGVec3d& linearAcceleration,
  60                                         const SGVec3d& angularAcceleration)
  61 {
  62     // The current linear and angular velocity
  63     SGVec3d linearVelocity = getLinearBodyVelocity();
  64     SGVec3d angularVelocity = getAngularBodyVelocity();
  65
  66     // an euler step for the velocities, the positions get upgraded below
  67     linearVelocity += dt*linearAcceleration;
  68     angularVelocity += dt*angularAcceleration;
  69
  70     advanceByBodyVelocity(dt, linearVelocity, angularVelocity);
  71 }
  72
  73 void
  74 AIPhysics::advanceByBodyVelocity(const double& dt,
  75                                     const SGVec3d& linearVelocity,
  76                                     const SGVec3d& angularVelocity)
  77 {
  78     // Do an euler step with the derivatives at mSimTime
  79     _location.eulerStepBodyVelocities(dt, _linearBodyVelocity, _angularBodyVelocity);
  80     _geodPosition = SGGeod::fromCart(_location.getPosition());
  81     _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
  82
  83     // Store the new velocities for the next interval at mSimTim + dt
  84     _linearBodyVelocity = linearVelocity;
  85     _angularBodyVelocity = angularVelocity;
  86 }
  87
  88 void
  89 AIPhysics::advanceToLocation(const double& dt, const SGLocationd& location)
  90 {
  91     // At first we need to move along with the announced velocities, so:
  92     // Do an euler step with the derivatives at mSimTime.
  93     _location.eulerStepBodyVelocities(dt, _linearBodyVelocity, _angularBodyVelocity);
  94     _geodPosition = SGGeod::fromCart(_location.getPosition());
  95     _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
  96
  97     // Now compute velocities that will move to the desired position, orientation if the next
  98     // advance method is called with the same dt value
  99     SGVec3d positionDifference = location.getPosition() - _location.getPosition();
 100     _linearBodyVelocity = (1/dt)*_location.getOrientation().transform(positionDifference);
 101     _angularBodyVelocity = SGQuatd::forwardDifferenceVelocity(_location.getOrientation(), location.getOrientation(), dt);
 102 }
 103
 104 } // namespace fgai