X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flinal%2Fsquarematrix.h;h=1cdc56726455f9ab2b97589b1b93bacbad702eac;hb=643aa7b2317f88463f66da11e595ebe0f6c9621d;hp=6cda86a5baa6b202ec00dba93acb0a152b03a985;hpb=839a88cd2b9a2f054323627c4e76cdcefc652c63;p=libs%2Fmath.git

diff --git a/source/linal/squarematrix.h b/source/linal/squarematrix.h
index 6cda86a..1cdc567 100644
--- a/source/linal/squarematrix.h
+++ b/source/linal/squarematrix.h
@@ -1,26 +1,103 @@
 #ifndef MSP_LINAL_SQUAREMATRIX_H_
 #define MSP_LINAL_SQUAREMATRIX_H_
 
+#include <cmath>
+#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() { }
+};
+
+/**
+A mathematical matrix with S rows and columns.  Some operations are provided
+here that are only possible for square matrices.
+*/
 template<typename T, unsigned S>
 class SquareMatrix: public Matrix<T, S, S>
 {
 public:
-	SquareMatrix();
-	SquareMatrix(const T *);
+	SquareMatrix() { }
+	SquareMatrix(const T *d): Matrix<T, S, S>(d) { }
 	template<typename U>
-	SquareMatrix(const Matrix<U, S, S> &);
+	SquareMatrix(const Matrix<U, S, S> &m): Matrix<T, S, S>(m) { }
+
 	static SquareMatrix identity();
 
 	SquareMatrix &operator*=(const SquareMatrix &);
 
-	void invert();
+	SquareMatrix &invert();
 };
 
+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)
+{
+	return *this = *this*m;
+}
+
+template<typename T, unsigned S>
+SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
+{
+	using std::abs;
+
+	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