Commits

Martin Felis  committed 7a77f7a

updated SimpleMath

  • Participants
  • Parent commits 723ec47
  • Tags v1.0.0

Comments (0)

Files changed (7)

File src/SimpleMath/SimpleMath.h

 #include "SimpleMathDynamic.h"
 #include "SimpleMathMixed.h"
 #include "SimpleMathQR.h"
+#include "SimpleMathCommaInitializer.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 */

File src/SimpleMath/SimpleMathBlock.h

 					abort();
 				}
 				
-				value_type temp_values[mRowCount * mColCount];
+				value_type* temp_values = new value_type [mRowCount * mColCount];
 
 				for (unsigned int i = 0; i < mRowCount; i++) {
 					for (unsigned int j = 0; j < mColCount; j++) {
 						(*this)(i,j) = temp_values[i * mColCount + j];
 					}
 				}
+
+				delete[] temp_values;
 			}
 
 			return *this;
 				abort();
 			}
 
-			value_type temp_values[mRowCount * mColCount];
+			value_type *temp_values = new value_type[mRowCount * mColCount];
 
 			for (unsigned int i = 0; i < mRowCount; i++) {
 				for (unsigned int j = 0; j < mColCount; j++) {
 					(*this)(i,j) = temp_values[i * mColCount + j];
 				}
 			}
+			
+			delete[] temp_values;
 
 			return *this;
 		}

File src/SimpleMath/SimpleMathDynamic.h

  */
 namespace SimpleMath {
 
+template <typename matrix_type>
+class HouseholderQR;
+
+template <typename matrix_type>
+class ColPivHouseholderQR;
+
+template <typename matrix_type>
+class CommaInitializer;
+
 namespace Fixed {
 	template <typename val_type, unsigned int ncols, unsigned int nrows> class Matrix;
 }
 
+
 /** \brief Namespace for elements of varying size.
  */
 namespace Dynamic {
 			return *this;
 		}
 
+		CommaInitializer<matrix_type> operator<< (const val_type& value) {
+			return CommaInitializer<matrix_type> (*this, value);
+		}
+
 		// conversion different val_types
 		template <typename other_type>
 		Matrix (const Matrix<other_type> &matrix) :
 				}
 			}
 
-		Matrix (const Block<matrix_type, value_type> &block) :
+		template <typename other_matrix_type>
+		Matrix (const Block<other_matrix_type, value_type> &block) :
 			nrows(block.rows()),
 			ncols(block.cols()),
 			mapped_data (false) {
 			return mData[0];
 		}
 
-//		const HouseholderQR<matrix_type> colPivHouseholderQR() const {
-//			return HouseholderQR<matrix_type>(*this);
-//		}
+		const HouseholderQR<matrix_type> householderQR() const {
+			return HouseholderQR<matrix_type>(*this);
+		}
+		const ColPivHouseholderQR<matrix_type> colPivHouseholderQR() const {
+			return ColPivHouseholderQR<matrix_type>(*this);
+		}
 
 	private:
 		unsigned int nrows;

File src/SimpleMath/SimpleMathFixed.h

 #include <assert.h>
 
 #include "compileassert.h"
-
 #include "SimpleMathBlock.h"
 
 /** \brief Namespace for a highly inefficient math library
  */
 namespace SimpleMath {
 
-namespace Dynamic {
-template <typename val_type> class Matrix;
-}
-
 template <typename matrix_type>
 class HouseholderQR;
 
 template <typename matrix_type>
 class ColPivHouseholderQR;
 
+template <typename matrix_type>
+class CommaInitializer;
+
+namespace Dynamic {
+template <typename val_type> class Matrix;
+}
+
 /** \brief Namespace for fixed size elements
  */
 namespace Fixed {
 
 			return *this;
 		}
+		
+		CommaInitializer<matrix_type> operator<< (const val_type& value) {
+			return CommaInitializer<matrix_type> (*this, value);
+		}
 
 		// conversion Dynamic->Fixed
 		Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix);
 			return result;
 		}
 
+		// multiplication with dynamic sized matrix
+		template <typename other_type>
+		Dynamic::Matrix<val_type> operator*(const Dynamic::Matrix<other_type> &other_matrix) {
+			assert (ncols == other_matrix.rows());
+
+			Dynamic::Matrix<val_type> result(nrows, other_matrix.cols());
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_matrix.cols(); j++) {
+					for (k = 0; k < other_matrix.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * static_cast<val_type>(other_matrix(k,j));
+					}
+				}
+			}
+			
+			return result;
+		}
+
+
 		void operator*=(const Matrix &matrix) {
 			matrix_type temp (*this);
 			*this = temp * matrix;

File src/SimpleMath/SimpleMathQR.h

 
 				return x;
 			}
+			Dynamic::Matrix<value_type> inverse() const {
+				assert (mIsFactorized);
+
+				VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
+				MatrixXXd result (mQ.cols(), mQ.cols());
+
+				for (unsigned int i = 0; i < mQ.cols(); i++) {
+					rhs_temp[i] = 1.;
+
+					result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
+
+					rhs_temp[i] = 0.;
+				}
+
+				return result;
+			}
 			Dynamic::Matrix<value_type> matrixQ () const {
 				return mQ;
 			}
 					unsigned int block_rows = mR.rows() - i;
 					unsigned int block_cols = mR.cols() - i;
 
-					// find and swap the column with the highest
+					// find and swap the column with the highest norm
 					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++) {
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
 						VectorXd column = mR.block(i, j, block_rows, 1);
 						
 						if (column.squaredNorm() > col_norm_max) {
 						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;
+						VectorXd temp_col = mR.block(0, i, mR.rows(), 1);
+						mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1);
+						mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col;
 
 						unsigned int temp_index = mPermutations[i];
 						mPermutations[i] = mPermutations[col_index_norm_max];
 
 				return x;
 			}
+			Dynamic::Matrix<value_type> inverse() const {
+				assert (mIsFactorized);
+				assert (0 && !"Not properly tested!");
+
+				VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
+				MatrixXXd result (mQ.cols(), mQ.cols());
+
+				for (unsigned int i = 0; i < mQ.cols(); i++) {
+					rhs_temp[i] = 1.;
+
+					result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
+
+					rhs_temp[i] = 0.;
+				}
+
+				return result;
+			}
+
 			Dynamic::Matrix<value_type> matrixQ () const {
 				return mQ;
 			}

File src/SimpleMath/compileassert.h

 #define JOIN( X, Y ) JOIN2(X,Y)
 #define JOIN2( X, Y ) X##Y
 
-namespace static_assert
+namespace static_assert_compat
 {
     template <bool> struct STATIC_ASSERT_FAILURE;
     template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
 }
 
 #define COMPILE_ASSERT(x) \
-    typedef ::static_assert::static_assert_test<\
-        sizeof(::static_assert::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
+    typedef ::static_assert_compat::static_assert_test<\
+        sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
             JOIN(_static_assert_typedef, __LINE__)
 
 #else // __cplusplus

File utils/matlab/ZYXEulerToMatrix.m

-function [M] = ZYXEulerToMatrix (e)
+function [Rx, Ry, Rz, M] = ZYXEulerToMatrix (e)
 %
 % Calculates the rotation matrix from ZYX Euler Angles
 %