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

   1
   2 /*******************************************************************************
   3
   4  Copyright (c) 2009, Charles McGarvey
   5  All rights reserved.
   6
   7  Redistribution   and   use  in  source  and  binary  forms,  with  or  without
   8  modification, are permitted provided that the following conditions are met:
   9
  10    * Redistributions  of  source  code  must retain the above copyright notice,
  11      this list of conditions and the following disclaimer.
  12    * Redistributions  in binary form must reproduce the above copyright notice,
  13      this  list of conditions and the following disclaimer in the documentation
  14      and/or other materials provided with the distribution.
  15
  16  THIS  SOFTWARE  IS  PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  17  AND  ANY  EXPRESS  OR  IMPLIED  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  DISCLAIMED.  IN  NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  20  FOR  ANY  DIRECT,  INDIRECT,  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  DAMAGES  (INCLUDING,  BUT  NOT  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  22  SERVICES;  LOSS  OF  USE,  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  23  CAUSED  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  24  OR  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  25  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26
  27 *******************************************************************************/
  28
  29 #ifndef _MOOF_MATH_HH_
  30 #define _MOOF_MATH_HH_
  31
  32 /**
  33  * @file Math.hh
  34  * General math-related types and functions.
  35  */
  36
  37 #include <cmath>
  38 #include <cml/cml.h>
  39
  40 #include <SDL/SDL_opengl.h>
  41
  42 #if HAVE_CONFIG_H
  43 #include "config.h"
  44 #endif
  45
  46
  47 #if USE_DOUBLE_PRECISION
  48
  49 typedef GLdouble        GLscalar;
  50 #define GL_SCALAR       GL_DOUBLE
  51 #define SCALAR(D)       (D)
  52
  53 #else
  54
  55 typedef GLfloat         GLscalar;
  56 #define GL_SCALAR       GL_FLOAT
  57 #define SCALAR(F)       (F##f)
  58
  59 #endif
  60
  61
  62 namespace Mf {
  63
  64
  65 typedef GLscalar                                                                Scalar;
  66
  67 typedef cml::vector< Scalar, cml::fixed<2> >    Vector2;
  68 typedef cml::vector< Scalar, cml::fixed<3> >    Vector3;
  69 typedef cml::vector< Scalar, cml::fixed<4> >    Vector4;
  70
  71 typedef cml::matrix< Scalar, cml::fixed<2,2>,
  72                 cml::col_basis, cml::col_major >                Matrix2;
  73 typedef cml::matrix< Scalar, cml::fixed<3,3>,
  74                 cml::col_basis, cml::col_major >                Matrix3;
  75 typedef cml::matrix< Scalar, cml::fixed<4,4>,
  76                 cml::col_basis, cml::col_major >                Matrix4;
  77
  78 typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first,
  79                 cml::positive_cross >                                   Quaternion;
  80
  81 typedef cml::constants<Scalar>                                  Constants;
  82
  83
  84 inline Vector3 demote(const Vector4& vec)
  85 {
  86         return Vector3(vec[0], vec[1], vec[2]);
  87 }
  88
  89 inline Vector2 demote(const Vector3& vec)
  90 {
  91         return Vector2(vec[0], vec[1]);
  92 }
  93
  94 inline Vector4 promote(const Vector3& vec, Scalar extra = 0.0)
  95 {
  96         return Vector4(vec[0], vec[1], vec[2], extra);
  97 }
  98
  99 inline Vector3 promote(const Vector2& vec, Scalar extra = 0.0)
 100 {
 101         return Vector3(vec[0], vec[1], extra);
 102 }
 103
 104
 105 template <typename R, typename P>
 106 inline R convert(const P& p)
 107 {
 108         return R(p);
 109 }
 110
 111 template <>
 112 inline Vector3 convert<Vector3,Vector4>(const Vector4& vec)
 113 {
 114         return Vector3(vec[0], vec[1], vec[2]);
 115 }
 116
 117 template <>
 118 inline Vector2 convert<Vector2,Vector3>(const Vector3& vec)
 119 {
 120         return Vector2(vec[0], vec[1]);
 121 }
 122
 123 template <>
 124 inline Vector4 convert<Vector4,Vector3>(const Vector3& vec)
 125 {
 126         return Vector4(vec[0], vec[1], vec[2], SCALAR(0.0));
 127 }
 128
 129 template <>
 130 inline Vector3 convert<Vector3,Vector2>(const Vector2& vec)
 131 {
 132         return Vector3(vec[0], vec[1], SCALAR(0.0));
 133 }
 134
 135 template <typename P>
 136 struct cast
 137 {
 138         cast(const P& p) : param(p) {}
 139         template <typename R>
 140         operator R() { return convert<R,P>(param); }
 141 private:
 142         const P& param;
 143 };
 144
 145
 146
 147 const Scalar EPSILON = SCALAR(0.000001);
 148
 149 /**
 150  * Check the equality of scalars with a certain degree of error allowed.
 151  */
 152
 153 inline bool isEqual(Scalar a, Scalar b, Scalar epsilon = EPSILON)
 154 {
 155         return std::abs(a - b) < epsilon;
 156 }
 157
 158
 159
 160 // Here are some generic implementations of a few simple integrators.  To use,
 161 // you need one type representing the state and another containing the
 162 // derivatives of the primary state variables.  The state class must implement
 163 // these methods:
 164 //
 165 // void getDerivative(Derivative_Type& derivative, Scalar absoluteTime);
 166 // void step(const Derivative_Type& derivative, Scalar deltaTime);
 167 //
 168 // Additionally, the derivative class must overload a few operators:
 169 //
 170 // Derivative_Type operator+(const Derivative_Type& other) const
 171 // Derivative_Type operator*(const Derivative_Type& other) const
 172
 173 template<typename S, typename D>
 174 inline D evaluate(const S& state, Scalar t)
 175 {
 176         D derivative;
 177         state.getDerivative(derivative, t);
 178         return derivative;
 179 }
 180
 181 template<typename S, typename D>
 182 inline D evaluate(S state,  Scalar t, Scalar dt, const D& derivative)
 183 {
 184         state.step(derivative, dt);
 185         return evaluate<S,D>(state, t + dt);
 186 }
 187
 188
 189 template<typename S, typename D>
 190 inline void euler(S& state, Scalar t, Scalar dt)
 191 {
 192         D a = evaluate<S,D>(state, t);
 193
 194         state.step(a, dt);
 195 }
 196
 197 template<typename S, typename D>
 198 inline void rk2(S& state, Scalar t, Scalar dt)
 199 {
 200         D a = evaluate<S,D>(state, t);
 201         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 202
 203         state.step(b, dt);
 204 }
 205
 206 template<typename S, typename D>
 207 inline void rk4(S& state, Scalar t, Scalar dt)
 208 {
 209         D a = evaluate<S,D>(state, t);
 210         D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
 211         D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
 212         D d = evaluate<S,D>(state, t, dt, c);
 213
 214         state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt);
 215 }
 216
 217
 218 } // namespace Mf
 219
 220 #endif // _MOOF_MATH_HH_
 221
 222 /** vim: set ts=4 sw=4 tw=80: *************************************************/
 223