vecmat3 added explicit linear solver

t.cc

@@ -2792,6 +2792,25 @@ if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random");
 close(f);
 srand(seed);
 
+Mat3<double> a;
+a.randomize(10.);
+cout<<a<<endl; 
+Vec3<double> b;
+b.randomize(10.);
+cout <<b<<endl;
+Vec3<double> x = a.linear_solve(b);
+cout <<x<<endl;
+cout <<"linear solve error = "<<(b-a*x).norm()<<endl;
+}
+
+if(0)
+{
+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);
+
 Mat3<double> a;
 a.randomize(10.);
 cout<<a<<endl;

vecmat3.cc

@@ -207,7 +207,7 @@ void Mat3<T>::transposeme()
 {T t; t=q[0][1]; q[0][1]=q[1][0]; q[1][0]=t; t=q[0][2]; q[0][2]=q[2][0]; q[2][0]=t; t=q[1][2]; q[1][2]=q[2][1]; q[2][1]=t;};
 
 template<typename T>
-const Mat3<T> Mat3<T>::inverse() const
+const Mat3<T> Mat3<T>::inverse(T *det) const
                 {
                 Mat3<T> r;
                 r[0][0]=  q[2][2]*q[1][1]-q[2][1]*q[1][2];
@@ -219,7 +219,27 @@ const Mat3<T> Mat3<T>::inverse() const
                 r[2][0]=  q[2][1]*q[1][0]-q[2][0]*q[1][1];
                 r[2][1]= -q[2][1]*q[0][0]+q[2][0]*q[0][1];
                 r[2][2]=  q[1][1]*q[0][0]-q[1][0]*q[0][1];
-                return r/determinant();
+		T d=determinant();
+		if(det) *det=d;
+                return r/d;
+                }
+
+template<typename T>
+const Vec3<T> Mat3<T>::linear_solve(const  Vec3<T> &rhs, T *det) const
+                {
+                Vec3<T> r;
+                r[0]= ( q[2][2]*q[1][1]-q[2][1]*q[1][2]) * rhs[0]
+                      +( -q[2][2]*q[0][1]+q[2][1]*q[0][2]) * rhs[1]
+                      +( +q[1][2]*q[0][1]-q[1][1]*q[0][2]) * rhs[2];
+                r[1]= (-q[2][2]*q[1][0]+q[2][0]*q[1][2]) * rhs[0]
+                      +( +q[2][2]*q[0][0]-q[2][0]*q[0][2])  * rhs[1]
+                      +( -q[1][2]*q[0][0]+q[1][0]*q[0][2]) * rhs[2];
+                r[2]= ( q[2][1]*q[1][0]-q[2][0]*q[1][1]) * rhs[0]
+                      +( -q[2][1]*q[0][0]+q[2][0]*q[0][1]) *rhs[1]
+                      +( q[1][1]*q[0][0]-q[1][0]*q[0][1]) * rhs[2];
+		T d=determinant();
+		if(det) *det=d;
+                return r/d;
                 }
 
 template<typename T>

vecmat3.h

@@ -172,7 +172,8 @@ public:
 	T determinant() const;
 	void transposeme();
 	const Mat3 transpose() const {Mat3 r(*this); r.transposeme(); return r;};
-	const Mat3 inverse() const;
+	const Mat3 inverse(T *det = NULL) const;
+	const Vec3<T> linear_solve(const  Vec3<T> &rhs, T *det = NULL) const;
 	//C-style IO
         int fprintf(FILE *f, const char *format) const {int n= ::fprintf(f,format,q[0][0],q[0][1],q[0][2]); n+=::fprintf(f,format,q[1][0],q[1][1],q[1][2]); n+=::fprintf(f,format,q[2][0],q[2][1],q[2][2]); return n;};
         int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);};