minor refactoring and state progress

[chaz/yoink] / src / Moof / Math.hh

diff --git a/src/Moof/Math.hh b/src/Moof/Math.hh
index 563746e380e5b38fc347f856b3cebd6b3b63c2bd..c63cfe7db1bddace21e4a4fa8e8b04a460122a19 100644 (file)
--- a/src/Moof/Math.hh
+++ b/src/Moof/Math.hh
@@ -37,38 +37,124 @@
 #include <cmath>
 #include <cml/cml.h>
 
+#include <Moof/OpenGL.hh>              // GLscalar
+
 
 namespace Mf {
 
 
-// Basic types.
+typedef GLscalar                                                               Scalar;
+
+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>,
+               cml::col_basis, cml::col_major >                Matrix4;
+
+typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first,
+               cml::positive_cross >                                   Quaternion;
+
+typedef cml::constants<Scalar>                                 Constants;
 
-typedef float                          Scalar;                         ///< Scalar type.
 
-typedef cml::vector2f          Vector2;
-typedef cml::vector3f          Vector3;
-typedef cml::vector4f          Vector4;
+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]);
+}
 
-typedef cml::matrix33f_c       Matrix3;
-typedef cml::matrix44f_c       Matrix4;
+inline Vector4 promote(const Vector3& vec, Scalar extra = 1.0)
+{
+       return Vector4(vec[0], vec[1], vec[2], extra);
+}
 
-typedef cml::quaternionf_p     Quaternion;
+inline Vector3 promote(const Vector2& vec, Scalar extra = 1.0)
+{
+       return Vector3(vec[0], vec[1], extra);
+}
 
-typedef Vector4                                Color;
 
 
-const Scalar EPSILON = 0.000001f;
+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<typename S, typename D>
+inline D evaluate(const S& state, Scalar t)
+{
+       D derivative;
+       state.getDerivative(derivative, t);
+       return derivative;
+}
+
+template<typename S, typename D>
+inline D evaluate(S state,  Scalar t, Scalar dt, const D& derivative)
+{
+       state.step(derivative, dt);
+       return evaluate<S,D>(state, t + dt);
+}
+
+
+template<typename S, typename D>
+inline void euler(S& state, Scalar t, Scalar dt)
+{
+       D a = evaluate<S,D>(state, t);
+
+       state.step(a, dt);
+}
+
+template<typename S, typename D>
+inline void rk2(S& state, Scalar t, Scalar dt)
+{
+       D a = evaluate<S,D>(state, t);
+       D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
+
+       state.step(b, dt);
+}
+
+template<typename S, typename D>
+inline void rk4(S& state, Scalar t, Scalar dt)
+{
+       D a = evaluate<S,D>(state, t);
+       D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
+       D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
+       D d = evaluate<S,D>(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_