first commit

2022-10-20 20:29:11 +08:00
commit 4d531f8044
3238 changed files with 1387862 additions and 0 deletions
@@ -0,0 +1,104 @@
+// Copyright (C) 2009 - 2012  Mathias Froehlich
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "AIPhysics.hxx"
+
+#include <simgear/math/SGGeometry.hxx>
+#include "AIEnvironment.hxx"
+
+namespace fgai {
+
+AIPhysics::AIPhysics(const AIPhysics& physics) :
+    _location(physics._location),
+    _linearBodyVelocity(physics._linearBodyVelocity),
+    _angularBodyVelocity(physics._angularBodyVelocity),
+    _geodPosition(physics._geodPosition),
+    _horizontalLocalOrientation(physics._horizontalLocalOrientation)
+{
+}
+
+AIPhysics::AIPhysics(const SGLocationd& location, const SGVec3d& linearBodyVelocity,
+                           const SGVec3d& angularBodyVelocity) :
+    _location(location),
+    _linearBodyVelocity(linearBodyVelocity),
+    _angularBodyVelocity(angularBodyVelocity)
+{
+    _geodPosition = SGGeod::fromCart(_location.getPosition());
+    _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
+}
+
+AIPhysics::~AIPhysics()
+{
+}
+
+void
+AIPhysics::update(AIObject& object, const SGTimeStamp& dt)
+{
+    advanceByBodyVelocity(dt.toSecs(), _linearBodyVelocity, _angularBodyVelocity);
+}
+
+void
+AIPhysics::advanceByBodyAcceleration(const double& dt,
+                                        const SGVec3d& linearAcceleration,
+                                        const SGVec3d& angularAcceleration)
+{
+    // The current linear and angular velocity
+    SGVec3d linearVelocity = getLinearBodyVelocity();
+    SGVec3d angularVelocity = getAngularBodyVelocity();
+    
+    // an euler step for the velocities, the positions get upgraded below
+    linearVelocity += dt*linearAcceleration;
+    angularVelocity += dt*angularAcceleration;
+    
+    advanceByBodyVelocity(dt, linearVelocity, angularVelocity);
+}
+
+void
+AIPhysics::advanceByBodyVelocity(const double& dt,
+                                    const SGVec3d& linearVelocity,
+                                    const SGVec3d& angularVelocity)
+{
+    // Do an euler step with the derivatives at mSimTime
+    _location.eulerStepBodyVelocities(dt, _linearBodyVelocity, _angularBodyVelocity);
+    _geodPosition = SGGeod::fromCart(_location.getPosition());
+    _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
+    
+    // Store the new velocities for the next interval at mSimTim + dt
+    _linearBodyVelocity = linearVelocity;
+    _angularBodyVelocity = angularVelocity;
+}
+
+void
+AIPhysics::advanceToLocation(const double& dt, const SGLocationd& location)
+{
+    // At first we need to move along with the announced velocities, so:
+    // Do an euler step with the derivatives at mSimTime.
+    _location.eulerStepBodyVelocities(dt, _linearBodyVelocity, _angularBodyVelocity);
+    _geodPosition = SGGeod::fromCart(_location.getPosition());
+    _horizontalLocalOrientation = SGQuatd::fromLonLat(_geodPosition);
+    
+    // Now compute velocities that will move to the desired position, orientation if the next
+    // advance method is called with the same dt value
+    SGVec3d positionDifference = location.getPosition() - _location.getPosition();
+    _linearBodyVelocity = (1/dt)*_location.getOrientation().transform(positionDifference);
+    _angularBodyVelocity = SGQuatd::forwardDifferenceVelocity(_location.getOrientation(), location.getOrientation(), dt);
+}
+
+} // namespace fgai