From 0b3a6c5473041768341e409ce041e5c988d77879 Mon Sep 17 00:00:00 2001
From: Jiri Pittner <jiri@pittnerovi.com>
Date: Sun, 21 Nov 2021 00:08:09 +0100
Subject: [PATCH] gauss elimination for vecmat3

---
 t.cc       | 37 ++++++++++++++++++++++++++++-
 vecmat3.cc | 15 +++++++++++-
 vecmat3.h  | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 117 insertions(+), 3 deletions(-)

diff --git a/t.cc b/t.cc
index 27e2ed3..bf89030 100644
--- a/t.cc
+++ b/t.cc
@@ -2321,7 +2321,7 @@ Polynomial<double> pp({1,2,3,4,5});
 cout<<pp;
 }
 
-if(1)
+if(0)
 {
 //prepare random symmetric mat3
 int seed;
@@ -2352,4 +2352,39 @@ cout<<"eival error = "<<(w-www).norm()<<endl;
 cout<<"eivec error = "<<(m.diffabs(vvv)).norm()<<endl; //just ignore signs due to arb. phases (not full check)
 }
 
+if(1)
+{
+//prepare random  mat3
+int seed;
+int f=open("/dev/random",O_RDONLY);
+if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random");
+close(f);
+srand(seed);
+
+NRMat<double> tmp(3,3);
+tmp.randomize(2.);
+Mat3<double> mm(tmp);
+NRMat<double> m(&mm[0][0],3,3);
+cout <<m<<"3 3\n"<<mm<<endl;
+
+double rr[2][3]={{1,2,3},{4,5,6}};
+NRMat r(rr);
+cout<<r;
+
+double d;
+linear_solve(m,&r,&d);
+
+Mat3<double> mmi=mm.inverse();
+double dd=simple_gaussj(mm.elements(),rr);
+cout <<"det="<<dd<<endl;
+cout <<"error of inverse = "<<(mmi-mm).norm()<<endl;
+cout <<"3 3\n"<<mm<<NRMat<double>(rr);
+
+cout<<"linear solve det="<<d<<endl;
+cout <<r;
+cout <<"det error="<<d-dd<<endl;
+cout <<"solution error="<<(r-rr).norm()<<endl;
+
+}
+
 }
diff --git a/vecmat3.cc b/vecmat3.cc
index 27c1c7f..8578bc2 100644
--- a/vecmat3.cc
+++ b/vecmat3.cc
@@ -522,7 +522,7 @@ q[2][1]=q[1][2]=tmp;
 //
 
 //numeric_limits not available on some crosscompilers for small MCUs
-#ifdef ARM_SOURCE37
+#ifdef QUAT_NO_DOUBLE
 #define DBL_EPSILON 1.19209290e-07f
 #else
 #define  DBL_EPSILON std::numeric_limits<T>::epsilon()
@@ -785,6 +785,19 @@ Mat3<T> A(*this); //scratch copy
 //end eigensolver for 3x3 matrix
 /////////////////////////////////////////////////////////////////////////////////////////////
 
+template<typename T>
+T Mat3<T>::norm(const T scalar) const
+{
+        T sum(0);
+        for(int i=0; i<3; i++)
+                for(int j=0; j<3; j++) {
+                        T tmp = q[i][j];
+                        if(i == j) tmp -= scalar;
+                        sum += tmp*tmp;
+                }
+return sqrt(sum);
+}
+
 //force instantization
 #define INSTANTIZE(T) \
 template class Vec3<T>; \
diff --git a/vecmat3.h b/vecmat3.h
index 9d7f7e1..5a9b95f 100644
--- a/vecmat3.h
+++ b/vecmat3.h
@@ -1,6 +1,6 @@
 /*
     LA: linear algebra C++ interface library
-    Copyright (C) 2020 Jiri Pittner <jiri.pittner@jh-inst.cas.cz> or <jiri@pittnerovi.com>
+    Copyright (C) 2020-2021 Jiri Pittner <jiri.pittner@jh-inst.cas.cz> or <jiri@pittnerovi.com>
 
     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -46,6 +46,9 @@ class Vec3
 public:
 	//just plain old data
 	T q[3];
+	T (&elements())[3] {return q;};
+        const T (&elements()const)[3]  {return q;};
+
 	//
 	Vec3(void) {};
 	Vec3(const T x, const T u=0, const T v=0) {q[0]=x; q[1]=u; q[2]=v;}; //Vec3 from real(s)
@@ -98,6 +101,8 @@ public:
 	//just plain old data
 	T q[3][3];
 	//
+	T (&elements())[3][3] {return q;};
+	const T (&elements()const)[3][3]  {return q;};
 	Mat3(void) {};
 	Mat3(const T (&a)[3][3]) {memcpy(q,a,3*3*sizeof(T));}
 	Mat3(const T x) {memset(q,0,9*sizeof(T)); q[0][0]=q[1][1]=q[2][2]=x;}; //scalar matrix
@@ -148,9 +153,70 @@ public:
 	void symmetrize(); //average offdiagonal elements
 	void eival_sym(Vec3<T> &w) const; //only for real symmetric matrix, symmetry is not checked
 	void eivec_sym(Vec3<T> &w, Mat3 &v) const; //only for real symmetric matrix, symmetry is not checked
+	T norm(const T scalar = 0) const;
 };
 
 
+//a simple gauss elimination as a template also for larger-size matrices in form of C-style arrays
+#define SWAP(a,b) {T temp=(a);(a)=(b);(b)=temp;}
+
+template<typename T, int n, int m>
+T simple_gaussj(T (&a)[n][n],T (&b)[m][n]) //returns determinant, m is number of rhs to solve
+{
+	int indxc[n],indxr[n],ipiv[n];
+	int i,icol,irow,j,k,l,ll;
+	T det,big,dum,pivinv;
+
+	det=1;
+	for (j=0;j<n;j++) ipiv[j]=0;
+	for (i=0;i<n;i++) {
+		big=0.0;
+		for (j=0;j<n;j++)
+			if (ipiv[j] != 1)
+				for (k=0;k<n;k++) {
+					if (ipiv[k] == 0) {
+						if (abs(a[j][k]) >= big) {
+							big=abs(a[j][k]);
+							irow=j;
+							icol=k;
+						}
+					} else if (ipiv[k] > 1) {return 0;}
+				}
+		++(ipiv[icol]);
+		if (irow != icol) {
+			det = (-det);
+			for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l])
+			for (l=0;l<m;l++) SWAP(b[l][irow],b[l][icol])
+		}
+		indxr[i]=irow;
+		indxc[i]=icol;
+		if (a[icol][icol] == 0) {return 0;}
+		pivinv=1/a[icol][icol];
+		det *= a[icol][icol];
+		a[icol][icol]=1.0;
+		for (l=0;l<n;l++) a[icol][l] *= pivinv;
+		for (l=0;l<m;l++) b[l][icol] *= pivinv;
+		for (ll=0;ll<n;ll++)
+			if (ll != icol) {
+				dum=a[ll][icol];
+				a[ll][icol]=0.0;
+				for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
+				for (l=0;l<m;l++) b[l][ll] -= b[l][icol]*dum;
+			}
+	}
+	for (l=n-1;l>=0;l--) {
+		if (indxr[l] != indxc[l])
+			for (k=0;k<n;k++)
+				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
+	}
+return det;
+}
+
+#undef SWAP
+
+
+
+
 	
 //stream I/O
 #ifndef AVOID_STDSTREAM