Dogcows Code - chaz/yoink/blob - src/moof/math.hh

   1
   2 /*]  Copyright (c) 2009-2011, Charles McGarvey  [*****************************
   3 **]  All rights reserved.
   4 *
   5 * Distributable under the terms and conditions of the 2-clause BSD license;
   6 * see the file COPYING for a complete text of the license.
   7 *
   8 *****************************************************************************/
   9
  10 #ifndef _MOOF_MATH_HH_
  11 #define _MOOF_MATH_HH_
  12
  13 #include <cmath>
  14
  15 #include <boost/shared_ptr.hpp>
  16 #include <SDL/SDL_opengl.h>
  17
  18 #include <cml/cml.h>
  19
  20 #if HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24
  25 #if ENABLE_DOUBLE_PRECISION
  26 typedef GLdouble        GLscalar;
  27 #define GL_SCALAR       GL_DOUBLE
  28 #define SCALAR(D)       (D)
  29 #else
  30 typedef GLfloat         GLscalar;
  31 #define GL_SCALAR       GL_FLOAT
  32 #define SCALAR(F)       (F##f)
  33 #endif
  34
  35
  36 /**
  37  * \file math.hh
  38  * General math-related types and functions.
  39  */
  40
  41 namespace moof {
  42
  43
  44 using namespace cml;
  45
  46 typedef GLscalar scalar;
  47
  48 typedef vector< scalar, fixed<2> > vector2;
  49 typedef vector< scalar, fixed<3> > vector3;
  50 typedef vector< scalar, fixed<4> > vector4;
  51
  52 typedef matrix< scalar, fixed<2,2>, col_basis, col_major > matrix2;
  53 typedef matrix< scalar, fixed<3,3>, col_basis, col_major > matrix3;
  54 typedef matrix< scalar, fixed<4,4>, col_basis, col_major > matrix4;
  55
  56 typedef quaternion< scalar, fixed<>, vector_first, positive_cross > quaternion;
  57
  58 typedef constants<scalar> constants;
  59
  60 inline vector3 demote(const vector4& vec)
  61 {
  62         return vector3(vec[0], vec[1], vec[2]);
  63 }
  64
  65 inline vector2 demote(const vector3& vec)
  66 {
  67         return vector2(vec[0], vec[1]);
  68 }
  69
  70 inline vector4 promote(const vector3& vec, scalar extra = SCALAR(0.0))
  71 {
  72         return vector4(vec[0], vec[1], vec[2], extra);
  73 }
  74
  75 inline vector3 promote(const vector2& vec, scalar extra = SCALAR(0.0))
  76 {
  77         return vector3(vec[0], vec[1], extra);
  78 }
  79
  80
  81 const scalar EPSILON = SCALAR(0.000001);
  82
  83 /**
  84  * Check the equality of scalars with a certain degree of error allowed.
  85  */
  86
  87 inline bool is_equal(scalar a, scalar b, scalar epsilon = EPSILON)
  88 {
  89         return std::abs(a - b) < epsilon;
  90 }
  91
  92
  93 // Here are some generic implementations of a few simple integrators.  To use,
  94 // you need one type representing the state and another containing the
  95 // derivatives of the primary state variables.  The state class must implement
  96 // these methods:
  97 //
  98 // void calculate_derivative(Derivative_Type& derivative, scalar absoluteTime);
  99 // void step(const Derivative_Type& derivative, scalar deltaTime);
 100 //
 101 // Additionally, the derivative class must overload a few operators:
 102 //
 103 // Derivative_Type operator + (const Derivative_Type& other) const
 104 // Derivative_Type operator * (const Derivative_Type& other) const
 105
 106 template <class S, class D>
 107 inline D evaluate(const S& state, scalar t)
 108 {
 109         D derivative;
 110         state.calculate_derivative(derivative, t);
 111         return derivative;
 112 }
 113
 114 template <class S, class D>
 115 inline D evaluate(S state,  scalar t, scalar dt, const D& derivative)
 116 {
 117         state.step(derivative, dt);
 118         return evaluate<S,D>(state, t + dt);
 119 }
 120
 121 template <class S, class D>
 122 inline void euler(S& state, scalar t, scalar dt)
 123 {
 124         D a = evaluate<S,D>(state, t);
 125         state.step(a, dt);
 126 }
 127
 128 template <class S, class D>
 129 inline void rk2(S& state, scalar t, scalar dt)
 130 {
 131         D a = evaluate<S,D>(state, t);
 132         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 133         state.step(b, dt);
 134 }
 135
 136 template <class S, class D>
 137 inline void rk4(S& state, scalar t, scalar dt)
 138 {
 139         D a = evaluate<S,D>(state, t);
 140         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 141         D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
 142         D d = evaluate<S,D>(state, t, dt, c);
 143         state.step((a + (b + c) * SCALAR(2.0) + d) * (SCALAR(1.0)/SCALAR(6.0)), dt);
 144 }
 145
 146
 147 } // namespace moof
 148
 149 #endif // _MOOF_MATH_HH_
 150