X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FMath.hh;h=c63cfe7db1bddace21e4a4fa8e8b04a460122a19;hp=6665a195bc0341f0acb629d4e036bea95efd0c19;hb=a31d65a998121df0651c57bfb68782e2a07d2e2f;hpb=31d52677b38d935297d132bdbd956c655cd3feee

diff --git a/src/Moof/Math.hh b/src/Moof/Math.hh
index 6665a19..c63cfe7 100644
--- a/src/Moof/Math.hh
+++ b/src/Moof/Math.hh
@@ -84,7 +84,7 @@ inline Vector3 promote(const Vector2& vec, Scalar extra = 1.0)
 
 
 
-const Scalar EPSILON = 0.000001;
+const Scalar EPSILON = SCALAR(0.000001);
 
 /**
  * Check the equality of scalars with a certain degree of error allowed.
@@ -96,6 +96,65 @@ inline bool isEqual(Scalar a, Scalar b, Scalar epsilon = 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_