diff --git a/vecmat3.cc b/vecmat3.cc index 8578bc2..0f0414f 100644 --- a/vecmat3.cc +++ b/vecmat3.cc @@ -47,6 +47,46 @@ template Vec3& Vec3::fast_normalize(void) {normalize(); return *this;}; +template +Mat3 Vec3::outer(const Vec3 &rhs) const +{ +Mat3 m; +m[0][0]=q[0]*rhs.q[0]; +m[0][1]=q[0]*rhs.q[1]; +m[0][2]=q[0]*rhs.q[2]; + +m[1][0]=q[1]*rhs.q[0]; +m[1][1]=q[1]*rhs.q[1]; +m[1][2]=q[1]*rhs.q[2]; + +m[2][0]=q[2]*rhs.q[0]; +m[2][1]=q[2]*rhs.q[1]; +m[2][2]=q[2]*rhs.q[2]; + +return m; +} + + +template +void Vec3::inertia(Mat3 &m, const T weight) const +{ +T r2 = q[0]*q[0]+q[1]*q[1]+q[2]*q[2]; + +m[0][0] -= weight*(q[0]*q[0]-r2); +m[0][1] -= weight*q[0]*q[1]; +m[0][2] -= weight*q[0]*q[2]; + +m[1][0] -= weight*q[1]*q[0]; +m[1][1] -= weight*(q[1]*q[1]-r2); +m[1][2] -= weight*q[1]*q[2]; + +m[2][0] -= weight*q[2]*q[0]; +m[2][1] -= weight*q[2]*q[1]; +m[2][2] -= weight*(q[2]*q[2]-r2); +} + + + template const Vec3 Vec3::operator*(const Mat3 &rhs) const { diff --git a/vecmat3.h b/vecmat3.h index 67a97e8..a34ca0e 100644 --- a/vecmat3.h +++ b/vecmat3.h @@ -86,6 +86,9 @@ public: Vec3& normalize(void) {*this /= norm(); return *this;}; Vec3& fast_normalize(void); const Vec3 operator*(const Mat3 &rhs) const; + Mat3 outer(const Vec3 &rhs) const; //tensor product + void inertia(Mat3 &itensor, const T weight) const; //contribution to inertia tensor + //C-style IO int fprintf(FILE *f, const char *format) const {return ::fprintf(f,format,q[0],q[1],q[2]);}; int sprintf(char *f, const char *format) const {return ::sprintf(f,format,q[0],q[1],q[2]);}; @@ -94,6 +97,10 @@ public: }; +template +inline T hypot3(const Vec3 &c, const Vec3 &d) {return((c-d).norm());} + + template class Mat3 {