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
AIPhysics(const AIPhysics & physics)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
AIPhysics(const SGLocationd & location,const SGVec3d & linearBodyVelocity,const SGVec3d & angularBodyVelocity)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
~AIPhysics()47 AIPhysics::~AIPhysics()
48 {
49 }
50
51 void
update(AIObject & object,const SGTimeStamp & dt)52 AIPhysics::update(AIObject& object, const SGTimeStamp& dt)
53 {
54 advanceByBodyVelocity(dt.toSecs(), _linearBodyVelocity, _angularBodyVelocity);
55 }
56
57 void
advanceByBodyAcceleration(const double & dt,const SGVec3d & linearAcceleration,const SGVec3d & angularAcceleration)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
advanceByBodyVelocity(const double & dt,const SGVec3d & linearVelocity,const SGVec3d & angularVelocity)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
advanceToLocation(const double & dt,const SGLocationd & location)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
105