From 65ef6aa6383db3a48462879a44c0977165b9b11b Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Thu, 8 Oct 2009 21:32:06 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@199 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- sources/reactphysics3d/integration/Euler.cpp | 53 +++++++++++ sources/reactphysics3d/integration/Euler.h | 53 +++++++++++ .../integration/IntegrationAlgorithm.cpp | 34 +++++++ .../integration/IntegrationAlgorithm.h | 42 +++++++++ .../integration/RungeKutta4.cpp | 91 +++++++++++++++++++ .../reactphysics3d/integration/RungeKutta4.h | 54 +++++++++++ 6 files changed, 327 insertions(+) create mode 100644 sources/reactphysics3d/integration/Euler.cpp create mode 100644 sources/reactphysics3d/integration/Euler.h create mode 100644 sources/reactphysics3d/integration/IntegrationAlgorithm.cpp create mode 100644 sources/reactphysics3d/integration/IntegrationAlgorithm.h create mode 100644 sources/reactphysics3d/integration/RungeKutta4.cpp create mode 100644 sources/reactphysics3d/integration/RungeKutta4.h diff --git a/sources/reactphysics3d/integration/Euler.cpp b/sources/reactphysics3d/integration/Euler.cpp new file mode 100644 index 00000000..38614711 --- /dev/null +++ b/sources/reactphysics3d/integration/Euler.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D 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 Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "Euler.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Euler::Euler() { + +} + +// Copy-constructor +Euler::Euler(const Euler& euler) { + +} + +// Destructor +Euler::~Euler() { + +} + +// Integrate a body state over time. This method use the Euler integration algorithm +void Euler::integrate(BodyState& bodyState, const Time& time, const Time& timeStep) { + double dt = timeStep.getValue(); // Timestep + + // Compute the integrated body state + bodyState.setPosition(bodyState.getPosition() + bodyState.getLinearVelocity() * dt); + bodyState.setLinearMomentum(bodyState.getLinearMomentum() + bodyState.computeForce(time) * dt); + bodyState.setOrientation(bodyState.getOrientation() + bodyState.getSpin() * dt); + bodyState.setAngularMomentum(bodyState.getAngularMomentum() + bodyState.computeTorque(time) * dt); + + // Recalculate the secondary values of the body state + bodyState.recalculate(); +} diff --git a/sources/reactphysics3d/integration/Euler.h b/sources/reactphysics3d/integration/Euler.h new file mode 100644 index 00000000..71edade4 --- /dev/null +++ b/sources/reactphysics3d/integration/Euler.h @@ -0,0 +1,53 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D 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 Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +#ifndef EULER_H +#define EULER_H + +// Libraries +#include "IntegrationAlgorithm.h" +#include "../body/BodyState.h" +#include "../body/DerivativeBodyState.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Euler : + This class will be used to solve the differential equation of + movement by integrating a body state. This class implements + the Euler algorithm. It's important to undersand that Euler + algorithm should be used only for testing purpose because the + Euler algorithm is not a good one. + ------------------------------------------------------------------- +*/ +class Euler : public IntegrationAlgorithm { + private : + + public : + Euler(); // Constructor + Euler(const Euler& euler); // Copy-constructor + virtual ~Euler(); // Destructor + + void integrate(BodyState& bodyState, const Time& t, const Time& dt); // Integrate a body state over time +}; + +} + +#endif diff --git a/sources/reactphysics3d/integration/IntegrationAlgorithm.cpp b/sources/reactphysics3d/integration/IntegrationAlgorithm.cpp new file mode 100644 index 00000000..9490229b --- /dev/null +++ b/sources/reactphysics3d/integration/IntegrationAlgorithm.cpp @@ -0,0 +1,34 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D 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 Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +// Libraries +#include "IntegrationAlgorithm.h" + +// Namespace +using namespace reactphysics3d; + +// Constructor +IntegrationAlgorithm::IntegrationAlgorithm() { + +} + +// Destructor +IntegrationAlgorithm::~IntegrationAlgorithm() { + +} diff --git a/sources/reactphysics3d/integration/IntegrationAlgorithm.h b/sources/reactphysics3d/integration/IntegrationAlgorithm.h new file mode 100644 index 00000000..669fec5c --- /dev/null +++ b/sources/reactphysics3d/integration/IntegrationAlgorithm.h @@ -0,0 +1,42 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D 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 Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +#ifndef INTEGRATIONALGORITHM_H +#define INTEGRATIONALGORITHM_H + +// Libraries +#include "../body/BodyState.h" +#include "../physics/physics.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +class IntegrationAlgorithm { + private : + + public : + IntegrationAlgorithm(); // Constructor + virtual ~IntegrationAlgorithm(); // Destructor + + virtual void integrate(BodyState& bodyState, const Time& t, const Time& dt)=0; // Integrate a body state over time +}; + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/sources/reactphysics3d/integration/RungeKutta4.cpp b/sources/reactphysics3d/integration/RungeKutta4.cpp new file mode 100644 index 00000000..cb1602d3 --- /dev/null +++ b/sources/reactphysics3d/integration/RungeKutta4.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * Copyright (C) 2009 Daniel Chappuis * + **************************************************************************** + * This file is part of ReactPhysics3D. * + * * + * ReactPhysics3D is free software: you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License as published * + * by the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * ReactPhysics3D 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 Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with ReactPhysics3D. If not, see . * + ***************************************************************************/ + +// Libraries +#include "RK4.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +RK4::RK4() { + +} + +// Copy-constructor +RK4::RK4(const RK4& rk4) { + +} + +// Destructor +RK4::~RK4() { + +} + +// Compute a derivative body state at time t +DerivativeBodyState RK4::evaluate(const BodyState& bodyState, const Time& time) { + + // Compute the derivaties values at time t + Vector3D linearVelocity = bodyState.getLinearVelocity(); + Vector3D force = bodyState.computeForce(time); + Vector3D torque = bodyState.computeTorque(time); + Quaternion spin = bodyState.getSpin(); + + // Return the derivative body state at time t + return DerivativeBodyState(linearVelocity, force, torque, spin); +} + +// Compute a derivative body state at time t + dt according to the last derivative body state +DerivativeBodyState RK4::evaluate(BodyState bodyState, const Time& time, const Time& timeStep, + const DerivativeBodyState& lastDerivativeBodyState) { + // Compute the bodyState at time t + dt + bodyState.computeAtTime(timeStep, lastDerivativeBodyState); + + // Compute the derivaties values at time t + Vector3D linearVelocity = bodyState.getLinearVelocity(); + Vector3D force = bodyState.computeForce(time + timeStep); + Vector3D torque = bodyState.computeTorque(time + timeStep); + Quaternion spin = bodyState.getSpin(); + + // Return the derivative body state at time t + return DerivativeBodyState(linearVelocity, force, torque, spin); +} + +// Integrate a body state over time. This method use the RK4 integration algorithm +void RK4::integrate(BodyState& bodyState, const Time& time, const Time& timeStep) { + + // Compute the 4 derivatives body states at different time values. + DerivativeBodyState a = evaluate(bodyState, time); + DerivativeBodyState b = evaluate(bodyState, time, timeStep*0.5, a); + DerivativeBodyState c = evaluate(bodyState, time, timeStep*0.5, b); + DerivativeBodyState d = evaluate(bodyState, time, timeStep, c); + + double dt = timeStep.getValue(); // Timestep + + // Compute the integrated body state + bodyState.setPosition(bodyState.getPosition() + (a.getLinearVelocity() + (b.getLinearVelocity() + c.getLinearVelocity()) * 2.0 + + d.getLinearVelocity()) * (1.0/6.0) * dt); + bodyState.setLinearMomentum(bodyState.getLinearMomentum() + (a.getForce() + (b.getForce() + c.getForce())*2.0 + d.getForce()) * (1.0/6.0) * dt); + bodyState.setOrientation(bodyState.getOrientation() + (a.getSpin() + (b.getSpin() + c.getSpin())*2.0 + d.getSpin()) * (1.0/6.0) * dt); + bodyState.setAngularMomentum(bodyState.getAngularMomentum() + (a.getTorque() + (b.getTorque() + c.getTorque())*2.0 + d.getTorque()) * (1.0/6.0) * dt); + + // Recalculate the secondary values of the body state + bodyState.recalculate(); +} diff --git a/sources/reactphysics3d/integration/RungeKutta4.h b/sources/reactphysics3d/integration/RungeKutta4.h new file mode 100644 index 00000000..49bf922e --- /dev/null +++ b/sources/reactphysics3d/integration/RungeKutta4.h @@ -0,0 +1,54 @@ +/**************************************************************************** +* Copyright (C) 2009 Daniel Chappuis * +**************************************************************************** +* This file is part of ReactPhysics3D. * +* * +* ReactPhysics3D is free software: you can redistribute it and/or modify * +* it under the terms of the GNU Lesser General Public License as published * +* by the Free Software Foundation, either version 3 of the License, or * +* (at your option) any later version. * +* * +* ReactPhysics3D 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 Lesser General Public License for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with ReactPhysics3D. If not, see . * +***************************************************************************/ + +#ifndef RUNGEKUTTA4_H +#define RUNGEKUTTA4_H + +// Libraries +#include "IntegrationAlgorithm.h" +#include "../body/BodyState.h" +#include "../body/DerivativeBodyState.h" + +// Namespace ReactPhysics3D +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class RungeKutta4 : + This class will be used to solve the differential equation of + movement by integrating a body state. This class implements + the Runge-Kutta 4 (RK4) integrator. + ------------------------------------------------------------------- +*/ +class RungeKutta4 : public IntegrationAlgorithm { + private : + DerivativeBodyState evaluate(const BodyState& bodyState, const Time& time); // Compute a derivative body state + DerivativeBodyState evaluate(BodyState bodyState, const Time& time, + const Time& timeStep, const DerivativeBodyState& lastDerivativeBodyState); // Compute a derivative body state + + public : + RungeKutta4(); // Constructor + RungeKutta4(const RungeKutta4& rk4); // Copy-constructor + virtual ~RungeKutta4(); // Destructor + + void integrate(BodyState& bodyState, const Time& t, const Time& dt); // Integrate a body state over time +}; + +} + +#endif