Commits

Martin Felis committed 4af2cf9

updated to newer SimpleMath library

  • Participants
  • Parent commits a34981f
  • Branches dev

Comments (0)

Files changed (7)

include/rbdl/SimpleMath/SimpleMath.h

 #include "SimpleMathDynamic.h"
 #include "SimpleMathMixed.h"
 
+typedef SimpleMath::Fixed::Matrix<int, 3, 1> Vector3i;
+
+typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d;
+typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d;
+
+typedef SimpleMath::Fixed::Matrix<double, 4, 1> Vector4d;
+
+typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
+typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
+typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
+
+
 #endif /* _SIMPLEMATH_H */

include/rbdl/SimpleMath/SimpleMathDynamic.h

 #include <cstdlib>
 #include <assert.h>
 
-#include "rbdl/compileassert.h"
+#include "compileassert.h"
 
 /** \brief Namespace for a highly inefficient math library
  *
 class Matrix {
 	public:
 		typedef Matrix<val_type> matrix_type;
+		typedef val_type value_type;
 
 		Matrix() :
 			nrows (0),
 			ncols (0),
+			mapped_data (false),
 			mData (NULL) {};
 		Matrix(unsigned int rows) :
 			nrows (rows),
-			ncols (1) {
+			ncols (1),
+			mapped_data (false) {
 				mData = new val_type[rows];
 			}
 		Matrix(unsigned int rows, unsigned int cols) :
 			nrows (rows),
-			ncols (cols) {
+			ncols (cols),
+			mapped_data (false) {
 				mData = new val_type[rows * cols];
 			}
+
+		Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) :
+			nrows (rows),
+			ncols (cols),
+			mapped_data (true) {
+				mData = data_ptr;
+			}
+	
 		unsigned int rows() const {
 			return nrows;
 		}
 			return nrows * ncols;
 		}
 		void resize (unsigned int rows, unsigned int cols=1) {
-			if (nrows * ncols > 0 && mData != NULL) {
+			if (nrows * ncols > 0 && mData != NULL && mapped_data == false) {
 				delete[] mData;
 			}
 
 
 		Matrix(const Matrix &matrix) :
 			nrows (matrix.nrows),
-			ncols (matrix.ncols) {
+			ncols (matrix.ncols),
+			mapped_data (false) {
 			unsigned int i;
 		
 			mData = new val_type[nrows * ncols];
 		}
 		Matrix& operator=(const Matrix &matrix) {
 			if (this != &matrix) {
-				delete[] mData;
+				if (!mapped_data) {
+					delete[] mData;
 
-				nrows = matrix.nrows;
-				ncols = matrix.ncols;
+					nrows = matrix.nrows;
+					ncols = matrix.ncols;
+					mapped_data = false;
 
-				mData = new val_type[nrows * ncols];
+					mData = new val_type[nrows * ncols];
 
-				unsigned int i;
-				for (i = 0; i < nrows * ncols; i++)
-					mData[i] = matrix.mData[i];
+					unsigned int i;
+					for (i = 0; i < nrows * ncols; i++)
+						mData[i] = matrix.mData[i];
+				} else {
+					// we overwrite any existing data
+					nrows = matrix.nrows;
+					ncols = matrix.ncols;
+					mapped_data = true;
+
+					unsigned int i;
+					for (i = 0; i < nrows * ncols; i++)
+						mData[i] = matrix.mData[i];
+				}
 			}
 			return *this;
 		}
 
 		~Matrix() {
-			if (nrows * ncols > 0 || mData != NULL)
+			if (nrows * ncols > 0 && mData != NULL && mapped_data == false) {
 				delete[] mData;
+				mData = NULL;
+			}
 
 			nrows = 0;
 			ncols = 0;
-			mData = NULL;
 		};
 
 		// comparison
 	private:
 		unsigned int nrows;
 		unsigned int ncols;
+		bool mapped_data;
 
 		val_type* mData;
 };

include/rbdl/SimpleMath/SimpleMathFixed.h

 
 #include <sstream>
 #include <cstdlib>
+#include <cmath>
 #include <assert.h>
 
-#include "rbdl/compileassert.h"
+#include "compileassert.h"
 
 /** \brief Namespace for a highly inefficient math library
  *
 template <typename val_type> class Matrix;
 }
 
-
 /** \brief Namespace for fixed size elements
  */
 namespace Fixed {
 	val_type *parent;
 };
 
-/** \brief Fixed size matrix class 
+/** \brief Fixed size matrix class
  */
-
 template <typename val_type, unsigned int nrows, unsigned int ncols>
 class Matrix {
 	public:
+		typedef Matrix<val_type, nrows, ncols> matrix_type;
+		typedef val_type value_type;
+
 		unsigned int rows() const {
 			return nrows;
 		}
 			return nrows * ncols;
 		}
 
-		typedef Matrix<val_type, nrows, ncols> matrix_type;
-
 		Matrix() {};
 		Matrix(const Matrix &matrix) {
 			unsigned int i;
 		}
 
 		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
+				) {
+			assert (nrows == 4);
+			assert (ncols == 1);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
+				) {
+			COMPILE_ASSERT (nrows * ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
+				const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
+				const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
+				const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
+				) {
+			COMPILE_ASSERT (nrows == 4);
+			COMPILE_ASSERT (ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+
+			mData[1 * 4 + 0] = v10;
+			mData[1 * 4 + 1] = v11;
+			mData[1 * 4 + 2] = v12;
+			mData[1 * 4 + 3] = v13;
+			
+			mData[2 * 4 + 0] = v20;
+			mData[2 * 4 + 1] = v21;
+			mData[2 * 4 + 2] = v22;
+			mData[2 * 4 + 3] = v23;
+
+			mData[3 * 4 + 0] = v30;
+			mData[3 * 4 + 1] = v31;
+			mData[3 * 4 + 2] = v32;
+			mData[3 * 4 + 3] = v33;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
+				const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
+				const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
+				const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
+				) {
+			COMPILE_ASSERT (nrows == 4);
+			COMPILE_ASSERT (ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+
+			mData[1 * 4 + 0] = v10;
+			mData[1 * 4 + 1] = v11;
+			mData[1 * 4 + 2] = v12;
+			mData[1 * 4 + 3] = v13;
+			
+			mData[2 * 4 + 0] = v20;
+			mData[2 * 4 + 1] = v21;
+			mData[2 * 4 + 2] = v22;
+			mData[2 * 4 + 3] = v23;
+
+			mData[3 * 4 + 0] = v30;
+			mData[3 * 4 + 1] = v31;
+			mData[3 * 4 + 2] = v32;
+			mData[3 * 4 + 3] = v33;
+		}
+
+		Matrix (
 				const val_type &v00, const val_type &v01, const val_type &v02,
 				const val_type &v03, const val_type &v04, const val_type &v05
 				) {
 			mData[30 + 4] = v54;
 			mData[30 + 5] = v55;
 		}
-
+		
 		// comparison
 		bool operator==(const Matrix &matrix) const {
 			for (unsigned int i = 0; i < nrows * ncols; i++) {
 			return sqrt(this->squaredNorm());
 		}
 
-		void normalize() {
+		matrix_type normalize() {
 			val_type length = this->norm();
 
 			for (unsigned int i = 0; i < ncols * nrows; i++)
 				mData[i] /= length;
+
+			return *this;
 		}
 
 		Matrix<val_type, 3, 1> cross(const Matrix<val_type, 3, 1> &other_vector) {
 			return result;
 		}
 
+		static matrix_type Identity() {
+			matrix_type result;
+			result.identity();
+			return result;
+		}
 
 		static matrix_type Identity(int ignore_me, int ignore_me_too) {
 			matrix_type result;

include/rbdl/SimpleMath/SimpleMathGL.h

+#ifndef _SIMPLEMATHGL_H_
+#define _SIMPLEMATHGL_H_
+
+#include "SimpleMath.h"
+#include <cmath>
+
+namespace SimpleMath {
+
+namespace GL {
+
+inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) {
+	float c = cosf (rot_deg * M_PI / 180.f);
+	float s = sinf (rot_deg * M_PI / 180.f);
+	return Matrix44f (
+			x * x * (1.0f - c) + c,
+			y * x * (1.0f - c) + z * s,
+			x * z * (1.0f - c) - y * s,
+			0.f, 
+
+			x * y * (1.0f - c) - z * s,
+			y * y * (1.0f - c) + c,
+			y * z * (1.0f - c) + x * s,
+			0.f,
+
+			x * z * (1.0f - c) + y * s,
+			y * z * (1.0f - c) - x * s,
+			z * z * (1.0f - c) + c,
+			0.f,
+
+			0.f, 0.f, 0.f, 1.f
+			);
+}
+
+inline Matrix44f TranslateMat44 (float x, float y, float z) {
+	return Matrix44f (
+			1.f, 0.f, 0.f, 0.f,
+			0.f, 1.f, 0.f, 0.f,
+			0.f, 0.f, 1.f, 0.f,
+			  x,   y,   z, 1.f
+			);
+}
+
+inline Matrix44f ScaleMat44 (float x, float y, float z) {
+	return Matrix44f (
+			  x, 0.f, 0.f, 0.f,
+			0.f,   y, 0.f, 0.f,
+			0.f, 0.f,   z, 0.f,
+			0.f, 0.f, 0.f, 1.f
+			);
+}
+
+/** Quaternion 
+ *
+ * order: x,y,z,w
+ */
+class Quaternion : public Vector4f {
+	public:
+		Quaternion () :
+			Vector4f (0.f, 0.f, 0.f, 1.f)
+		{}
+		Quaternion (const Vector4f vec4) :
+			Vector4f (vec4)
+		{}
+		Quaternion (float x, float y, float z, float w):
+			Vector4f (x, y, z, w)
+		{}
+		/** This function is equivalent to multiplicate their corresponding rotation matrices */
+		Quaternion operator* (const Quaternion &q) const {
+			return Quaternion (
+					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					);
+		}
+		Quaternion& operator*=(const Quaternion &q) {
+			set (
+					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					);
+			return *this;
+		}
+
+		static Quaternion fromGLRotate (float angle, float x, float y, float z) {
+			float st = sinf (angle * M_PI / 360.f);
+			return Quaternion (
+						st * x,
+						st * y,
+						st * z,
+						cosf (angle * M_PI / 360.f)
+						);
+		}
+
+		Quaternion normalize() {
+			return Vector4f::normalize();
+		}
+
+		Quaternion slerp (float alpha, const Quaternion &quat) const {
+			// check whether one of the two has 0 length
+			float s = sqrt (squaredNorm() * quat.squaredNorm());
+
+			// division by 0.f is unhealthy!
+			assert (s != 0.f);
+
+			float angle = acos (dot(quat) / s);
+			if (angle == 0.f || isnan(angle)) {
+				return *this;
+			}
+			assert(!isnan(angle));
+
+			float d = 1.f / sinf (angle);
+			float p0 = sinf ((1.f - alpha) * angle);
+			float p1 = sinf (alpha * angle);
+
+			if (dot (quat) < 0.f) {
+				return Quaternion( ((*this) * p0 - quat * p1) * d);
+			}
+			return Quaternion( ((*this) * p0 + quat * p1) * d);
+		}
+
+		Matrix44f toGLMatrix() const {
+			float x = (*this)[0];
+			float y = (*this)[1];
+			float z = (*this)[2];
+			float w = (*this)[3];
+			return Matrix44f (
+					1 - 2*y*y - 2*z*z,
+					2*x*y + 2*w*z,
+					2*x*z - 2*w*y,
+					0.f,
+
+					2*x*y - 2*w*z,
+					1 - 2*x*x - 2*z*z,
+					2*y*z + 2*w*x,
+					0.f,
+
+					2*x*z + 2*w*y,
+					2*y*z - 2*w*x,
+					1 - 2*x*x - 2*y*y,
+					0.f,
+					
+					0.f,
+					0.f,
+					0.f,
+					1.f);
+		}
+
+		Quaternion conjugate() const {
+			return Quaternion (
+					-(*this)[0],
+					-(*this)[1],
+					-(*this)[2],
+					(*this)[3]);
+		}
+
+		Vector3f rotate (const Vector3f &vec) const {
+			Vector3f vn (vec);
+			vn.normalize();
+
+			Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
+
+			res_quat = vec_quat * (*this);
+			res_quat = conjugate() * res_quat;
+
+			return Vector3f (res_quat[0], res_quat[1], res_quat[2]);
+		}
+};
+
+// namespace GL
+}
+
+// namespace SimpleMath
+}
+
+/* _SIMPLEMATHGL_H_ */
+#endif

include/rbdl/SimpleMath/SimpleMathMap.h

+#ifndef SIMPLEMATHMAP_H
+#define SIMPLEMATHMAP_H
+
+#include "compileassert.h"
+
+namespace SimpleMath {
+
+/** \brief \brief Wraps a varying size matrix type around existing data
+ *
+ * \warning If you create a matrix using the map function and then assign
+ *  a bigger matrix you invalidate the original memory!
+ *
+ */
+template < typename MatrixType >
+MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) {
+	return MatrixType (rows, cols, data);
+}
+
+}
+
+// SIMPLEMATHMAP_H
+#endif

include/rbdl/SimpleMath/SimpleMathMixed.h

 
 #include <sstream>
 #include <cstdlib>
+#include <assert.h>
 #include <iostream>
-#include <assert.h>
 
-#include "rbdl/compileassert.h"
+#include "compileassert.h"
 
 /** \brief Namespace for a highly inefficient math library
  *
 	return *this;
 }
 
-
 // multiplication
 template <typename val_type, unsigned int nrows, unsigned int ncols>
 inline Dynamic::Matrix<val_type> operator*(

include/rbdl/SimpleMath/compileassert.h

+#ifndef _COMPILE_ASSERT_H
+#define _COMPILE_ASSERT_H
+
+/* 
+ * This is a simple compile time assertion tool taken from:
+ *   http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx
+ * written by Abhinaba Basu!
+ *
+ * Thanks!
+ */
+
+#ifdef __cplusplus
+
+#define JOIN( X, Y ) JOIN2(X,Y)
+#define JOIN2( X, Y ) X##Y
+
+namespace static_assert
+{
+    template <bool> struct STATIC_ASSERT_FAILURE;
+    template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
+
+    template<int x> struct static_assert_test{};
+}
+
+#define COMPILE_ASSERT(x) \
+    typedef ::static_assert::static_assert_test<\
+        sizeof(::static_assert::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
+            JOIN(_static_assert_typedef, __LINE__)
+
+#else // __cplusplus
+
+#define COMPILE_ASSERT(x) extern int __dummy[(int)x]
+
+#endif // __cplusplus
+
+#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) 
+
+// _COMPILE_ASSERT_H_
+#endif