Commits

Martin Felis  committed b80c204 Draft

spatialalgebra now templated

  • Participants
  • Parent commits 58f43fd

Comments (0)

Files changed (5)

 #include "Logging.h"
 
 #include "Model.h"
+#include "mathutils.h"
 
 namespace RigidBodyDynamics {
 

File src/cmlwrapper.h

 
 typedef cml::quaternion<double, cml::fixed<>, cml::vector_first, cml::positive_cross> Quaternion;
 
-#include "spatialalgebra.h"
+// #include "spatialalgebra.h"
 
 /*
 typedef SpatialAlgebra::SpatialVector SpatialVector;

File src/mathutils.cc

-#include <mathutils.h>
+#include "mathutils.h"
 
 #include "stacktrace.h"
 #include <cmath>
 
 #include "Logging.h"
 
+namespace SpatialAlgebra {
+	SpatialVector SpatialVectorZero (0., 0., 0., 0., 0., 0.);
+}
+
 using namespace SpatialAlgebra;
 
+
 Vector3d Vector3dZero (0., 0., 0.);
 Matrix3d Matrix3dIdentity (
 		1., 0., 0.,

File src/mathutils.h

 #include "cmlwrapper.h"
 #include "spatialalgebra.h"
 
+namespace SpatialAlgebra {
+	extern SpatialVector SpatialVectorZero;
+}
+
 extern Vector3d Vector3dZero;
 extern Matrix3d Matrix3dIdentity;
 extern Matrix3d Matrix3dZero;

File src/spatialalgebra.h

  */
 namespace SpatialAlgebra {
 
-class SpatialVector;
-class SpatialMatrix;
+namespace Templates {
+	template<class T>
+		class SpatialVector;
+
+	template<class T>
+		class SpatialMatrix;
+}
+
+typedef Templates::SpatialVector<double> SpatialVector;
+typedef Templates::SpatialMatrix<double> SpatialMatrix;
+
+namespace Templates {
 
 /** \brief Vector class for spatial vectors (both motion and force vectors)
  */
+template<class T>
 class SpatialVector {
 	public:
 		SpatialVector() {};
 		};
 		~SpatialVector() {};
 
-		SpatialVector (const double &v0, const double &v1, const double &v2,
-				const double &v3, const double &v4, const double &v5) {
+		SpatialVector (const T &v0, const T &v1, const T &v2,
+				const T &v3, const T &v4, const T &v5) {
 			mData[0] = v0;
 			mData[1] = v1;
 			mData[2] = v2;
 		}
 
 		// access operators
-		const double& operator[](const unsigned int &index) const {
+		const T& operator[](const unsigned int &index) const {
 			assert (index	>= 0 && index < 6);
 			return mData[index];
 		};
-		double& operator[](const unsigned int &index) {
+		T& operator[](const unsigned int &index) {
 			assert (index	>= 0 && index < 6);
 			return mData[index];
 		}
 
-		const double& operator()(const unsigned int &index) const {
+		const T& operator()(const unsigned int &index) const {
 			assert (index	>= 0 && index < 6);
 			return mData[index];
 		};
-		double& operator()(const unsigned int &index) {
+		T& operator()(const unsigned int &index) {
 			assert (index	>= 0 && index < 6);
 			return mData[index];
 		};
 		
-		void set(const double &v0, const double &v1, const double &v2,
-				const double &v3, const double &v4, const double &v5) {
+		void set(const T &v0, const T &v1, const T &v2,
+				const T &v3, const T &v4, const T &v5) {
 			mData[0] = v0;
 			mData[1] = v1;
 			mData[2] = v2;
 		}
 
 		// Operators with scalars
-		SpatialVector operator*(const double &scalar) const {
+		SpatialVector operator*(const T &scalar) const {
 			return SpatialVector(
 					mData[0] * scalar,
 					mData[1] * scalar,
 					mData[5] * scalar
 					);
 		};
-		void operator*=(const double &scalar) {
+		void operator*=(const T &scalar) {
 			mData[0] *= scalar;
 			mData[1] *= scalar;
 			mData[2] *= scalar;
 			mData[4] *= scalar;
 			mData[5] *= scalar;
 		};
-		void operator/=(const double &scalar) {
+		void operator/=(const T &scalar) {
 			mData[0] /= scalar;
 			mData[1] /= scalar;
 			mData[2] /= scalar;
 			mData[4] /= scalar;
 			mData[5] /= scalar;
 		};
-		SpatialVector operator/(const double& val) {
+		SpatialVector operator/(const T& val) {
 			return SpatialVector (
 					mData[0] / val,
 					mData[1] / val,
 			mData[4] -= vector.mData[4];
 			mData[5] -= vector.mData[5];
 		}
-		double operator*(const SpatialVector &vector) const {
+		T operator*(const SpatialVector &vector) const {
 			return mData[0] * vector.mData[0]
 				+ mData[1] * vector.mData[1] 
 				+ mData[2] * vector.mData[2] 
 				+ mData[4] * vector.mData[4] 
 				+ mData[5] * vector.mData[5];
 		}
-		inline SpatialMatrix outer_product(const SpatialVector &vector) const;
+		inline SpatialMatrix<T> outer_product(const SpatialVector<T> &vector) const;
 
-		inline SpatialMatrix crossm() const;
-		inline SpatialMatrix crossf() const;
+		inline SpatialMatrix<T> crossm() const;
+		inline SpatialMatrix<T> crossf() const;
 			
 	private:
-		double mData[6];
+		T mData[6];
 };
 
 /** \brief Matrix class for spatial matrices (both spatial transformations and inertias)
  */
+template <class T>
 class SpatialMatrix {
 	public:
 		SpatialMatrix() {};
 		~SpatialMatrix() {};
 
 		SpatialMatrix (
-				const double &v00, const double &v01, const double &v02,
-				const double &v03, const double &v04, const double &v05,
+				const T &v00, const T &v01, const T &v02,
+				const T &v03, const T &v04, const T &v05,
 
-				const double &v10, const double &v11, const double &v12,
-				const double &v13, const double &v14, const double &v15,
+				const T &v10, const T &v11, const T &v12,
+				const T &v13, const T &v14, const T &v15,
 
-				const double &v20, const double &v21, const double &v22,
-				const double &v23, const double &v24, const double &v25,
+				const T &v20, const T &v21, const T &v22,
+				const T &v23, const T &v24, const T &v25,
 
-				const double &v30, const double &v31, const double &v32,
-				const double &v33, const double &v34, const double &v35,
+				const T &v30, const T &v31, const T &v32,
+				const T &v33, const T &v34, const T &v35,
 
-				const double &v40, const double &v41, const double &v42,
-				const double &v43, const double &v44, const double &v45,
+				const T &v40, const T &v41, const T &v42,
+				const T &v43, const T &v44, const T &v45,
 
-				const double &v50, const double &v51, const double &v52,
-				const double &v53, const double &v54, const double &v55
+				const T &v50, const T &v51, const T &v52,
+				const T &v53, const T &v54, const T &v55
 				) {
 			mData[0] = v00;
 			mData[1] = v01;
 		}
 
 		// access operators
-		const double& operator[](const unsigned int &index) const {
+		const T& operator[](const unsigned int &index) const {
 			assert (index	>= 0 && index < 36);
 			return mData[index];
 		};
-		double& operator[](const unsigned int &index) {
+		T& operator[](const unsigned int &index) {
 			assert (index	>= 0 && index < 36);
 			return mData[index];
 		}
 
-		const double& operator()(const unsigned int &row, const unsigned int &col) const {
+		const T& operator()(const unsigned int &row, const unsigned int &col) const {
 			assert (row	>= 0 && row < 6 && row	>= 0 && row < 6);
 			return mData[row*6 + col];
 		};
-		double& operator()(const unsigned int &row, const unsigned int &col) {
+		T& operator()(const unsigned int &row, const unsigned int &col) {
 			assert (row	>= 0 && row < 6 && row	>= 0 && row < 6);
 			return mData[row*6 + col];
 		};
 		
 		void set(
-				const double v00, const double v01, const double v02,
-				const double v03, const double v04, const double v05,
+				const T v00, const T v01, const T v02,
+				const T v03, const T v04, const T v05,
 
-				const double v10, const double v11, const double v12,
-				const double v13, const double v14, const double v15,
+				const T v10, const T v11, const T v12,
+				const T v13, const T v14, const T v15,
 
-				const double v20, const double v21, const double v22,
-				const double v23, const double v24, const double v25,
+				const T v20, const T v21, const T v22,
+				const T v23, const T v24, const T v25,
 
-				const double v30, const double v31, const double v32,
-				const double v33, const double v34, const double v35,
+				const T v30, const T v31, const T v32,
+				const T v33, const T v34, const T v35,
 
-				const double v40, const double v41, const double v42,
-				const double v43, const double v44, const double v45,
+				const T v40, const T v41, const T v42,
+				const T v43, const T v44, const T v45,
 
-				const double v50, const double v51, const double v52,
-				const double v53, const double v54, const double v55
+				const T v50, const T v51, const T v52,
+				const T v53, const T v54, const T v55
 				) {
 			mData[0] = v00;
 			mData[1] = v01;
 		}
 
 		// Operators with scalars
-		SpatialMatrix operator*(const double &scalar) const {
+		SpatialMatrix operator*(const T &scalar) const {
 			return SpatialMatrix(
 					mData[0] * scalar,
 					mData[1] * scalar,
 					mData[30 + 5] * scalar
 					);
 		}
-		void operator*=(const double &scalar) {
+		void operator*=(const T &scalar) {
 			for (unsigned int i = 0; i < 36; i++)
 				mData[i] *= scalar;
 		};
-		void operator/=(const double &scalar) {
+		void operator/=(const T &scalar) {
 			for (unsigned int i = 0; i < 36; i++)
 				mData[i] /= scalar;
 		}
-		SpatialMatrix operator/(const double& val) const {
+		SpatialMatrix operator/(const T& val) const {
 			return SpatialMatrix (
 					mData[0 * 6 + 0] / val,
 					mData[0 * 6 + 1] / val,
 		}
 
 		// Operators with SpatialVectors
-		SpatialVector operator*(const SpatialVector &vector) const {
-			SpatialVector result (
+		SpatialVector<T> operator*(const SpatialVector<T> &vector) const {
+			SpatialVector<T> result (
 					0., 0., 0., 0., 0., 0.
 					);
 
 		}
 
 	private:
-		double mData[36];
+		T mData[36];
 };
 
-inline std::ostream& operator<<(std::ostream& output, const SpatialVector &vector) {
+template <class T>
+inline std::ostream& operator<<(std::ostream& output, const SpatialVector<T> &vector) {
 	output << vector[0] << " " << vector[1] << " " << vector[2] << " "
 		<< vector[3] << " " << vector[4] << " " << vector[5];
 	return output;
 }
 
-inline std::ostream& operator<<(std::ostream& output, const SpatialMatrix &matrix) {
+template <class T>
+inline std::ostream& operator<<(std::ostream& output, const SpatialMatrix<T> &matrix) {
 	output << std::endl;
 	output << "[ " <<matrix(0,0) << " " << matrix(0,1) << " " << matrix(0,2) << " "
 		<< matrix(0,3) << " " << matrix(0,4) << " " << matrix(0,5) << " ]" << std::endl;
 	return output;
 }
 
-inline SpatialVector operator*(const double& val, const SpatialVector &vec) {
-	return SpatialVector (
+template <class T>
+inline SpatialVector<T> operator*(const T& val, const SpatialVector<T> &vec) {
+	return SpatialVector<T> (
 			vec[0] * val,
 			vec[1] * val,
 			vec[2] * val,
 			);
 }
 
-inline SpatialMatrix operator*(const double& val, const SpatialMatrix &mat) {
-	return SpatialMatrix (
+template <class T>
+inline SpatialMatrix<T> operator*(const T& val, const SpatialMatrix<T> &mat) {
+	return SpatialMatrix<T> (
 			mat(0,0) * val,
 			mat(0,1) * val,
 			mat(0,2) * val,
 
 // Functions of SpatialVector that are dependent on the declaration of
 // SpatialMatrix.
-SpatialMatrix SpatialVector::crossm() const
+template <class T>
+SpatialMatrix<T> SpatialVector<T>::crossm() const
 {
-	return SpatialMatrix (
+	return SpatialMatrix<T> (
 		        0,  -mData[2],  mData[1],         0,          0,         0,
 		 mData[2],          0, -mData[0],         0,          0,         0, 
 		-mData[1],   mData[0],         0,         0,          0,         0,
 
 // Functions of SpatialVector that are dependent on the declaration of
 // SpatialMatrix.
-SpatialMatrix SpatialVector::crossf() const
+template <class T>
+SpatialMatrix<T> SpatialVector<T>::crossf() const
 {
 	return this->crossm().adjoint();
 }
 
-
-inline SpatialMatrix SpatialVector::outer_product(const SpatialVector &vec) const {
-	return SpatialMatrix (
+template <class T>
+inline SpatialMatrix<T> SpatialVector<T>::outer_product(const SpatialVector<T> &vec) const {
+	return SpatialMatrix<T> (
 			mData[0] * vec[0], mData[0] * vec[1], mData[0] * vec[2], mData[0] * vec[3], mData[0] * vec[4], mData[0] * vec[5],
 			mData[1] * vec[0], mData[1] * vec[1], mData[1] * vec[2], mData[1] * vec[3], mData[1] * vec[4], mData[1] * vec[5],
 			mData[2] * vec[0], mData[2] * vec[1], mData[2] * vec[2], mData[2] * vec[3], mData[2] * vec[4], mData[2] * vec[5],
  *
  * \returns solution x
  */
-inline SpatialVector SpatialLinSolve (SpatialMatrix A, SpatialVector b) {
-	SpatialVector x(0., 0., 0., 0., 0., 0.);
+template <class T>
+inline SpatialVector<T> SpatialLinSolve (SpatialMatrix<T> A, SpatialVector<T> b) {
+	SpatialVector<T> x(0., 0., 0., 0., 0., 0.);
 	
 	int i,j;
 	for (j = 0; j < 6; j++) {
 		for (i = j + 1; i < 6; i++) {
-			double d = A(i,j)/A(j,j);
+			T d = A(i,j)/A(j,j);
 
 			b[i] -= b[j] * d;
 
 	return x;
 }
 
-const SpatialVector SpatialVectorZero (0., 0., 0., 0., 0., 0.);
+const SpatialVector<double> SpatialVectorZero (0., 0., 0., 0., 0., 0.);
 
 }
+}
 
 #endif /* SPATIALALGEBRA_H */