Commits

Martin Felis committed 49c081e

added missing files

Comments (0)

Files changed (5)

src/SimpleMath/SimpleMathBlock.h

+/**
+ * This is a highly inefficient math library. It was conceived by Martin
+ * Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
+ * that uses a highly efficient math library.
+ *
+ * It is intended to be used as a fast compiling substitute for the
+ * blazingly fast Eigen3 library and tries to mimic its API to a certain
+ * extend.
+ *
+ * Feel free to use it wherever you like. However, no guarantees are given
+ * that this code does what it says it would.
+ */
+
+#ifndef SIMPLEMATHBLOCK_H
+#define SIMPLEMATHBLOCK_H
+
+#include <cstdlib>
+#include <cmath>
+#include <iostream>
+#include <assert.h>
+
+#include "compileassert.h"
+
+// #include "SimpleMathQR.h"
+
+/** \brief Namespace for a highly inefficient math library
+ *
+ */
+namespace SimpleMath {
+
+/** \brief Namespace for fixed size elements
+ */
+// forward declaration
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+class Matrix;
+
+template <typename matrix_type, typename val_type>
+class Block {
+	public:
+		typedef val_type value_type;
+
+		Block() :
+			mParentRows(0),
+			mParentCols(0),
+			mParentRowStart(0),
+			mParentColStart(0)
+		{ }
+		Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) :
+			mParentRows (matrix.rows()),
+			mParentCols (matrix.cols()),
+			mParentRowStart (row_start),
+			mParentColStart (col_start),
+			mRowCount (row_count),
+			mColCount (col_count),
+			mTransposed (false) {
+				assert (mParentRows >= mParentRowStart + mRowCount);
+				assert (mParentCols >= mParentColStart + mColCount);
+
+				// without the following line we could not create blocks from const
+				// matrices
+				mParentMatrix = const_cast<matrix_type*>(&matrix);
+			}
+
+		// copy data from the other block into this
+		Block& operator=(const Block &other) {
+			if (this != &other) {
+				if (mRowCount != other.rows() || mColCount != other.cols()) {
+					std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
+					abort();
+				}
+				
+				value_type temp_values[mRowCount * mColCount];
+
+				for (unsigned int i = 0; i < mRowCount; i++) {
+					for (unsigned int j = 0; j < mColCount; j++) {
+						temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
+					}
+				}
+
+				for (unsigned int i = 0; i < mRowCount; i++) {
+					for (unsigned int j = 0; j < mColCount; j++) {
+						(*this)(i,j) = temp_values[i * mColCount + j];
+					}
+				}
+			}
+
+			return *this;
+		}
+
+		template <typename other_matrix_type>
+		// copy data from the other block into this
+		Block& operator=(const other_matrix_type &other) {
+			if (mRowCount != other.rows() || mColCount != other.cols()) {
+				std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
+				abort();
+			}
+
+			value_type temp_values[mRowCount * mColCount];
+
+			for (unsigned int i = 0; i < mRowCount; i++) {
+				for (unsigned int j = 0; j < mColCount; j++) {
+					temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
+				}
+			}
+
+			for (unsigned int i = 0; i < mRowCount; i++) {
+				for (unsigned int j = 0; j < mColCount; j++) {
+					(*this)(i,j) = temp_values[i * mColCount + j];
+				}
+			}
+
+			return *this;
+		}
+
+		unsigned int rows() const {
+			return mRowCount;
+		}
+		unsigned int cols() const {
+			return mColCount;
+		}
+		const val_type& operator() (const unsigned int i, const unsigned int j) const {
+			assert (i < mRowCount);
+			assert (j < mColCount);
+
+			if (!mTransposed) {
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			return (*mParentMatrix) (j + mParentColStart, i + mParentRowStart);
+		}
+
+		val_type& operator() (const unsigned int i, const unsigned int j) {
+			assert (i < mRowCount);
+			assert (j < mColCount);
+
+			if (!mTransposed) {
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			return (*mParentMatrix) (j + mParentColStart, i + mParentRowStart);
+		}
+
+		Block transpose() const {
+			Block result (*this);
+			result.mTransposed = mTransposed ^ true;
+			return result;
+		}
+
+	private:
+		matrix_type *mParentMatrix;
+		const unsigned int mParentRows;
+		const unsigned int mParentCols;
+		const unsigned int mParentRowStart;
+		const unsigned int mParentColStart;
+		const unsigned int mRowCount;
+		const unsigned int mColCount;
+		bool mTransposed;
+};
+
+template <typename matrix_type, typename val_type>
+inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &block) {
+	unsigned int i,j;
+	for (i = 0; i < block.rows(); i++) {
+		output << "[ ";
+		for (j = 0; j < block.cols(); j++) {
+			output << block(i,j);
+
+			if (j < block.cols() - 1)
+				output << ", ";
+		}
+		output << " ]";
+		
+		if (block.rows() > 1 && i < block.rows() - 1)
+			output << std::endl;
+	}
+
+	return output;
+}
+
+
+}
+
+#endif /* SIMPLEMATHBLOCK_H */

src/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

src/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

src/SimpleMath/SimpleMathQR.h

+#ifndef _SIMPLE_MATH_QR_H
+#define _SIMPLE_MATH_QR_H
+
+#include <iostream>
+#include <limits>
+
+#include "SimpleMathFixed.h"
+#include "SimpleMathDynamic.h"
+#include "SimpleMathBlock.h"
+
+namespace SimpleMath {
+
+	template <typename matrix_type>
+	class HouseholderQR {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+
+		private:
+			HouseholderQR() {}
+
+			typedef Dynamic::Matrix<value_type> MatrixXXd;
+			typedef Dynamic::Matrix<value_type> VectorXd;
+			
+			bool mIsFactorized;
+			MatrixXXd mQ;
+			MatrixXXd mR;
+
+		public:
+			HouseholderQR(const matrix_type &matrix) :
+				mIsFactorized(false),
+				mQ(matrix.rows(), matrix.rows()),
+				mR(matrix) {
+					compute();
+			}
+			HouseholderQR compute() {
+				mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
+
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					unsigned int block_rows = mR.rows() - i;
+					unsigned int block_cols = mR.cols() - i;
+
+					MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+					VectorXd column = current_block.block(0, 0, block_rows, 1);
+
+					value_type alpha = - column.norm();
+					if (current_block(0,0) < 0) {
+						alpha = - alpha;
+					}
+
+					VectorXd v = current_block.block(0, 0, block_rows, 1);
+					v[0] = v[0] - alpha;
+
+					MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
+					Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
+						- MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));
+
+					mR = Q * mR;
+
+					// Normalize so that we have positive diagonal elements
+					if (mR(i,i) < 0) {
+						mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
+						Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
+					}
+
+					mQ = mQ * Q;
+				}
+
+				mIsFactorized = true;
+
+				return *this;
+			}
+			Dynamic::Matrix<value_type> solve (
+					const Dynamic::Matrix<value_type> &rhs
+					) const {
+				assert (mIsFactorized);
+
+				VectorXd y = mQ.transpose() * rhs;
+				VectorXd x = VectorXd::Zero(mR.cols());
+
+				for (int i = mR.cols() - 1; i >= 0; --i) {
+					value_type z = y[i];
+
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
+						z = z - x[j] * mR(i,j);
+					}
+
+					if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
+						std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
+						abort();
+					}
+					x[i] = z / mR(i,i);
+				}
+
+				return x;
+			}
+			Dynamic::Matrix<value_type> matrixQ () const {
+				return mQ;
+			}
+			Dynamic::Matrix<value_type> matrixR () const {
+				return mR;
+			}
+	};
+
+	template <typename matrix_type>
+	class ColPivHouseholderQR {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+
+		private:
+			ColPivHouseholderQR() {}
+
+			typedef Dynamic::Matrix<value_type> MatrixXXd;
+			typedef Dynamic::Matrix<value_type> VectorXd;
+			
+			bool mIsFactorized;
+			MatrixXXd mQ;
+			MatrixXXd mR;
+			unsigned int *mPermutations;
+			value_type mThreshold;
+			unsigned int mRank;
+
+		public:
+			ColPivHouseholderQR(const matrix_type &matrix) :
+				mIsFactorized(false),
+				mQ(matrix.rows(), matrix.rows()),
+				mR(matrix),
+				mThreshold (std::numeric_limits<value_type>::epsilon() * matrix.cols()) {
+					mPermutations = new unsigned int [matrix.cols()];
+					for (unsigned int i = 0; i < matrix.cols(); i++) {
+						mPermutations[i] = i;
+					}
+					compute();
+			}
+			~ColPivHouseholderQR() {
+				delete[] mPermutations;
+			}
+
+			ColPivHouseholderQR& setThreshold (const value_type& threshold) {
+				mThreshold = threshold;
+				
+				return *this;
+			}
+			ColPivHouseholderQR& compute() {
+				mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
+
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					unsigned int block_rows = mR.rows() - i;
+					unsigned int block_cols = mR.cols() - i;
+
+					// find and swap the column with the highest
+					unsigned int col_index_norm_max = i;
+					value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm();
+
+					for (unsigned int j = i; j < mR.cols(); j++) {
+						VectorXd column = mR.block(i, j, block_rows, 1);
+						
+						if (column.squaredNorm() > col_norm_max) {
+							col_index_norm_max = j;
+							col_norm_max = column.squaredNorm();
+						}
+					}
+
+					if (col_norm_max < mThreshold) {
+						// if all entries of the column is close to zero, we bail out
+						break;
+					}
+
+					if (col_index_norm_max != i) {
+						VectorXd temp_col = mR.block(i, i, block_rows, 1);
+						mR.block(i,i,block_rows,1) = mR.block(i, col_index_norm_max, block_rows, 1);
+						mR.block(i, col_index_norm_max, block_rows, 1) = temp_col;
+
+						unsigned int temp_index = mPermutations[i];
+						mPermutations[i] = mPermutations[col_index_norm_max];
+						mPermutations[col_index_norm_max] = temp_index;
+					}
+
+					MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+					VectorXd column = current_block.block(0, 0, block_rows, 1);
+
+					value_type alpha = - column.norm();
+					if (current_block(0,0) < 0) {
+						alpha = - alpha;
+					}
+
+					VectorXd v = current_block.block(0, 0, block_rows, 1);
+					v[0] = v[0] - alpha;
+
+					MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
+
+					Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
+						- MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));
+
+					mR = Q * mR;
+
+					// Normalize so that we have positive diagonal elements
+					if (mR(i,i) < 0) {
+						mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
+						Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
+					}
+
+					mQ = mQ * Q;
+				}
+
+				mIsFactorized = true;
+
+				return *this;
+			}
+			Dynamic::Matrix<value_type> solve (
+					const Dynamic::Matrix<value_type> &rhs
+					) const {
+				assert (mIsFactorized);
+
+				VectorXd y = mQ.transpose() * rhs;
+				VectorXd x = VectorXd::Zero(mR.cols());
+
+				for (int i = mR.cols() - 1; i >= 0; --i) {
+					value_type z = y[i];
+
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
+						z = z - x[mPermutations[j]] * mR(i,j);
+					}
+
+					if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
+						std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
+						abort();
+					}
+					x[mPermutations[i]] = z / mR(i,i);
+				}
+
+				return x;
+			}
+			Dynamic::Matrix<value_type> matrixQ () const {
+				return mQ;
+			}
+			Dynamic::Matrix<value_type> matrixR () const {
+				return mR;
+			}
+			Dynamic::Matrix<value_type> matrixP () const {
+				MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols());
+				MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols());
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1);
+				}
+				return P;
+			}
+
+			unsigned int rank() const {
+				value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
+
+				for (unsigned int i = 1; i < mR.cols(); i++) {
+					if (fabs(mR(i,i) < abs_threshold))
+						return i;
+				}
+
+				return mR.cols();
+			}
+	};
+
+}
+
+/* _SIMPLE_MATH_QR_H */
+#endif

src/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