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