Initial commit.

.gitignore

@@ -0,0 +1 @@
+build*/

CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8)
+
+project(matrix CXX)
+
+list(APPEND CMAKE_CXX_FLAGS
+	-Wall
+	-Weffc++
+	-Wfatal-errors
+	)
+
+string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+
+enable_testing()
+
+include_directories(matrix)
+
+add_subdirectory(test)

LICENSE

@@ -0,0 +1,28 @@
+Copyright (c) 2015, James Goppert
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of ocr nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+

matrix/Dcm.hpp

@@ -0,0 +1,64 @@
+/**
+ * @file Dcm.hpp
+ *
+ * A direction cosine matrix class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include <Matrix.hpp>
+#include <Quaternion.hpp>
+#include <Euler.hpp>
+
+namespace matrix
+{
+
+template<typename Type>
+class Quaternion;
+
+template<typename Type>
+class Dcm : public Matrix<Type, 3, 3>
+{
+public:
+	virtual ~Dcm() {};
+
+	typedef Matrix<Type, 3, 1> Vector3;
+
+	Dcm() : Matrix<Type, 3, 3>()
+	{
+	}
+
+	Dcm(const Quaternion<Type> & q) {
+		// TODO
+		Dcm &c = *this;
+		c(0, 0) = 0;
+		c(0, 1) = 0;
+		c(0, 2) = 0;
+		c(1, 0) = 0;
+		c(1, 1) = 0;
+		c(1, 2) = 0;
+		c(2, 0) = 0;
+		c(2, 1) = 0;
+		c(2, 2) = 0;
+	}
+
+	Dcm(const Euler<Type> & e) {
+		// TODO
+		Dcm &c = *this;
+		c(0, 0) = 0;
+		c(0, 1) = 0;
+		c(0, 2) = 0;
+		c(1, 0) = 0;
+		c(1, 1) = 0;
+		c(1, 2) = 0;
+		c(2, 0) = 0;
+		c(2, 1) = 0;
+		c(2, 2) = 0;
+	}
+};
+
+typedef Dcm<float> Dcmf;
+
+}; // namespace matrix

matrix/Euler.hpp

@@ -0,0 +1,61 @@
+/**
+ * @file Euler.hpp
+ *
+ * Euler angle tait-bryan body 3-2-1
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+#include <Vector.hpp>
+#include <Dcm.hpp>
+#include <Quaternion.hpp>
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Quaternion;
+
+template<typename Type>
+class Euler : public Vector<Type, 3>
+{
+public:
+	virtual ~Euler() {};
+
+	Euler() : Vector<Type, 3>()
+	{
+	}
+
+	Euler(Type roll, Type pitch, Type yaw) : Vector<Type, 3>()
+	{
+		Euler &v(*this);
+		v(0) = roll;
+		v(1) = pitch;
+		v(2) = yaw;
+	}
+
+	Euler(const Dcm<Type> & dcm) {
+		// TODO
+		Euler &e = *this;
+		e(0) = 0;
+		e(1) = 0;
+		e(2) = 0;
+	}
+
+	Euler(const Quaternion<Type> & q) {
+		// TODO
+		Euler &e = *this;
+		e(0) = 0;
+		e(1) = 0;
+		e(2) = 0;
+	}
+
+};
+
+typedef Euler<float> Eulerf;
+
+}; // namespace matrix

matrix/Matrix.hpp

@@ -0,0 +1,472 @@
+/**
+ * @file Matrix.hpp
+ *
+ * A simple matrix template library.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+namespace matrix
+{
+
+template<typename Type, size_t  M, size_t N>
+class Matrix
+{
+
+protected:
+	Type _data[M][N];
+	size_t _rows;
+	size_t _cols;
+
+public:
+
+	virtual ~Matrix() {};
+
+	Matrix() :
+		_data(),
+		_rows(M),
+		_cols(N)
+	{
+	}
+
+	Matrix(const Type *data) :
+		_data(),
+		_rows(M),
+		_cols(N)
+	{
+		memcpy(_data, data, sizeof(_data));
+	}
+
+	Matrix(const Matrix &other) :
+		_data(),
+		_rows(M),
+		_cols(N)
+	{
+		memcpy(_data, other._data, sizeof(_data));
+	}
+
+	/**
+	 * Accessors/ Assignment etc.
+	 */
+
+	Type *data()
+	{
+		return _data[0];
+	}
+
+	inline size_t rows() const
+	{
+		return _rows;
+	}
+
+	inline size_t cols() const
+	{
+		return _rows;
+	}
+
+	inline Type operator()(size_t i, size_t j) const
+	{
+		return _data[i][j];
+	}
+
+	inline Type &operator()(size_t i, size_t j)
+	{
+		return _data[i][j];
+	}
+
+	/**
+	 * Matrix Operations
+	 */
+
+	// this might use a lot of programming memory
+	// since it instantiates a class for every
+	// required mult pair, but it provides
+	// compile time size_t checking
+	template<size_t P>
+	Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &other) const
+	{
+		const Matrix<Type, M, N> &self = *this;
+		Matrix<Type, M, P> res;
+		res.setZero();
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t k = 0; k < P; k++) {
+				for (size_t j = 0; j < N; j++) {
+					res(i, k) += self(i, j) * other(j, k);
+				}
+			}
+		}
+
+		return res;
+	}
+
+	Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
+	{
+		Matrix<Type, M, N> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				res(i , j) = self(i, j) + other(i, j);
+			}
+		}
+
+		return res;
+	}
+
+	bool operator==(const Matrix<Type, M, N> &other) const
+	{
+		Matrix<Type, M, N> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				if (self(i , j) != other(i, j)) {
+					return false;
+				}
+			}
+		}
+
+		return true;
+	}
+
+	operator Type();
+
+	Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
+	{
+		Matrix<Type, M, N> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				res(i , j) = self(i, j) - other(i, j);
+			}
+		}
+
+		return res;
+	}
+
+	void operator+=(const Matrix<Type, M, N> &other)
+	{
+		Matrix<Type, M, N> &self = *this;
+		self = self + other;
+	}
+
+	void operator-=(const Matrix<Type, M, N> &other)
+	{
+		Matrix<Type, M, N> &self = *this;
+		self = self - other;
+	}
+
+	void operator*=(const Matrix<Type, M, N> &other)
+	{
+		Matrix<Type, M, N> &self = *this;
+		self = self * other;
+	}
+
+	/**
+	 * Scalar Operations
+	 */
+
+	Matrix<Type, M, N> operator*(Type scalar) const
+	{
+		Matrix<Type, M, N> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				res(i , j) = self(i, j) * scalar;
+			}
+		}
+
+		return res;
+	}
+
+	Matrix<Type, M, N> operator+(Type scalar) const
+	{
+		Matrix<Type, M, N> res;
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				res(i , j) = self(i, j) + scalar;
+			}
+		}
+
+		return res;
+	}
+
+	void operator*=(Type scalar)
+	{
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				self(i, j) = self(i, j) * scalar;
+			}
+		}
+	}
+
+	void operator/=(Type scalar)
+	{
+		Matrix<Type, M, N> &self = *this;
+		self = self * (1.0f / scalar);
+	}
+
+	/**
+	 * Misc. Functions
+	 */
+
+	void print()
+	{
+		Matrix<Type, M, N> &self = *this;
+		printf("\n");
+
+		for (size_t i = 0; i < M; i++) {
+			printf("[");
+
+			for (size_t j = 0; j < N; j++) {
+				printf("%10g\t", double(self(i, j)));
+			}
+
+			printf("]\n");
+		}
+	}
+
+	Matrix<Type, N, M> transpose() const
+	{
+		Matrix<Type, N, M> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				res(j, i) = self(i, j);
+			}
+		}
+
+		return res;
+	}
+
+
+	// tranpose alias
+	inline Matrix<Type, N, M> T() const
+	{
+		return transpose();
+	}
+
+	Matrix<Type, M, M> expm(float dt, size_t n) const
+	{
+		Matrix<float, M, M> res;
+		res.setIdentity();
+		Matrix<float, M, M> A_pow = *this;
+		float k_fact = 1;
+		size_t k = 1;
+
+		while (k < n) {
+			res += A_pow * (float(pow(dt, k)) / k_fact);
+
+			if (k == n) { break; }
+
+			A_pow *= A_pow;
+			k_fact *= k;
+			k++;
+		}
+
+		return res;
+	}
+
+	Matrix<Type, M, 1> diagonal() const
+	{
+		Matrix<Type, M, 1> res;
+		// force square for now
+		const Matrix<Type, M, M> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			res(i) = self(i, i);
+		}
+
+		return res;
+	}
+
+	void setZero()
+	{
+		memset(_data, 0, sizeof(_data));
+	}
+
+	void setIdentity()
+	{
+		setZero();
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M and i < N; i++) {
+			self(i, i) = 1;
+		}
+	}
+
+	inline void swapRows(size_t a, size_t b)
+	{
+		if (a == b) { return; }
+
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t j = 0; j < cols(); j++) {
+			Type tmp = self(a, j);
+			self(a, j) = self(b, j);
+			self(b, j) = tmp;
+		}
+	}
+
+	inline void swapCols(size_t a, size_t b)
+	{
+		if (a == b) { return; }
+
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < rows(); i++) {
+			Type tmp = self(i, a);
+			self(i, a) = self(i, b);
+			self(i, b) = tmp;
+		}
+	}
+
+	/**
+	 * inverse based on LU factorization with partial pivotting
+	 */
+	Matrix <Type, M, M> inverse() const
+	{
+		Matrix<Type, M, M> L;
+		L.setIdentity();
+		const Matrix<Type, M, M> &A = (*this);
+		Matrix<Type, M, M> U = A;
+		Matrix<Type, M, M> P;
+		P.setIdentity();
+
+		//printf("A:\n"); A.print();
+
+		// for all diagonal elements
+		for (size_t n = 0; n < N; n++) {
+
+			// if diagonal is zero, swap with row below
+			if (fabsf(U(n, n)) < 1e-8f) {
+				//printf("trying pivot for row %d\n",n);
+				for (size_t i = 0; i < N; i++) {
+					if (i == n) { continue; }
+
+					//printf("\ttrying row %d\n",i);
+					if (fabsf(U(i, n)) > 1e-8f) {
+						//printf("swapped %d\n",i);
+						U.swapRows(i, n);
+						P.swapRows(i, n);
+					}
+				}
+			}
+
+#ifdef MATRIX_ASSERT
+			//printf("A:\n"); A.print();
+			//printf("U:\n"); U.print();
+			//printf("P:\n"); P.print();
+			//fflush(stdout);
+			ASSERT(fabsf(U(n, n)) > 1e-8f);
+#endif
+
+			// failsafe, return zero matrix
+			if (fabsf(U(n, n)) < 1e-8f) {
+				Matrix<Type, M, M> zero;
+				zero.setZero();
+				return zero;
+			}
+
+			// for all rows below diagonal
+			for (size_t i = (n + 1); i < N; i++) {
+				L(i, n) = U(i, n) / U(n, n);
+
+				// add i-th row and n-th row
+				// multiplied by: -a(i,n)/a(n,n)
+				for (size_t k = n; k < N; k++) {
+					U(i, k) -= L(i, n) * U(n, k);
+				}
+			}
+		}
+
+		//printf("L:\n"); L.print();
+		//printf("U:\n"); U.print();
+
+		// solve LY=P*I for Y by forward subst
+		Matrix<Type, M, M> Y = P;
+
+		// for all columns of Y
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of L
+			for (size_t i = 0; i < N; i++) {
+				// for all columns of L
+				for (size_t j = 0; j < i; j++) {
+					// for all existing y
+					// subtract the component they
+					// contribute to the solution
+					Y(i, c) -= L(i, j) * Y(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				// Y(i,c) /= L(i,i);
+				// but L(i,i) = 1.0
+			}
+		}
+
+		//printf("Y:\n"); Y.print();
+
+		// solve Ux=y for x by back subst
+		Matrix<Type, M, M> X = Y;
+
+		// for all columns of X
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of U
+			for (size_t k = 0; k < N; k++) {
+				// have to go in reverse order
+				size_t i = N - 1 - k;
+
+				// for all columns of U
+				for (size_t j = i + 1; j < N; j++) {
+					// for all existing x
+					// subtract the component they
+					// contribute to the solution
+					X(i, c) -= U(i, j) * X(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				X(i, c) /= U(i, i);
+			}
+		}
+
+		//printf("X:\n"); X.print();
+		return X;
+	}
+
+	// inverse alias
+	inline Matrix<Type, N, M> I() const
+	{
+		return inverse();
+	}
+
+};
+
+template <>
+Matrix<float,1,1>::operator float()
+{
+	return (*this)(0,0);
+}
+
+typedef Matrix<float, 3,3> Matrix3f;
+
+}; // namespace matrix

matrix/Quaternion.hpp

@@ -0,0 +1,89 @@
+/**
+ * @file Matrix.hpp
+ *
+ * A quaternion class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+#include <Vector.hpp>
+#include <Dcm.hpp>
+#include <Euler.hpp>
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Euler;
+
+template<typename Type>
+class Quaternion : public Vector<Type, 4>
+{
+public:
+	virtual ~Quaternion() {};
+
+	Quaternion() : Vector<Type, 4>()
+	{
+		// TODO
+		Quaternion &q = *this;
+		q(0) = 1;
+		q(1) = 0;
+		q(2) = 0;
+		q(3) = 0;
+	}
+
+	Quaternion(const Dcm<Type> & dcm) {
+		// TODO
+		Quaternion &q = *this;
+		q(0) = 0;
+		q(1) = 0;
+		q(2) = 0;
+		q(3) = 0;
+	}
+
+	Quaternion(const Euler<Type> & e) {
+		// TODO
+		Quaternion &q = *this;
+		q(0) = 0;
+		q(1) = 0;
+		q(2) = 0;
+		q(3) = 0;
+	}
+
+	Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
+	{
+		// TODO
+		Quaternion &q = *this;
+		q(0) = a;
+		q(1) = b;
+		q(2) = c;
+		q(3) = d;
+	}
+
+	Quaternion operator*(const Quaternion &q) const
+	{
+		// TODO
+		const Quaternion &p = *this;
+		Quaternion r;
+		r(0) = 0;
+		r(1) = 0;
+		r(2) = 0;
+		r(3) = 0;
+		return r;
+	}
+
+	Matrix<Type, 4, 1> derivative() const {
+		// TODO
+		Matrix<Type, 4, 1> d;
+		return d;
+	}
+
+};
+
+typedef Quaternion<float> Quatf;
+
+}; // namespace matrix

matrix/Vector.hpp

@@ -0,0 +1,52 @@
+/**
+ * @file Vector.hpp
+ *
+ * Vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+#include <Matrix.hpp>
+
+namespace matrix
+{
+
+template<typename Type, size_t N>
+class Vector : public Matrix<Type, N, 1>
+{
+public:
+	virtual ~Vector() {};
+
+	Vector() : Matrix<Type, N, 1>()
+	{
+	}
+
+	inline Type operator()(size_t i) const
+	{
+		const Matrix<Type, N, 1> &v = *this;
+		return v(i, 0);
+	}
+
+	inline Type &operator()(size_t i)
+	{
+		Matrix<Type, N, 1> &v = *this;
+		return v(i, 0);
+	}
+
+	Type dot(const Vector & b) {
+		Vector &a(*this);
+		Type r = 0;
+		for (int i = 0; i<3; i++) {
+			r += a(i)*b(i);
+		}
+		return r;
+	}
+
+	Type norm() {
+		Vector &a(*this);
+		return sqrt(a.dot(a));
+	}
+};
+
+}; // namespace matrix

matrix/Vector3.hpp

@@ -0,0 +1,46 @@
+/**
+ * @file Vector3.hpp
+ *
+ * 3D vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+#include <Vector.hpp>
+
+namespace matrix
+{
+
+template<typename Type>
+class Vector3 : public Vector<Type, 3>
+{
+public:
+	virtual ~Vector3() {};
+
+	Vector3() : Vector<Type, 3>()
+	{
+	}
+
+	Vector3(Type x, Type y, Type z) : Vector<Type, 3>()
+	{
+		Vector3 &v(*this);
+		v(0) = x;
+		v(1) = y;
+		v(2) = z;
+	}
+
+	Vector3 cross(const Vector3 & b) {
+		// TODO
+		Vector3 &a(*this);
+		Vector3 c;
+		c(0) = 0;
+		c(1) = 0;
+		c(2) = 0;
+		return c;
+	}
+};
+
+typedef Vector3<float> Vector3f;
+
+}; // namespace matrix

test/CMakeLists.txt

@@ -0,0 +1,20 @@
+set(tests
+	setIdentity
+	inverse
+	matrixMult
+	vectorAssignment
+	matrixAssignment
+	matrixScalarMult
+	transpose
+	quaternion
+	dcm
+	vector
+	vector3
+	euler
+	)
+
+foreach(test ${tests})
+	add_executable(${test}
+		${test}.cpp)
+	add_test(${test} ${test})
+endforeach()

test/dcm.cpp

@@ -0,0 +1,13 @@
+#include "Dcm.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	Dcmf dcm;
+	Quatf q = Quatf(dcm);
+	Eulerf e = Eulerf(dcm);
+	return 0;
+}

test/euler.cpp

@@ -0,0 +1,15 @@
+#include "Euler.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	Eulerf e;
+	float dp = e.T()*e;
+	Dcmf dcm = Dcmf(e);
+	Quatf q = Quatf(e);
+	float n = e.norm();
+	return 0;
+}

test/inverse.cpp

@@ -0,0 +1,30 @@
+#include "Matrix.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+	Matrix3f A(data);
+	Matrix3f A_I = A.inverse();
+	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+	Matrix3f A_I_check(data_check);
+	(void)A_I;
+	assert(A_I == A_I_check);
+
+	// stess test
+	static const size_t n = 100;
+	Matrix<float, n, n> A_large;
+	A_large.setIdentity();
+	Matrix<float, n, n> A_large_I;
+	A_large_I.setZero();
+
+	for (size_t i = 0; i < 100; i++) {
+		A_large_I = A_large.inverse();
+		assert(A_large == A_large_I);
+	}
+
+	return 0;
+}

test/matrixAssignment.cpp

@@ -0,0 +1,29 @@
+#include "Matrix.hpp"
+#include <assert.h>
+
+using namespace matrix;
+
+int main()
+{
+	Matrix3f m;
+	m.setZero();
+	m(0, 0) = 1;
+	m(0, 1) = 2;
+	m(0, 2) = 3;
+	m(1, 0) = 4;
+	m(1, 1) = 5;
+	m(1, 2) = 6;
+	m(2, 0) = 7;
+	m(2, 1) = 8;
+	m(2, 2) = 9;
+
+	m.print();
+
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f m2(data);
+	m2.print();
+
+	assert(m == m2);
+
+	return 0;
+}

test/matrixMult.cpp

@@ -0,0 +1,26 @@
+#include "Matrix.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+	Matrix3f A(data);
+	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+	Matrix3f A_I(data_check);
+	Matrix3f I;
+	I.setIdentity();
+	A.print();
+	A_I.print();
+	Matrix3f R = A * A_I;
+	R.print();
+	assert(R == I);
+
+	Matrix3f R2 = A;
+	R2 *= A_I;
+	R2.print();
+	assert(R2 == I);
+	return 0;
+}

test/matrixScalarMult.cpp

@@ -0,0 +1,18 @@
+#include "Matrix.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f A(data);
+	A = A * 2;
+	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	Matrix3f A_check(data_check);
+	A.print();
+	A_check.print();
+	assert(A == A_check);
+	return 0;
+}

test/quaternion.cpp

@@ -0,0 +1,15 @@
+#include "Quaternion.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	Quatf p(1, 0, 0, 0);
+	Quatf q(0, 1, 0, 0);
+	Quatf r = p*q;
+	Dcmf dcm = Dcmf(p);
+	Eulerf e = Eulerf(p);
+	return 0;
+}

test/setIdentity.cpp

@@ -0,0 +1,25 @@
+#include "Matrix.hpp"
+#include <assert.h>
+
+using namespace matrix;
+
+int main()
+{
+	Matrix3f A;
+	A.setIdentity();
+	assert(A.rows() == 3);
+	assert(A.cols() == 3);
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			if (i == j) {
+				assert(A(i, j) == 1);
+
+			} else {
+				assert(A(i, j) == 0);
+			}
+		}
+	}
+
+	return 0;
+}

test/transpose.cpp

@@ -0,0 +1,18 @@
+#include "Matrix.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	float data[9] = {1, 2, 3, 4, 5, 6};
+	Matrix<float, 2, 3> A(data);
+	Matrix<float, 3, 2> A_T = A.transpose();
+	float data_check[9] = {1, 4, 2, 5, 3, 6};
+	Matrix<float, 3, 2> A_T_check(data_check);
+	A_T.print();
+	A_T_check.print();
+	assert(A_T == A_T_check);
+	return 0;
+}

test/vector.cpp

@@ -0,0 +1,13 @@
+#include "Vector.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	Vector<float, 5> v;
+	float n = v.norm();
+	float r = v.dot(v);
+	return 0;
+}

test/vector3.cpp

@@ -0,0 +1,13 @@
+#include "Vector3.hpp"
+#include <assert.h>
+#include <stdio.h>
+
+using namespace matrix;
+
+int main()
+{
+	Vector3f a(1, 0, 0);
+	Vector3f b(0, 1, 0);
+	Vector3f c = a.cross(b);
+	return 0;
+}

test/vectorAssignment.cpp

@@ -0,0 +1,28 @@
+#include "Vector3.hpp"
+#include <assert.h>
+
+using namespace matrix;
+
+int main()
+{
+	Vector3f v;
+	v(0) = 1;
+	v(1) = 2;
+	v(2) = 3;
+
+	v.print();
+
+	assert(v(0) == 1);
+	assert(v(1) == 2);
+	assert(v(2) == 3);
+
+	Vector3f v2(4, 5, 6);
+
+	v2.print();
+
+	assert(v2(0) == 4);
+	assert(v2(1) == 5);
+	assert(v2(2) == 6);
+
+	return 0;
+}