X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FRigidBody.hh;h=ad6e33ff67c2135115dfe0b2700bdb863bc99686;hp=501f7c9bdbc07279feda2334c995bf3556c1e47b;hb=00612586426be6d9a976f141a25ffc1f0d284501;hpb=25aefe01ef7dbdb603c51411e04b0d6a6107684f diff --git a/src/Moof/RigidBody.hh b/src/Moof/RigidBody.hh index 501f7c9..ad6e33f 100644 --- a/src/Moof/RigidBody.hh +++ b/src/Moof/RigidBody.hh @@ -1,72 +1,316 @@ -/******************************************************************************* - - Copyright (c) 2009, Charles McGarvey - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*******************************************************************************/ +/*] Copyright (c) 2009-2010, Charles McGarvey [************************** +**] All rights reserved. +* +* vi:ts=4 sw=4 tw=75 +* +* Distributable under the terms and conditions of the 2-clause BSD license; +* see the file COPYING for a complete text of the license. +* +**************************************************************************/ #ifndef _MOOF_RIGIDBODY_HH_ #define _MOOF_RIGIDBODY_HH_ +#include + +#include +#include + +#include #include -#include namespace Mf { +template +struct LinearState +{ + typedef cml::vector< Scalar, cml::fixed > Vector; + typedef boost::function + ForceFunction; + + // primary + + Vector position; + Vector momentum; + + // secondary + + Vector velocity; + + // user + + Vector force; + std::vector forces; + + // constant + + Scalar mass; + Scalar inverseMass; + + + void recalculateLinear() + { + velocity = momentum * inverseMass; + } + + + struct GravityForce + { + explicit GravityForce(Scalar a = -9.8) + { + force.zero(); + acceleration = a; + } + + const Vector& operator () (const LinearState& state) + { + force[1] = state.mass * acceleration; + return force; + } + + private: + + Vector force; + Scalar acceleration; + }; + + + void init() + { + position.zero(); + momentum.zero(); + + velocity.zero(); + + force.zero(); + forces.clear(); + + mass = SCALAR(1.0); + inverseMass = 1.0 / mass; + } + + + struct Derivative + { + Vector velocity; + Vector force; + + Derivative operator*(Scalar dt) const + { + Derivative derivative; + derivative.velocity = dt * velocity; + derivative.force = dt * force; + return derivative; + } + + Derivative operator+(const Derivative& other) const + { + Derivative derivative; + derivative.velocity = velocity + other.velocity; + derivative.force = force + other.force; + return derivative; + } + }; + + + Vector getForce() const + { + Vector f(force); + + for (size_t i = 0; i < forces.size(); ++i) + { + f += forces[i](*this); + } + + return f; + } + + void getDerivative(Derivative& derivative, Scalar t) const + { + derivative.velocity = velocity; + derivative.force = getForce(); + } + + void step(const Derivative& derivative, Scalar dt) + { + position += dt * derivative.velocity; + momentum += dt * derivative.force; + recalculateLinear(); + } +}; + + +struct RotationalState2 +{ + // primary + + Scalar orientation; + Scalar angularMomentum; + + // secondary + + Scalar angularVelocity; + + // constant + + Scalar inertia; + Scalar inverseInertia; + + + void recalculateRotational() + { + angularVelocity = angularMomentum * inertia; + } + + + struct Derivative + { + Scalar angularVelocity; + Scalar torque; + }; + + void step(const Derivative& derivative, Scalar dt) + { + orientation += dt * derivative.angularVelocity; + angularMomentum += dt * derivative.torque; + recalculateRotational(); + } +}; + +struct RotationalState3 +{ + // primary + + Quaternion orientation; + Vector3 angularMomentum; + + // secondary + + Quaternion spin; + Vector3 angularVelocity; + + // constant + + Scalar inertia; + Scalar inverseInertia; + + + void recalculateRotational() + { + angularVelocity = angularMomentum * inertia; + } +}; + + +struct State2 : public LinearState<2>, public RotationalState2 +{ + void recalculate() + { + recalculateLinear(); + recalculateRotational(); + } + + void update(Scalar t, Scalar dt) + { + rk4,LinearState<2>::Derivative>(*this, t, dt); + } +}; + +struct State3 : public LinearState<3>, public RotationalState3 +{ + void recalculate() + { + recalculateLinear(); + recalculateRotational(); + } + + void update(Scalar t, Scalar dt) + { + rk4,LinearState<3>::Derivative>(*this, t, dt); + } +}; + + +template +inline T interpolate(const T& a, const T& b, Scalar alpha) +{ + return cml::lerp(a, b, alpha); +} + +template <> +inline State2 interpolate(const State2& a, const State2& b, + Scalar alpha) +{ + State2 state(b); + state.position = interpolate(a.position, b.position, alpha); + state.momentum = interpolate(a.momentum, b.momentum, alpha); + state.orientation = interpolate(a.orientation, b.orientation, alpha); + state.angularMomentum = interpolate(a.angularMomentum, + b.angularMomentum, alpha); + return state; +} + +template <> +inline State3 interpolate(const State3& a, const State3& b, + Scalar alpha) +{ + State3 state(b); + state.position = interpolate(a.position, b.position, alpha); + state.momentum = interpolate(a.momentum, b.momentum, alpha); + state.orientation = cml::slerp(a.orientation, b.orientation, alpha); + state.angularMomentum = interpolate(a.angularMomentum, + b.angularMomentum, alpha); + return state; +} + + + /** - * Interface for physical things with mass, momentum, yada yada yada. + * Interface for anything that can move. */ -template -class RigidBody +template +class RigidBody : public Entity { +protected: + + T mState; + T mPrevState; + public: + virtual ~RigidBody() {} virtual void update(Scalar t, Scalar dt) { - prevState_ = currentState_; - currentState_.integrate(t, dt); + mPrevState = mState; + mState.update(t, dt); } - T getInterpolatedState(Scalar alpha) const + const T& getState() const { - return currentState_.interpolate(alpha, prevState_); + return mState; } -protected: - T prevState_; - T currentState_; + T getState(Scalar alpha) const + { + return interpolate(mPrevState, mState, alpha); + } + + const T& getLastState() const + { + return mPrevState; + } }; +typedef RigidBody RigidBody2; +typedef RigidBody RigidBody3; + } // namespace Mf #endif // _MOOF_RIGIDBODY_HH_ -/** vim: set ts=4 sw=4 tw=80: *************************************************/ - -