From 731b2a128dc073ec95deeaa86a960a06363b69b9 Mon Sep 17 00:00:00 2001
From: Jiri Pittner <jiri@pittnerovi.com>
Date: Sat, 18 Nov 2023 18:48:20 +0100
Subject: [PATCH] SVD for Mat3

---
 t.cc       |  27 ++++++++--
 vecmat3.cc | 150 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 vecmat3.h  |  13 +++++
 3 files changed, 181 insertions(+), 9 deletions(-)

diff --git a/t.cc b/t.cc
index 7d22e69..6a26ada 100644
--- a/t.cc
+++ b/t.cc
@@ -2786,6 +2786,12 @@ cout <<"resorted graph = "<<adjperm<<endl;
 
 if(1)
 {
+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;
@@ -2794,22 +2800,37 @@ Mat3<double> work(ata);
 Vec3<double> w;
 Mat3<double> v;
 work.eivec_sym(w,v,true);
-cout <<w<<endl<<sqrt(w[0])<<" "<<sqrt(w[1])<<" "<<sqrt(w[2])<<" "<<endl<<endl<<v<<endl;
+//cout <<w<<endl<<sqrt(w[0])<<" "<<sqrt(w[1])<<" "<<sqrt(w[2])<<" "<<endl<<endl<<v<<endl;
 
 Mat3<double> aat = a*a.transpose();
 Mat3<double> work2(aat);
 Vec3<double> w2; 
 Mat3<double> v2;
 work2.eivec_sym(w2,v2,true);
-cout <<w2<<endl<<sqrt(w2[0])<<" "<<sqrt(w2[1])<<" "<<sqrt(w2[2])<<" "<<endl<<endl<<v2<<endl;
+//cout <<w2<<endl<<sqrt(w2[0])<<" "<<sqrt(w2[1])<<" "<<sqrt(w2[2])<<" "<<endl<<endl<<v2<<endl;
+//cout <<"dets  "<<v2.determinant()<<" "<<v.determinant()<<endl;
+
+Mat3<double> u3,v3;
+Vec3<double> w3;
+a.svd(u3,w3,v3);
+cout <<"Mat3 SVD\n";
+cout <<u3<<endl<<w3<<endl<<endl<<v3.transpose()<<endl<<endl;
+cout <<"Mat3 SVD dets "<<u3.determinant()<<" "<<v3.determinant()<<endl;
 
 
 NRMat<double> aa(a);
-cout <<aa.nrows()<<" "<<aa.ncols()<<endl;
 NRMat<double> uu(3,3),vv(3,3);
 NRVec<double> ss(3);
 singular_decomposition(aa,&uu,ss,&vv,0);
+cout <<"REGULAR SVD\n";
 cout <<uu<<ss<<vv<<endl;
+
+cout <<"svd dets  "<<determinant(uu)<<" "<<determinant(vv)<<endl;
+
+Mat3<double> ainv= a.inverse();
+Mat3<double> ainv2= a.svdinverse();
+cout <<endl<<"Inverse via svd\n"<<ainv2<<endl;
+cout <<"Difference of inverses = "<<(ainv-ainv2).norm()<<endl;
 }
 
 }
diff --git a/vecmat3.cc b/vecmat3.cc
index ef30d54..4ec933f 100644
--- a/vecmat3.cc
+++ b/vecmat3.cc
@@ -662,12 +662,6 @@ else for(int i=0;i<3;++i) for(int j=0; j<3; ++j) q[i][j]=x*RANDDOUBLESIGNED();
 #define NO_NUMERIC_LIMITS
 #endif
 
-#ifdef NO_NUMERIC_LIMITS
-#define DBL_EPSILON 1.19209290e-07f
-#else
-#define  DBL_EPSILON std::numeric_limits<T>::epsilon()
-#endif
-
 #define M_SQRT3    1.73205080756887729352744634151   // sqrt(3)
 #define SQR(x)      ((x)*(x))                        // x^2
 //
@@ -947,6 +941,150 @@ T Mat3<T>::norm(const T scalar) const
 return sqrt(sum);
 }
 
+//efficient SVD for 3x3 matrices cf. Mc.Adams et al. UWM technical report 1690 (2011)
+//
+
+
+template<typename T> 
+static inline T ABS(const T a)
+{
+return a<0?-a:a;
+}
+
+template<typename T> 
+static inline T MAX(const T a, const T b)
+{
+return a>b ? a : b;
+}
+
+
+
+template<typename T>
+void QRGivensQuaternion(T a1, T a2, T &ch, T &sh)
+{
+    // a1 = pivot point on diagonal
+    // a2 = lower triangular entry we want to annihilate
+    T rho = sqrt(a1*a1 + a2*a2);
+
+    sh = rho > DBL_EPSILON ? a2 : 0;
+    ch = ABS(a1) + MAX(rho,DBL_EPSILON);
+    if(a1 < 0)
+	{
+	T tmp=sh;
+	sh=ch;
+	ch=tmp;
+	}
+    T w = (T)1/sqrt(ch*ch+sh*sh);
+    ch *= w;
+    sh *= w;
+}
+
+
+template<typename T>
+void Mat3<T>::qrd(Mat3 &Q, Mat3 &R)
+{
+
+T ch1,sh1,ch2,sh2,ch3,sh3;
+T a,b;
+    
+// first givens rotation (ch,0,0,sh)
+QRGivensQuaternion(q[0][0],q[1][0],ch1,sh1);
+a=1-2*sh1*sh1;
+b=2*ch1*sh1;
+// apply B = Q' * B
+R[0][0]= a*q[0][0]+b*q[1][0];  R[0][1]= a*q[0][1]+b*q[1][1];  R[0][2]= a*q[0][2]+b*q[1][2];
+R[1][0]= -b*q[0][0]+a*q[1][0]; R[1][1]= -b*q[0][1]+a*q[1][1]; R[1][2]= -b*q[0][2]+a*q[1][2];
+R[2][0]= q[2][0];          R[2][1]= q[2][1];          R[2][2]= q[2][2];
+  
+// second givens rotation (ch,0,-sh,0)
+QRGivensQuaternion(R[0][0],R[2][0],ch2,sh2);
+a= 1-2*sh2*sh2;
+b= 2*ch2*sh2;
+// apply B = Q' * B;
+q[0][0]= a*R[0][0]+b*R[2][0];  q[0][1]= a*R[0][1]+b*R[2][1];  q[0][2]= a*R[0][2]+b*R[2][2];
+q[1][0]= R[1][0];           q[1][1]= R[1][1];           q[1][2]= R[1][2];
+q[2][0]= -b*R[0][0]+a*R[2][0]; q[2][1]= -b*R[0][1]+a*R[2][1]; q[2][2]= -b*R[0][2]+a*R[2][2];
+
+// third givens rotation (ch,sh,0,0)
+QRGivensQuaternion(q[1][1],q[2][1],ch3,sh3);
+a= 1-2*sh3*sh3;
+b= 2*ch3*sh3;
+// R is now set to desired value
+R[0][0]= q[0][0];             R[0][1]= q[0][1];           R[0][2]= q[0][2];
+R[1][0]= a*q[1][0]+b*q[2][0];     R[1][1]= a*q[1][1]+b*q[2][1];   R[1][2]= a*q[1][2]+b*q[2][2];
+R[2][0]= -b*q[1][0]+a*q[2][0];    R[2][1]= -b*q[1][1]+a*q[2][1];  R[2][2]= -b*q[1][2]+a*q[2][2];
+
+// construct the cumulative rotation Q=Q1 * Q2 * Q3
+// the number of floating point operations for three quaternion multiplications
+// is more or less comparable to the explicit form of the joined matrix.
+// certainly more memory-efficient!
+T sh12= sh1*sh1;
+T sh22= sh2*sh2;
+T sh32= sh3*sh3;
+
+Q[0][0]= (-1+2*sh12)*(-1+2*sh22); 
+Q[0][1]= 4*ch2*ch3*(-1+2*sh12)*sh2*sh3+2*ch1*sh1*(-1+2*sh32); 
+Q[0][2]= 4*ch1*ch3*sh1*sh3-2*ch2*(-1+2*sh12)*sh2*(-1+2*sh32);
+
+Q[1][0]= 2*ch1*sh1*(1-2*sh22); 
+Q[1][1]= -8*ch1*ch2*ch3*sh1*sh2*sh3+(-1+2*sh12)*(-1+2*sh32); 
+Q[1][2]= -2*ch3*sh3+4*sh1*(ch3*sh1*sh3+ch1*ch2*sh2*(-1+2*sh32));
+    
+Q[2][0]= 2*ch2*sh2; 
+Q[2][1]= 2*ch3*(1-2*sh22)*sh3; 
+Q[2][2]= (-1+2*sh22)*(-1+2*sh32);
+}
+
+
+template<typename T>
+void Mat3<T>::svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations) const
+{
+Mat3 ata= this->Ttimes(*this);
+Vec3<T> ww; //actually not needed squares of singular values
+ata.eivec_sym(ww,v,true);
+Mat3 uw = (*this)*v;
+Mat3 r;
+uw.qrd(u,r);
+w[0]= r[0][0];
+w[1]= r[1][1];
+w[2]= r[2][2];
+
+if(proper_rotations) return;
+if(w[2]<0)
+	{
+	w[2] = -w[2];
+	u[0][2] = -u[0][2];
+	u[1][2] = -u[1][2];
+	u[2][2] = -u[2][2];
+	}
+}
+
+
+template<typename T>
+void  Mat3<T>::diagmultl(const Vec3<T> &rhs)
+{
+for(int i=0; i<3; ++i) for(int j=0; j<3; ++j) q[i][j] *= rhs[i];
+}
+
+template<typename T>
+void  Mat3<T>::diagmultr(const Vec3<T> &rhs)
+{
+for(int i=0; i<3; ++i) for(int j=0; j<3; ++j) q[i][j] *= rhs[j];
+}
+
+
+template<typename T>
+const Mat3<T>   Mat3<T>::svdinverse(const T thr) const
+{
+Mat3 u,v;
+Vec3<T> w;
+svd(u,w,v);
+for(int i=0; i<3; ++i) w[i] = (w[i]<thr)? 0 : 1/w[i];
+v.diagmultr(w);
+return v.timesT(u);
+}
+
+
 //cf. https://en.wikipedia.org/wiki/3D_projection
 template<typename T>
 void perspective(T *proj_xy, const Vec3<T> &point, const Mat3<T> &rot_angle, const Vec3<T> &camera,  const Vec3<T> &plane_to_camera)
diff --git a/vecmat3.h b/vecmat3.h
index acd237e..d6c4461 100644
--- a/vecmat3.h
+++ b/vecmat3.h
@@ -33,6 +33,14 @@
 
 namespace LA_Vecmat3 {
 
+#ifdef NO_NUMERIC_LIMITS                     
+#define DBL_EPSILON 1.19209290e-07f
+#else 
+#define  DBL_EPSILON std::numeric_limits<T>::epsilon()
+#endif
+
+
+
 float fast_sqrtinv(float);
 
 //forward declaration
@@ -172,6 +180,11 @@ public:
 	void eival_sym(Vec3<T> &w, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked
 	void eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked
 	T norm(const T scalar = 0) const;
+	void qrd(Mat3 &q, Mat3 &r); //not const, destroys the matrix
+	void svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations=false) const;  //if proper_rotations = true, singular value can be negative but u and v are proper rotations
+	void  diagmultl(const Vec3<T> &rhs);
+	void  diagmultr(const Vec3<T> &rhs);
+	const Mat3 svdinverse(const T thr=1000*DBL_EPSILON) const;
 };