diff --git a/t.cc b/t.cc index 6c80e03..29d5a1e 100644 --- a/t.cc +++ b/t.cc @@ -426,11 +426,12 @@ cout < a; cin >>a ; NRMat abak=a; +NRMat abak2=a; NRMat u(a.nrows(),a.nrows()),v(a.ncols(),a.ncols()); NRVecs(a.ncols() ai=calcinverse(abak2); +cout <<"regular inverse "<ss(s);ss.copyonwrite(); +for(int i=0; i1e-12) ss[i]=1./ss[i]; else ss[i]=0; +sdiag.diagonalset(ss); +NRMat aii = v.transpose() *sdiag*u.transpose(); +cout <<"svd inverse "< 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 Vec3::operator*(const Mat3 &rhs) const return r; }; +template +const Vec3 Vec3::timesT(const Mat3 &rhs) const + { + Vec3 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 Mat3& Mat3::operator+=(const Mat3 &rhs) @@ -129,6 +141,58 @@ const Mat3 Mat3::operator*(const Mat3 &rhs) const return r; } +template +const Mat3 Mat3::Ttimes(const Mat3 &rhs) const + { + Mat3 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 +const Mat3 Mat3::timesT(const Mat3 &rhs) const + { + Mat3 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 +const Mat3 Mat3::TtimesT(const Mat3 &rhs) const + { + Mat3 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 T Mat3::determinant() const { @@ -165,6 +229,17 @@ const Vec3 Mat3::operator*(const Vec3 &rhs) const return r; } +template +const Vec3 Mat3::Ttimes(const Vec3 &rhs) const + { + Vec3 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 &rhs) const; + const Vec3 timesT(const Mat3 &rhs) const; //with transpose Mat3 outer(const Vec3 &rhs) const; //tensor product void inertia(Mat3 &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 operator*(const Vec3 &rhs) const; //matrix times vector + const Vec3 Ttimes(const Vec3 &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();