X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FMath.hh;h=d557c7f15114d7a07e15d393cbd66728bfba1f76;hp=4e1cda8c1a25f3a4f57cffe616a56564040f45f6;hb=2f239b9ba2a556a5ca810cfffc60552a56a4fe86;hpb=493ddb59a8620b49dfa0ff62ce93395ebfd02e86 diff --git a/src/Moof/Math.hh b/src/Moof/Math.hh index 4e1cda8..d557c7f 100644 --- a/src/Moof/Math.hh +++ b/src/Moof/Math.hh @@ -1,30 +1,13 @@ -/******************************************************************************* - - 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_MATH_HH_ #define _MOOF_MATH_HH_ @@ -37,7 +20,26 @@ #include #include -#include // GLscalar +#include + +#if HAVE_CONFIG_H +#include "config.h" +#endif + + +#if USE_DOUBLE_PRECISION + +typedef GLdouble GLscalar; +#define GL_SCALAR GL_DOUBLE +#define SCALAR(D) (D) + +#else + +typedef GLfloat GLscalar; +#define GL_SCALAR GL_FLOAT +#define SCALAR(F) (F##f) + +#endif namespace Mf { @@ -49,6 +51,8 @@ typedef cml::vector< Scalar, cml::fixed<2> > Vector2; typedef cml::vector< Scalar, cml::fixed<3> > Vector3; typedef cml::vector< Scalar, cml::fixed<4> > Vector4; +typedef cml::matrix< Scalar, cml::fixed<2,2>, + cml::col_basis, cml::col_major > Matrix2; typedef cml::matrix< Scalar, cml::fixed<3,3>, cml::col_basis, cml::col_major > Matrix3; typedef cml::matrix< Scalar, cml::fixed<4,4>, @@ -60,40 +64,141 @@ typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first, typedef cml::constants Constants; -inline Vector3& demoteVector(Vector3& left, const Vector4& right) +inline Vector3 demote(const Vector4& vec) +{ + return Vector3(vec[0], vec[1], vec[2]); +} + +inline Vector2 demote(const Vector3& vec) +{ + return Vector2(vec[0], vec[1]); +} + +inline Vector4 promote(const Vector3& vec, Scalar extra = 0.0) +{ + return Vector4(vec[0], vec[1], vec[2], extra); +} + +inline Vector3 promote(const Vector2& vec, Scalar extra = 0.0) +{ + return Vector3(vec[0], vec[1], extra); +} + + +template +inline R convert(const P& p) +{ + return R(p); +} + +template <> +inline Vector3 convert(const Vector4& vec) +{ + return Vector3(vec[0], vec[1], vec[2]); +} + +template <> +inline Vector2 convert(const Vector3& vec) +{ + return Vector2(vec[0], vec[1]); +} + +template <> +inline Vector4 convert(const Vector3& vec) { - left[0] = right[0]; - left[1] = right[1]; - left[2] = right[2]; - return left; + return Vector4(vec[0], vec[1], vec[2], SCALAR(0.0)); } -inline Vector4& promoteVector(Vector4& left, const Vector3& right) +template <> +inline Vector3 convert(const Vector2& vec) { - left[0] = right[0]; - left[1] = right[1]; - left[2] = right[2]; - left[3] = 1.0; - return left; + return Vector3(vec[0], vec[1], SCALAR(0.0)); } +template +struct cast +{ + cast(const P& p) : param(p) {} + template + operator R() { return convert(param); } +private: + const P& param; +}; + -const Scalar EPSILON = 0.000001; +const Scalar EPSILON = SCALAR(0.000001); /** * Check the equality of scalars with a certain degree of error allowed. */ -inline bool checkEquality(Scalar a, Scalar b, Scalar epsilon = EPSILON) +inline bool isEqual(Scalar a, Scalar b, Scalar epsilon = EPSILON) { return std::abs(a - b) < epsilon; } + +// Here are some generic implementations of a few simple integrators. To +// use, you need one type representing the state and another containing the +// derivatives of the primary state variables. The state class must +// implement these methods: +// +// void getDerivative(Derivative_Type& derivative, Scalar absoluteTime); +// void step(const Derivative_Type& derivative, Scalar deltaTime); +// +// Additionally, the derivative class must overload a few operators: +// +// Derivative_Type operator+(const Derivative_Type& other) const +// Derivative_Type operator*(const Derivative_Type& other) const + +template +inline D evaluate(const S& state, Scalar t) +{ + D derivative; + state.getDerivative(derivative, t); + return derivative; +} + +template +inline D evaluate(S state, Scalar t, Scalar dt, const D& derivative) +{ + state.step(derivative, dt); + return evaluate(state, t + dt); +} + + +template +inline void euler(S& state, Scalar t, Scalar dt) +{ + D a = evaluate(state, t); + + state.step(a, dt); +} + +template +inline void rk2(S& state, Scalar t, Scalar dt) +{ + D a = evaluate(state, t); + D b = evaluate(state, t, dt * SCALAR(0.5), a); + + state.step(b, dt); +} + +template +inline void rk4(S& state, Scalar t, Scalar dt) +{ + D a = evaluate(state, t); + D b = evaluate(state, t, dt * SCALAR(0.5), a); + D c = evaluate(state, t, dt * SCALAR(0.5), b); + D d = evaluate(state, t, dt, c); + + state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt); +} + + } // namespace Mf #endif // _MOOF_MATH_HH_ -/** vim: set ts=4 sw=4 tw=80: *************************************************/ -