Dogcows Code - chaz/yoink/blob - src/Moof/Math.hh

   1
   2 /*]  Copyright (c) 2009-2010, Charles McGarvey  [**************************
   3 **]  All rights reserved.
   4 *
   5 * vi:ts=4 sw=4 tw=75
   6 *
   7 * Distributable under the terms and conditions of the 2-clause BSD license;
   8 * see the file COPYING for a complete text of the license.
   9 *
  10 **************************************************************************/
  11
  12 #ifndef _MOOF_MATH_HH_
  13 #define _MOOF_MATH_HH_
  14
  15 /**
  16  * @file Math.hh
  17  * General math-related types and functions.
  18  */
  19
  20 #include <cmath>
  21 #include <cml/cml.h>
  22
  23 #include <SDL/SDL_opengl.h>
  24
  25 #if HAVE_CONFIG_H
  26 #include "config.h"
  27 #endif
  28
  29
  30 #if USE_DOUBLE_PRECISION
  31
  32 typedef GLdouble        GLscalar;
  33 #define GL_SCALAR       GL_DOUBLE
  34 #define SCALAR(D)       (D)
  35
  36 #else
  37
  38 typedef GLfloat         GLscalar;
  39 #define GL_SCALAR       GL_FLOAT
  40 #define SCALAR(F)       (F##f)
  41
  42 #endif
  43
  44
  45 namespace Mf {
  46
  47
  48 typedef GLscalar                                                                Scalar;
  49
  50 typedef cml::vector< Scalar, cml::fixed<2> >    Vector2;
  51 typedef cml::vector< Scalar, cml::fixed<3> >    Vector3;
  52 typedef cml::vector< Scalar, cml::fixed<4> >    Vector4;
  53
  54 typedef cml::matrix< Scalar, cml::fixed<2,2>,
  55                 cml::col_basis, cml::col_major >                Matrix2;
  56 typedef cml::matrix< Scalar, cml::fixed<3,3>,
  57                 cml::col_basis, cml::col_major >                Matrix3;
  58 typedef cml::matrix< Scalar, cml::fixed<4,4>,
  59                 cml::col_basis, cml::col_major >                Matrix4;
  60
  61 typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first,
  62                 cml::positive_cross >                                   Quaternion;
  63
  64 typedef cml::constants<Scalar>                                  Constants;
  65
  66
  67 inline Vector3 demote(const Vector4& vec)
  68 {
  69         return Vector3(vec[0], vec[1], vec[2]);
  70 }
  71
  72 inline Vector2 demote(const Vector3& vec)
  73 {
  74         return Vector2(vec[0], vec[1]);
  75 }
  76
  77 inline Vector4 promote(const Vector3& vec, Scalar extra = 0.0)
  78 {
  79         return Vector4(vec[0], vec[1], vec[2], extra);
  80 }
  81
  82 inline Vector3 promote(const Vector2& vec, Scalar extra = 0.0)
  83 {
  84         return Vector3(vec[0], vec[1], extra);
  85 }
  86
  87
  88 template <class R, class P>
  89 inline R convert(const P& p)
  90 {
  91         return R(p);
  92 }
  93
  94 template <>
  95 inline Vector3 convert<Vector3,Vector4>(const Vector4& vec)
  96 {
  97         return Vector3(vec[0], vec[1], vec[2]);
  98 }
  99
 100 template <>
 101 inline Vector2 convert<Vector2,Vector3>(const Vector3& vec)
 102 {
 103         return Vector2(vec[0], vec[1]);
 104 }
 105
 106 template <>
 107 inline Vector4 convert<Vector4,Vector3>(const Vector3& vec)
 108 {
 109         return Vector4(vec[0], vec[1], vec[2], SCALAR(0.0));
 110 }
 111
 112 template <>
 113 inline Vector3 convert<Vector3,Vector2>(const Vector2& vec)
 114 {
 115         return Vector3(vec[0], vec[1], SCALAR(0.0));
 116 }
 117
 118 template <class P>
 119 struct cast
 120 {
 121         cast(const P& p) : param(p) {}
 122         template <class R>
 123         operator R() { return convert<R,P>(param); }
 124 private:
 125         const P& param;
 126 };
 127
 128
 129
 130 const Scalar EPSILON = SCALAR(0.000001);
 131
 132 /**
 133  * Check the equality of scalars with a certain degree of error allowed.
 134  */
 135
 136 inline bool isEqual(Scalar a, Scalar b, Scalar epsilon = EPSILON)
 137 {
 138         return std::abs(a - b) < epsilon;
 139 }
 140
 141
 142
 143 // Here are some generic implementations of a few simple integrators.  To
 144 // use, you need one type representing the state and another containing the
 145 // derivatives of the primary state variables.  The state class must
 146 // implement these methods:
 147 //
 148 // void getDerivative(Derivative_Type& derivative, Scalar absoluteTime);
 149 // void step(const Derivative_Type& derivative, Scalar deltaTime);
 150 //
 151 // Additionally, the derivative class must overload a few operators:
 152 //
 153 // Derivative_Type operator+(const Derivative_Type& other) const
 154 // Derivative_Type operator*(const Derivative_Type& other) const
 155
 156 template <class S, class D>
 157 inline D evaluate(const S& state, Scalar t)
 158 {
 159         D derivative;
 160         state.getDerivative(derivative, t);
 161         return derivative;
 162 }
 163
 164 template <class S, class D>
 165 inline D evaluate(S state,  Scalar t, Scalar dt, const D& derivative)
 166 {
 167         state.step(derivative, dt);
 168         return evaluate<S,D>(state, t + dt);
 169 }
 170
 171
 172 template <class S, class D>
 173 inline void euler(S& state, Scalar t, Scalar dt)
 174 {
 175         D a = evaluate<S,D>(state, t);
 176
 177         state.step(a, dt);
 178 }
 179
 180 template <class S, class D>
 181 inline void rk2(S& state, Scalar t, Scalar dt)
 182 {
 183         D a = evaluate<S,D>(state, t);
 184         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 185
 186         state.step(b, dt);
 187 }
 188
 189 template <class S, class D>
 190 inline void rk4(S& state, Scalar t, Scalar dt)
 191 {
 192         D a = evaluate<S,D>(state, t);
 193         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 194         D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
 195         D d = evaluate<S,D>(state, t, dt, c);
 196
 197         state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt);
 198 }
 199
 200
 201 } // namespace Mf
 202
 203 #endif // _MOOF_MATH_HH_
 204