From a3d9772a9fd483278b6248a811dc6f4ce968892b Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Tue, 14 May 2013 00:11:38 +0300
Subject: [PATCH] Another big batch of stuff

This includes most of the missing things.
---
 source/linal/matrix.h       | 139 ++++++++++++++++++++++++++++++------
 source/linal/squarematrix.h |  86 +++++++++++++++++++++-
 source/linal/vector.h       |   9 +++
 source/linal/vector3.h      |   8 +--
 4 files changed, 216 insertions(+), 26 deletions(-)

diff --git a/source/linal/matrix.h b/source/linal/matrix.h
index 4fc2bc6..94a6e09 100644
--- a/source/linal/matrix.h
+++ b/source/linal/matrix.h
@@ -1,6 +1,7 @@
 #ifndef MSP_LINAL_MATRIX_H_
 #define MSP_LINAL_MATRIX_H_
 
+#include <algorithm>
 #include "vector.h"
 
 namespace Msp {
@@ -20,9 +21,12 @@ public:
 	Matrix(const T *);
 	template<typename U>
 	Matrix(const Matrix<U, M, N> &);
+
 	static Matrix from_columns(const Vector<T, M> *);
 	static Matrix from_rows(const Vector<T, N> *);
 
+	T &element(unsigned, unsigned);
+	const T &element(unsigned, unsigned) const;
 	T &operator()(unsigned, unsigned);
 	const T &operator()(unsigned, unsigned) const;
 
@@ -31,21 +35,72 @@ public:
 	Matrix &operator+=(const Matrix &);
 	Matrix &operator-=(const Matrix &);
 
-	Matrix<T, N, M> transpose() const;
+	Matrix &exchange_rows(unsigned, unsigned);
+	Matrix &multiply_row(unsigned, T);
+	Matrix &add_row(unsigned, unsigned, T);
 };
 
 template<typename T, unsigned M, unsigned N>
-inline T &Matrix<T, M, N>::operator()(unsigned i, unsigned j)
+inline Matrix<T, M, N>::Matrix()
+{
+	std::fill(data, data+M*N, T());
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Matrix<T, M, N>::Matrix(const T *d)
+{
+	std::copy(d, d+M*N, data);
+}
+
+template<typename T, unsigned M, unsigned N>
+template<typename U>
+inline Matrix<T, M, N>::Matrix(const Matrix<U, M, N> &m)
+{
+	std::copy(m.data, m.data+M*N, data);
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Matrix<T, M, N> Matrix<T, M, N>::from_columns(const Vector<T, M> *v)
+{
+	Matrix<T, M, N> m;
+	for(unsigned i=0; i<M; ++i)
+		for(unsigned j=0; j<N; ++j)
+			m(i, j) = v[j][i];
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Matrix<T, M, N> Matrix<T, M, N>::from_rows(const Vector<T, N> *v)
+{
+	Matrix<T, M, N> m;
+	for(unsigned i=0; i<M; ++i)
+		for(unsigned j=0; j<N; ++j)
+			m(i, j) = v[i][j];
+}
+
+template<typename T, unsigned M, unsigned N>
+inline T &Matrix<T, M, N>::element(unsigned i, unsigned j)
 {
 	return data[i+M*j];
 }
 
 template<typename T, unsigned M, unsigned N>
-inline const T &Matrix<T, M, N>::operator()(unsigned i, unsigned j) const
+inline const T &Matrix<T, M, N>::element(unsigned i, unsigned j) const
 {
 	return data[i+M*j];
 }
 
+template<typename T, unsigned M, unsigned N>
+inline T &Matrix<T, M, N>::operator()(unsigned i, unsigned j)
+{
+	return element(i, j);
+}
+
+template<typename T, unsigned M, unsigned N>
+inline const T &Matrix<T, M, N>::operator()(unsigned i, unsigned j) const
+{
+	return element(i, j);
+}
+
 template<typename T, unsigned M, unsigned N>
 inline Matrix<T, M, N> &Matrix<T, M, N>::operator*=(T s)
 {
@@ -67,6 +122,37 @@ inline Matrix<T, M, N> operator*(T s, const Matrix<T, M, N> &m)
 	return m*s;
 }
 
+template<typename T, unsigned M, unsigned P, unsigned N>
+inline Matrix<T, M, N> operator*(const Matrix<T, M, P> &m1, const Matrix<T, P, N> &m2)
+{
+	Matrix<T, M, N> r;
+	for(unsigned i=0; i<M; ++i)
+		for(unsigned j=0; j<N; ++j)
+			for(unsigned k=0; k<P; ++k)
+				r(i, j) += m1(i, k)*m2(k, j);
+	return r;
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Vector<T, M> operator*(const Matrix<T, M, N> &m, const Vector<T, N> &v)
+{
+	Vector<T, M> r;
+	for(unsigned i=0; i<M; ++i)
+		for(unsigned j=0; j<N; ++j)
+			r[i] += m(i, j)*v[j];
+	return r;
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Vector<T, N> operator*(const Vector<T, M> &v, const Matrix<T, M, N> &m)
+{
+	Vector<T, N> r;
+	for(unsigned j=0; j<N; ++j)
+		for(unsigned i=0; i<M; ++i)
+			r[j] += v[i]*m(i, j);
+	return r;
+}
+
 template<typename T, unsigned M, unsigned N>
 inline Matrix<T, M, N> &Matrix<T, M, N>::operator/=(T s)
 {
@@ -112,34 +198,47 @@ inline Matrix<T, M, N> operator-(const Matrix<T, M, N> &m1, const Matrix<T, M, N
 	return r -= m2;
 }
 
-template<typename T, unsigned M, unsigned P, unsigned N>
-Matrix<T, M, N> operator*(const Matrix<T, M, P> &m1, const Matrix<T, P, N> &m2)
+template<typename T, unsigned M, unsigned N>
+inline bool operator==(const Matrix<T, M, N> &a, const Matrix<T, M, N> &b)
 {
-	Matrix<T, M, N> r;
-	for(unsigned i=0; i<M; ++i)
-		for(unsigned j=0; j<N; ++j)
-			for(unsigned k=0; k<P; ++k)
-				r.at(i, j) += m1(i, k)*m2(k, j);
-	return r;
+	for(unsigned j=0; j<N; ++j)
+		for(unsigned i=0; i<M; ++i)
+			if(a(i, j)!=b(i, j))
+				return false;
+	return true;
 }
 
 template<typename T, unsigned M, unsigned N>
-Vector<T, M> operator*(const Matrix<T, M, N> &m, const Vector<T, N> &v)
+inline Matrix<T, M, N> &Matrix<T, M, N>::exchange_rows(unsigned i, unsigned j)
 {
-	Vector<T, M> r;
-	for(unsigned i=0; i<M; ++i)
-		for(unsigned j=0; j<N; ++j)
-			r[i] += m(i, j)*v[j];
-	return r;
+	for(unsigned k=0; k<N; ++k)
+		std::swap(element(i, k), element(j, k));
+	return *this;
 }
 
 template<typename T, unsigned M, unsigned N>
-Vector<T, N> operator*(const Vector<T, M> &v, const Matrix<T, M, N> &m)
+inline Matrix<T, M, N> &Matrix<T, M, N>::multiply_row(unsigned i, T s)
 {
-	Vector<T, N> r;
+	for(unsigned k=0; k<N; ++k)
+		element(i, k) *= s;
+	return *this;
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Matrix<T, M, N> &Matrix<T, M, N>::add_row(unsigned i, unsigned j, T s)
+{
+	for(unsigned k=0; k<N; ++k)
+		element(j, k) += element(i, k)*s;
+	return *this;
+}
+
+template<typename T, unsigned M, unsigned N>
+inline Matrix<T, N, M> transpose(const Matrix<T, M, N> &m)
+{
+	Matrix<T, N, M> r;
 	for(unsigned j=0; j<N; ++j)
 		for(unsigned i=0; i<M; ++i)
-			r[j] += v[i]*m(i, j);
+			r(j, i) = m(i, j);
 	return r;
 }
 
diff --git a/source/linal/squarematrix.h b/source/linal/squarematrix.h
index 6cda86a..4cdc9e8 100644
--- a/source/linal/squarematrix.h
+++ b/source/linal/squarematrix.h
@@ -1,26 +1,108 @@
 #ifndef MSP_LINAL_SQUAREMATRIX_H_
 #define MSP_LINAL_SQUAREMATRIX_H_
 
+#include <stdexcept>
 #include "matrix.h"
 
 namespace Msp {
 namespace LinAl {
 
+class not_invertible: public std::domain_error
+{
+public:
+	not_invertible(): domain_error(std::string()) { }
+	virtual ~not_invertible() throw() { }
+};
+
 template<typename T, unsigned S>
 class SquareMatrix: public Matrix<T, S, S>
 {
 public:
-	SquareMatrix();
+	SquareMatrix() { }
 	SquareMatrix(const T *);
 	template<typename U>
 	SquareMatrix(const Matrix<U, S, S> &);
+
 	static SquareMatrix identity();
 
 	SquareMatrix &operator*=(const SquareMatrix &);
 
-	void invert();
+	SquareMatrix &invert();
 };
 
+template<typename T, unsigned S>
+SquareMatrix<T, S>::SquareMatrix(const T *d):
+	Matrix<T, S, S>(d)
+{ }
+
+template<typename T, unsigned S>
+template<typename U>
+SquareMatrix<T, S>::SquareMatrix(const Matrix<U, S, S> &m):
+	Matrix<T, S, S>(m)
+{ }
+
+template<typename T, unsigned S>
+inline SquareMatrix<T, S> SquareMatrix<T, S>::identity()
+{
+	SquareMatrix<T, S> m;
+	for(unsigned i=0; i<S; ++i)
+		m(i, i) = T(1);
+	return m;
+}
+
+template<typename T, unsigned S>
+SquareMatrix<T, S> &SquareMatrix<T, S>::operator*=(const SquareMatrix<T, S> &m)
+{
+	Matrix<T, S, S>::operator*=(m);
+	return *this;
+}
+
+template<typename T, unsigned S>
+SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
+{
+	SquareMatrix<T, S> r = identity();
+	for(unsigned i=0; i<S; ++i)
+	{
+		if(this->element(i, i)==T(0))
+		{
+			unsigned pivot = i;
+			for(unsigned j=i+1; j<S; ++j)
+				if(abs(this->element(j, i))>abs(this->element(pivot, i)))
+					pivot = j;
+
+			if(pivot==i)
+				throw not_invertible();
+
+			this->exchange_rows(i, pivot);
+			r.exchange_rows(i, pivot);
+		}
+
+		for(unsigned j=i+1; j<S; ++j)
+		{
+			T a = -this->element(j, i)/this->element(i, i);
+			this->add_row(i, j, a);
+			r.add_row(i, j, a);
+		}
+
+		T a = T(1)/this->element(i, i);
+		this->multiply_row(i, a);
+		r.multiply_row(i, a);
+	}
+
+	for(unsigned i=S; i-->0; )
+		for(unsigned j=i; j-->0; )
+			r.add_row(i, j, -this->element(j, i));
+
+	return *this = r;
+}
+
+template<typename T, unsigned S>
+inline SquareMatrix<T, S> invert(const SquareMatrix<T, S> &m)
+{
+	SquareMatrix<T, S> r = m;
+	return r.invert();
+}
+
 } // namespace LinAl
 } // namespace Msp
 
diff --git a/source/linal/vector.h b/source/linal/vector.h
index 933611e..1a79bd8 100644
--- a/source/linal/vector.h
+++ b/source/linal/vector.h
@@ -141,6 +141,15 @@ inline Vector<T, N> operator-(const Vector<T, N> &v)
 	return r;
 }
 
+template<typename T, unsigned N>
+inline bool operator==(const Vector<T, N> &v, const Vector<T, N> &w)
+{
+	for(unsigned i=0; i<N; ++i)
+		if(v[i]!=w[i])
+			return false;
+	return true;
+}
+
 template<typename T, unsigned N>
 inline T inner_product(const Vector<T, N> &v1, const Vector<T, N> &v2)
 {
diff --git a/source/linal/vector3.h b/source/linal/vector3.h
index 90a5ff5..0197dbc 100644
--- a/source/linal/vector3.h
+++ b/source/linal/vector3.h
@@ -40,18 +40,18 @@ inline Vector3<T>::Vector3(const Vector<U, 3> &v):
 { }
 
 template<typename T>
-inline T dot(const Vector3<T> &v1, const Vector3<T> &v2)
+inline T dot(const Vector<T, 3> &v1, const Vector<T, 3> &v2)
 {
 	return inner_product(v1, v2);
 }
 
 template<typename T>
-inline Vector3<T> cross(const Vector3<T> &v1, const Vector3<T> &v2)
+inline Vector<T, 3> cross(const Vector<T, 3> &v1, const Vector<T, 3> &v2)
 {
-	return Vector3<T>(v1[1]*v2[2]-v1[2]*v2[1], v1[2]*v2[0]-v1[0]*v2[2], v1[0]*v2[1]-v1[1]*v2[0]);
+	return Vector<T, 3>(v1[1]*v2[2]-v1[2]*v2[1], v1[2]*v2[0]-v1[0]*v2[2], v1[0]*v2[1]-v1[1]*v2[0]);
 }
 
 } // namespace LinAl
-} // namespace LinAl
+} // namespace Msp
 
 #endif
-- 
2.43.0