typedef cml::constants<Scalar> Constants;
-inline Vector3& demoteVector(Vector3& left, const Vector4& right)
+inline Vector3 demote(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)
+inline Vector2 demote(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)
+inline Vector4 promote(const Vector3& vec, Scalar extra = 1.0)
{
- 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], extra);
}
-inline Vector3& promoteVector(Vector3& left, const Vector2& right, Scalar extra = 1.0)
+inline Vector3 promote(const Vector2& vec, Scalar extra = 1.0)
{
- left[0] = right[0];
- left[1] = right[1];
- left[3] = extra;
- return left;
+ return Vector3(vec[0], vec[1], extra);
}
-const Scalar EPSILON = 0.000001;
+const Scalar EPSILON = SCALAR(0.000001);
/**
* Check the equality of scalars with a certain degree of error allowed.
}
+
+// 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_