X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FMath.hh;h=45c6e90b7ee02543df9bed379ac519291529aaaa;hp=eb411e06d3299d90778117bafe81b08061c343ad;hb=3f6e44698c38b74bb622ad81ea9d2daa636981d2;hpb=57b78ebe21b1b48acd337daa5a1cb8c383959cfa

diff --git a/src/Moof/Math.hh b/src/Moof/Math.hh
index eb411e0..45c6e90 100644
--- a/src/Moof/Math.hh
+++ b/src/Moof/Math.hh
@@ -37,7 +37,26 @@
 #include <cmath>
 #include <cml/cml.h>
 
-#include <Moof/OpenGL.hh>		// GLscalar
+#include <SDL/SDL_opengl.h>
+
+#if HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+#if USE_DOUBLE_PRECISION
+
+typedef GLdouble	GLscalar;
+#define GL_SCALAR	GL_DOUBLE
+#define SCALAR(D)	(D)
+
+#else
+
+typedef GLfloat		GLscalar;
+#define GL_SCALAR	GL_FLOAT
+#define SCALAR(F)	(F##f)
+
+#endif
 
 
 namespace Mf {
@@ -62,41 +81,70 @@ typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first,
 typedef cml::constants<Scalar>					Constants;
 
 
-inline Vector3& demoteVector(Vector3& left, const Vector4& right)
+inline Vector3 demote(const Vector4& vec)
+{
+	return Vector3(vec[0], vec[1], vec[2]);
+}
+
+inline Vector2 demote(const Vector3& vec)
+{
+	return Vector2(vec[0], vec[1]);
+}
+
+inline Vector4 promote(const Vector3& vec, Scalar extra = 0.0)
+{
+	return Vector4(vec[0], vec[1], vec[2], extra);
+}
+
+inline Vector3 promote(const Vector2& vec, Scalar extra = 0.0)
+{
+	return Vector3(vec[0], vec[1], extra);
+}
+
+
+template <typename R, typename P>
+inline R convert(const P& p)
+{
+	return R(p);
+}
+
+template <>
+inline Vector3 convert<Vector3,Vector4>(const Vector4& vec)
 {
-	left[0] = right[0];
-	left[1] = right[1];
-	left[2] = right[2];
-	return left;
+	return Vector3(vec[0], vec[1], vec[2]);
 }
 
-inline Vector2& demoteVector(Vector2& left, const Vector3& right)
+template <>
+inline Vector2 convert<Vector2,Vector3>(const Vector3& vec)
 {
-	left[0] = right[0];
-	left[1] = right[1];
-	return left;
+	return Vector2(vec[0], vec[1]);
 }
 
-inline Vector4& promoteVector(Vector4& left, const Vector3& right, Scalar extra = 1.0)
+template <>
+inline Vector4 convert<Vector4,Vector3>(const Vector3& vec)
 {
-	left[0] = right[0];
-	left[1] = right[1];
-	left[2] = right[2];
-	left[3] = extra;
-	return left;
+	return Vector4(vec[0], vec[1], vec[2], SCALAR(0.0));
 }
 
-inline Vector3& promoteVector(Vector3& left, const Vector2& right, Scalar extra = 1.0)
+template <>
+inline Vector3 convert<Vector3,Vector2>(const Vector2& vec)
 {
-	left[0] = right[0];
-	left[1] = right[1];
-	left[3] = extra;
-	return left;
+	return Vector3(vec[0], vec[1], SCALAR(0.0));
 }
 
+template <typename P>
+struct cast
+{
+	cast(const P& p) : param(p) {}
+	template <typename R>
+	operator R() { return convert<R,P>(param); }
+private:
+	const P& param;
+};
 
 
-const Scalar EPSILON = 0.000001;
+
+const Scalar EPSILON = SCALAR(0.000001);
 
 /**
  * Check the equality of scalars with a certain degree of error allowed.
@@ -108,6 +156,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_