From c5a2865639a6844c4323c7eef7879c8627d2baca Mon Sep 17 00:00:00 2001
From: Jiri Pittner <jiri@pittnerovi.com>
Date: Thu, 16 Nov 2023 16:19:54 +0100
Subject: [PATCH] vecmat3 transposed multiplications

---
 t.cc       | 13 ++++++++--
 vecmat3.cc | 75 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 vecmat3.h  |  5 ++++
 3 files changed, 91 insertions(+), 2 deletions(-)

diff --git a/t.cc b/t.cc
index 6c80e03..29d5a1e 100644
--- a/t.cc
+++ b/t.cc
@@ -426,11 +426,12 @@ cout <<b;
 cout <<d;
 }
 
-if(0)
+if(1)
 {
 NRMat<double> a;
 cin >>a ;
 NRMat<double> abak=a;
+NRMat<double> abak2=a;
 NRMat<double> u(a.nrows(),a.nrows()),v(a.ncols(),a.ncols());
 NRVec<double>s(a.ncols()<a.nrows()?a.ncols():a.nrows());
 singular_decomposition(a,&u,s,&v,0);
@@ -440,6 +441,14 @@ sdiag.diagonalset(s);
 cout <<sdiag;
 cout <<v;
 cout << "Error "<<(u*sdiag*v-abak).norm()<<endl;
+NRMat<double> ai=calcinverse(abak2);
+cout <<"regular inverse "<<ai;
+NRVec<double>ss(s);ss.copyonwrite();
+for(int i=0; i<ss.size(); ++i) if(ss[i]>1e-12) ss[i]=1./ss[i]; else ss[i]=0;
+sdiag.diagonalset(ss);
+NRMat<double> aii = v.transpose() *sdiag*u.transpose();
+cout <<"svd inverse "<<aii;
+cout << "Error in inverse "<< (aii-ai).norm()<<endl;
 }
 
 if(0)
@@ -2087,7 +2096,7 @@ for(i=1; i<=n; ++i) v[i]=10.*i;
 cout <<v.permuted(p);
 }
 
-if(1)
+if(0)
 {
 int n;
 CyclePerm<int> c;
diff --git a/vecmat3.cc b/vecmat3.cc
index c9837f7..86d2174 100644
--- a/vecmat3.cc
+++ b/vecmat3.cc
@@ -97,6 +97,18 @@ const Vec3<T> Vec3<T>::operator*(const Mat3<T> &rhs) const
                 return r;
                 }; 
 
+template<typename T>
+const Vec3<T> Vec3<T>::timesT(const Mat3<T> &rhs) const
+                { 
+                Vec3<T> r;
+                r[0] = q[0]*rhs.q[0][0] + q[1]*rhs.q[0][1] + q[2]*rhs.q[0][2];
+                r[1] = q[0]*rhs.q[1][0] + q[1]*rhs.q[1][1] + q[2]*rhs.q[1][2];
+                r[2] = q[0]*rhs.q[2][0] + q[1]*rhs.q[2][1] + q[2]*rhs.q[2][2];
+                return r;
+                }; 
+
+
+
 
 template<typename T>
 Mat3<T>& Mat3<T>::operator+=(const Mat3 &rhs) 
@@ -129,6 +141,58 @@ const Mat3<T> Mat3<T>::operator*(const Mat3 &rhs) const
                 return r;
                 }  
 
+template<typename T>
+const Mat3<T> Mat3<T>::Ttimes(const Mat3 &rhs) const
+                {
+                Mat3<T> r;
+                r[0][0]= q[0][0]*rhs.q[0][0] + q[1][0]*rhs.q[1][0] + q[2][0]*rhs.q[2][0];
+                r[0][1]= q[0][0]*rhs.q[0][1] + q[1][0]*rhs.q[1][1] + q[2][0]*rhs.q[2][1];
+                r[0][2]= q[0][0]*rhs.q[0][2] + q[1][0]*rhs.q[1][2] + q[2][0]*rhs.q[2][2];
+                r[1][0]= q[0][1]*rhs.q[0][0] + q[1][1]*rhs.q[1][0] + q[2][1]*rhs.q[2][0];
+                r[1][1]= q[0][1]*rhs.q[0][1] + q[1][1]*rhs.q[1][1] + q[2][1]*rhs.q[2][1];
+                r[1][2]= q[0][1]*rhs.q[0][2] + q[1][1]*rhs.q[1][2] + q[2][1]*rhs.q[2][2];
+                r[2][0]= q[0][2]*rhs.q[0][0] + q[1][2]*rhs.q[1][0] + q[2][2]*rhs.q[2][0];
+                r[2][1]= q[0][2]*rhs.q[0][1] + q[1][2]*rhs.q[1][1] + q[2][2]*rhs.q[2][1];
+                r[2][2]= q[0][2]*rhs.q[0][2] + q[1][2]*rhs.q[1][2] + q[2][2]*rhs.q[2][2];
+                return r;
+                }
+
+
+template<typename T>
+const Mat3<T> Mat3<T>::timesT(const Mat3 &rhs) const
+                {
+                Mat3<T> r;
+                r[0][0]= q[0][0]*rhs.q[0][0] + q[0][1]*rhs.q[0][1] + q[0][2]*rhs.q[0][2];
+                r[0][1]= q[0][0]*rhs.q[1][0] + q[0][1]*rhs.q[1][1] + q[0][2]*rhs.q[1][2];
+                r[0][2]= q[0][0]*rhs.q[2][0] + q[0][1]*rhs.q[2][1] + q[0][2]*rhs.q[2][2];
+                r[1][0]= q[1][0]*rhs.q[0][0] + q[1][1]*rhs.q[0][1] + q[1][2]*rhs.q[0][2];
+                r[1][1]= q[1][0]*rhs.q[1][0] + q[1][1]*rhs.q[1][1] + q[1][2]*rhs.q[1][2];
+                r[1][2]= q[1][0]*rhs.q[2][0] + q[1][1]*rhs.q[2][1] + q[1][2]*rhs.q[2][2];
+                r[2][0]= q[2][0]*rhs.q[0][0] + q[2][1]*rhs.q[0][1] + q[2][2]*rhs.q[0][2];
+                r[2][1]= q[2][0]*rhs.q[1][0] + q[2][1]*rhs.q[1][1] + q[2][2]*rhs.q[1][2];
+                r[2][2]= q[2][0]*rhs.q[2][0] + q[2][1]*rhs.q[2][1] + q[2][2]*rhs.q[2][2];
+                return r;
+                }
+
+template<typename T>
+const Mat3<T> Mat3<T>::TtimesT(const Mat3 &rhs) const
+                {
+                Mat3<T> r;
+                r[0][0]= q[0][0]*rhs.q[0][0] + q[1][0]*rhs.q[0][1] + q[2][0]*rhs.q[0][2];
+                r[0][1]= q[0][0]*rhs.q[1][0] + q[1][0]*rhs.q[1][1] + q[2][0]*rhs.q[1][2];
+                r[0][2]= q[0][0]*rhs.q[2][0] + q[1][0]*rhs.q[2][1] + q[2][0]*rhs.q[2][2];
+                r[1][0]= q[0][1]*rhs.q[0][0] + q[1][1]*rhs.q[0][1] + q[2][1]*rhs.q[0][2];
+                r[1][1]= q[0][1]*rhs.q[1][0] + q[1][1]*rhs.q[1][1] + q[2][1]*rhs.q[1][2];
+                r[1][2]= q[0][1]*rhs.q[2][0] + q[1][1]*rhs.q[2][1] + q[2][1]*rhs.q[2][2];
+                r[2][0]= q[0][2]*rhs.q[0][0] + q[1][2]*rhs.q[0][1] + q[2][2]*rhs.q[0][2];
+                r[2][1]= q[0][2]*rhs.q[1][0] + q[1][2]*rhs.q[1][1] + q[2][2]*rhs.q[1][2];
+                r[2][2]= q[0][2]*rhs.q[2][0] + q[1][2]*rhs.q[2][1] + q[2][2]*rhs.q[2][2];
+                return r;
+                }
+
+
+
+
 template<typename T>
 T Mat3<T>::determinant() const 
 {
@@ -165,6 +229,17 @@ const Vec3<T> Mat3<T>::operator*(const Vec3<T> &rhs) const
                 return r;
                 }
 
+template<typename T>
+const Vec3<T> Mat3<T>::Ttimes(const Vec3<T> &rhs) const
+                {
+                Vec3<T> r;
+                r[0] = q[0][0]*rhs.q[0] + q[1][0]*rhs.q[1] + q[2][0]*rhs.q[2];
+                r[1] = q[0][1]*rhs.q[0] + q[1][1]*rhs.q[1] + q[2][1]*rhs.q[2];
+                r[2] = q[0][2]*rhs.q[0] + q[1][2]*rhs.q[1] + q[2][2]*rhs.q[2];
+                return r;
+                }
+
+
 
 
 
diff --git a/vecmat3.h b/vecmat3.h
index a8df298..95f6f4c 100644
--- a/vecmat3.h
+++ b/vecmat3.h
@@ -86,6 +86,7 @@ public:
 	Vec3& normalize(void) {*this /= norm(); return *this;};
 	Vec3& fast_normalize(void);
 	const Vec3 operator*(const Mat3<T> &rhs) const;
+	const Vec3 timesT(const Mat3<T> &rhs) const; //with transpose
 	Mat3<T> outer(const Vec3 &rhs) const; //tensor product
 	void inertia(Mat3<T> &itensor, const T weight) const; //contribution to inertia tensor
 
@@ -151,7 +152,11 @@ public:
 	const Mat3 operator+(const Mat3 &rhs) const {return Mat3(*this) += rhs;};
 	const Mat3 operator-(const Mat3 &rhs) const {return Mat3(*this) -= rhs;};
 	const Mat3 operator*(const Mat3 &rhs) const; //matrix product
+	const Mat3 timesT(const Mat3 &rhs) const; //matrix product with transpose
+	const Mat3 Ttimes(const Mat3 &rhs) const; //matrix product with transpose
+	const Mat3 TtimesT(const Mat3 &rhs) const; //matrix product with transpose
 	const Vec3<T> operator*(const Vec3<T> &rhs) const; //matrix times vector
+	const Vec3<T> Ttimes(const Vec3<T> &rhs) const; //matrix times vector with transpose
 	T trace() const  {return q[0][0]+q[1][1]+q[2][2];};
 	T determinant() const;
 	void transposeme();