From 2ab28bb5e8a910298c9271caf0c78062e1d19117 Mon Sep 17 00:00:00 2001 From: jiri Date: Sat, 4 Jan 2020 20:28:03 +0000 Subject: [PATCH] *** empty log message *** --- quaternion.cc | 74 +++++++++++++++++++++++--- quaternion.h | 129 ++++++++++++++++++++++++++++++++------------ t.cc | 68 ++++++++++++++++++++++- vecmat3.h | 145 ++++++++++++++++++++++++++++++++++++++++++++++---- 4 files changed, 366 insertions(+), 50 deletions(-) diff --git a/quaternion.cc b/quaternion.cc index 9b82ad7..a937165 100644 --- a/quaternion.cc +++ b/quaternion.cc @@ -38,8 +38,39 @@ return Quaternion //basically the same code as in normquat2rotmat, but avoiding extra storage template -void Quaternion::rotate(T *to, const T *from) const +void Quaternion::rotate(T *to, const T *from, Quaternion *grad) const { +//some subexpression eliminations +{ +T q00= q[0]*q[0]; +T q11= q[1]*q[1]; +T q22= q[2]*q[2]; +T q33= q[3]*q[3]; + +to[0] = (q00+q11-q22-q33) * from[0]; +to[1] = (q00+q22-q11-q33) * from[1]; +to[2] = (q00+q33-q11-q22) * from[2]; +} + +T q01= q[0]*q[1]; +T q02= q[0]*q[2]; +T q03= q[0]*q[3]; +T q12= q[1]*q[2]; +T q13= q[1]*q[3]; +T q23= q[2]*q[3]; + +T f0=from[0]+from[0]; +T f1=from[1]+from[1]; +T f2=from[2]+from[2]; + +to[0] += (q12+q03)*f1; +to[0] += (q13-q02)*f2; +to[1] += (q12-q03)*f0; +to[1] += (q23+q01)*f2; +to[2] += (q13+q02)*f0; +to[2] += (q23-q01)*f1; + +/* to[0] = (2*q[0]*q[0]-1+2*q[1]*q[1]) * from[0] + 2*(q[1]*q[2]+q[0]*q[3]) * from[1] + 2*(q[1]*q[3]-q[0]*q[2]) * from[2]; @@ -49,6 +80,26 @@ to[1] = 2*(q[1]*q[2]-q[0]*q[3]) * from[0] + to[2] = 2*(q[1]*q[3]+q[0]*q[2]) * from[0] + 2*(q[2]*q[3]-q[0]*q[1]) * from[1] + (2*q[0]*q[0]-1+2*q[3]*q[3]) * from[2]; +*/ + +if(grad) + { + grad[0][0]= q[0]*f0 + q[3]*f1 - q[2]*f2; + grad[0][1]= q[1]*f0 + q[2]*f1 + q[3]*f2; + grad[0][2]= -q[2]*f0 + q[1]*f1 - q[0]*f2; + grad[0][3]= -q[3]*f0 + q[0]*f1 + q[1]*f2; + + grad[1][0]= -q[3]*f0 + q[0]*f1 + q[1]*f2; + grad[1][1]= q[2]*f0 - q[1]*f1 + q[0]*f2; + grad[1][2]= q[1]*f0 + q[2]*f1 + q[3]*2; + grad[1][3]= -q[0]*f0 - q[3]*f1 + q[2]*f2; + + grad[2][0]= q[2]*f0 - q[1]*f1 + q[0]*f2; + grad[2][1]= q[3]*f0 - q[0]*f1 - q[1]*f2; + grad[2][2]= q[0]*f0 + q[3]*f1 - q[2]*f2; + grad[2][3]= q[1]*f0 + q[2]*f1 + q[3]*f2; + + } } @@ -62,12 +113,24 @@ rhs.rotate(&r[1],&q[1]); return r; } +template +Quaternion Quaternion::rotate_match(T *to, const T *from, const T *match) const +{ +Quaternion grad[3]; +Quaternion::rotate(to, from, grad); +Quaternion derivative; +derivative = grad[0] * (to[0]-match[0]); +derivative += grad[1] * (to[1]-match[1]); +derivative += grad[2] * (to[2]-match[2]); +return derivative; +} + //optionally skip this for microcontrollers if not needed //note that C++ standard headers should use float version of the functions for T=float #ifndef AVOID_GONIOM_FUNC template -void normquat2euler(const Quaternion &q, T *e) +void Quaternion::normquat2euler(T *e) const { e[0]= atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1); e[1]= -asin(2*q[1]*q[3]+2*q[0]*q[2]); @@ -75,7 +138,7 @@ e[2]= atan2(2*q[2]*q[3]-2*q[0]*q[1],2*q[0]*q[0]+2*q[3]*q[3]-1); } template -void axis2normquat(const T *axis, const T &angle, Quaternion &q) +void Quaternion::axis2normquat(const T *axis, const T &angle) { T a = (T).5*angle; q[0]=cos(a); @@ -86,7 +149,7 @@ q[3]=axis[2]*s; } template -void normquat2axis(const Quaternion &q, T *axis, T &angle) +void Quaternion::normquat2axis(T *axis, T &angle) const { T s = sqrt(q[1]*q[1] + q[2]*q[2] +q[3]*q[3]); angle = 2*atan2(s,q[0]); @@ -104,9 +167,6 @@ axis[2]= q[3]*s; template class Quaternion; \ #define INSTANTIZE2(T) \ -template void normquat2euler(const Quaternion &q, T *e); \ -template void axis2normquat(const T *axis, const T &angle, Quaternion &q); \ -template void normquat2axis(const Quaternion &q, T *axis, T &angle); \ INSTANTIZE(float) diff --git a/quaternion.h b/quaternion.h index e66ee98..3e6d086 100644 --- a/quaternion.h +++ b/quaternion.h @@ -43,14 +43,14 @@ public: //compiler generates default copy constructor and assignment operator //formal indexing - const T operator[](const int i) const {return this->q[i];}; - T& operator[](const int i) {return this->q[i];}; + inline const T operator[](const int i) const {return q[i];}; + inline T& operator[](const int i) {return q[i];}; //operations of quaternions with scalars Quaternion& operator=(const T x) {q[0]=x; memset(&q[1],0,3*sizeof(T)); return *this;}; //quaternion from real - Quaternion& operator+=(const T rhs) {this->q[0]+=rhs; return *this;}; - Quaternion& operator-=(const T rhs) {this->q[0]-=rhs; return *this;}; - Quaternion& operator*=(const T rhs) {this->q[0]*=rhs; this->q[1]*=rhs; this->q[2]*=rhs; this->q[3]*=rhs; return *this;}; + Quaternion& operator+=(const T rhs) {q[0]+=rhs; return *this;}; + Quaternion& operator-=(const T rhs) {q[0]-=rhs; return *this;}; + Quaternion& operator*=(const T rhs) {q[0]*=rhs; q[1]*=rhs; q[2]*=rhs; q[3]*=rhs; return *this;}; Quaternion& operator/=(const T rhs) {return *this *= ((T)1/rhs);}; const Quaternion operator+(const T rhs) const {return Quaternion(*this) += rhs;}; const Quaternion operator-(const T rhs) const {return Quaternion(*this) -= rhs;}; @@ -59,20 +59,27 @@ public: //quaternion algebra const Quaternion operator-() const {Quaternion r(*this); r.q[0]= -r.q[0]; r.q[1]= -r.q[1]; r.q[2]= -r.q[2]; r.q[3]= -r.q[3]; return r;}; //unary minus - Quaternion& operator+=(const Quaternion &rhs) {this->q[0]+=rhs.q[0];this->q[1]+=rhs.q[1];this->q[2]+=rhs.q[2];this->q[3]+=rhs.q[3]; return *this;}; - Quaternion& operator-=(const Quaternion &rhs) {this->q[0]-=rhs.q[0];this->q[1]-=rhs.q[1];this->q[2]-=rhs.q[2];this->q[3]-=rhs.q[3]; return *this;}; + Quaternion& operator+=(const Quaternion &rhs) {q[0]+=rhs.q[0];q[1]+=rhs.q[1];q[2]+=rhs.q[2];q[3]+=rhs.q[3]; return *this;}; + Quaternion& operator-=(const Quaternion &rhs) {q[0]-=rhs.q[0];q[1]-=rhs.q[1];q[2]-=rhs.q[2];q[3]-=rhs.q[3]; return *this;}; const Quaternion operator+(const Quaternion &rhs) const {return Quaternion(*this) += rhs;}; const Quaternion operator-(const Quaternion &rhs) const {return Quaternion(*this) -= rhs;}; const Quaternion operator*(const Quaternion &rhs) const; Quaternion& conjugateme(void) {q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; return *this;} Quaternion conjugate(void) const {return Quaternion(*this).conjugateme();} - T normsqr(void) const {return q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];}; - T norm(void) const {return sqrt(this->normsqr());}; - Quaternion& normalize(bool unique_sign=false) {*this /= this->norm(); if(unique_sign && q[0]<0) *this *= (T)-1; return *this;}; - Quaternion inverse(void) const {return Quaternion(*this).conjugateme()/this->normsqr();}; + T dot(const Quaternion &rhs) const {return q[0]*rhs.q[0] + q[1]*rhs.q[1] + q[2]*rhs.q[2] + q[3]*rhs.q[3];}; + T normsqr(void) const {return dot(*this);}; + T norm(void) const {return sqrt(normsqr());}; + Quaternion& normalize(bool unique_sign=false) {*this /= norm(); if(unique_sign && q[0]<0) *this *= (T)-1; return *this;}; + Quaternion inverse(void) const {return Quaternion(*this).conjugateme()/normsqr();}; const Quaternion operator/(const Quaternion &rhs) const {return *this * rhs.inverse();}; Quaternion rotateby(const Quaternion &rhs); //conjugation-rotation of this by NORMALIZED rhs - void rotate(T *to, const T *from) const; //rotate xyz vector by NORMALIZED *this + void rotate(T *to, const T *from, Quaternion *grad=NULL) const; //rotate xyz vector by NORMALIZED *this + Quaternion rotate_match(T *to, const T *from, const T *match) const; //gradient of quaternion rotation which should match a given vector by rotating the from vector + + //some conversions + void normquat2euler(T *) const; //"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R] + void axis2normquat(const T *axis, const T &angle); + void normquat2axis(T *axis, T &angle) const; }; @@ -97,27 +104,40 @@ return s; } -//"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R] -template -void normquat2euler(const Quaternion &q, T *); -//the following must be in .h due to the generic M type which is unspecified and can be any type providing [][], either plain C matrix or std::matrix or LA matrix +//the following must be in .h due to the generic M type which is unspecified and can be any type providing [][], either plain C matrix, Mat3 class, or std::matrix or LA matrix NRMat //conversion between quanternions and rotation matrices // template void normquat2rotmat(const Quaternion &q, M &a) { -a[0][0] = 2*q[0]*q[0]-1+2*q[1]*q[1]; -a[0][1] = 2*(q[1]*q[2]+q[0]*q[3]); -a[0][2] = 2*(q[1]*q[3]-q[0]*q[2]); -a[1][0] = 2*(q[1]*q[2]-q[0]*q[3]); -a[1][1] = 2*q[0]*q[0]-1+2*q[2]*q[2]; -a[1][2] = 2*(q[2]*q[3]+q[0]*q[1]); -a[2][0] = 2*(q[1]*q[3]+q[0]*q[2]); -a[2][1] = 2*(q[2]*q[3]-q[0]*q[1]); -a[2][2] = 2*q[0]*q[0]-1+2*q[3]*q[3]; +//some explicit common subexpression optimizations +{ +T q00= q[0]*q[0]; +T q11= q[1]*q[1]; +T q22= q[2]*q[2]; +T q33= q[3]*q[3]; + +a[0][0] = q00+q11-q22-q33; +a[1][1] = q00+q22-q11-q33; +a[2][2] = q00+q33-q11-q22; +} + +T q01= q[0]*q[1]; +T q02= q[0]*q[2]; +T q03= q[0]*q[3]; +T q12= q[1]*q[2]; +T q13= q[1]*q[3]; +T q23= q[2]*q[3]; + +a[0][1] = 2*(q12+q03); +a[0][2] = 2*(q13-q02); +a[1][0] = 2*(q12-q03); +a[1][2] = 2*(q23+q01); +a[2][0] = 2*(q13+q02); +a[2][1] = 2*(q23-q01); } template @@ -127,6 +147,57 @@ if(!already_normalized) q.normalize(); normquat2rotmat(q,a); } +//derivative of the rotation matrix by quaternion elements +template +void normquat2rotmatder(const Quaternion &q, Quaternion &a) +{ +//some explicit common subexpression optimizations +T q0= q[0]+q[0]; +T q1= q[1]+q[1]; +T q2= q[2]+q[2]; +T q3= q[3]+q[3]; + +a[0][0][0]= q0; +a[0][0][1]= q3; +a[0][0][2]= -q2; +a[0][1][0]= -q3; +a[0][1][1]= q0; +a[0][1][2]= q1; +a[0][2][0]= q2; +a[0][2][1]= -q1; +a[0][2][2]= q0; + +a[1][0][0]= q1; +a[1][0][1]= q2; +a[1][0][2]= q3; +a[1][1][0]= q2; +a[1][1][1]= -q1; +a[1][1][2]= q0; +a[1][2][0]= q3; +a[1][2][1]= -q0; +a[1][2][2]= -q1; + +a[2][0][0]= -q2; +a[2][0][1]= q1; +a[2][0][2]= -q0; +a[2][1][0]= q1; +a[2][1][1]= q2; +a[2][1][2]= q3; +a[2][2][0]= q0; +a[2][2][1]= q3; +a[2][2][2]= -q2; + +a[3][0][0]= -q3; +a[3][0][1]= q0; +a[3][0][2]= q1; +a[3][1][0]= -q0; +a[3][1][1]= -q3; +a[3][1][2]= q2; +a[3][2][0]= q1; +a[3][2][1]= q2; +a[3][2][2]= q3; +} + //normalized quaternion from rotation matrix //convention compatible with the paper on MEMS sensors by Sebastian O.H. Madgwick @@ -165,14 +236,6 @@ if(a[0][1]-a[1][0]<0) q[3] = -q[3]; } -//rotation about unit vector axis (given by pointer for simplicity) through an angle to a normalized quaternion -#ifndef AVOID_GONIOM_FUNC -template -void axis2normquat(const T *axis, const T &angle, Quaternion &q); - -template -void normquat2axis(const Quaternion &q, T *axis, T &angle); -#endif diff --git a/t.cc b/t.cc index 651841f..9ea432f 100644 --- a/t.cc +++ b/t.cc @@ -20,6 +20,8 @@ #include #include "la.h" +#include "vecmat3.h" +#include "quaternion.h" using namespace std; using namespace LA; @@ -1848,7 +1850,7 @@ double det=determinant_destroy(a); cout << "det= "<>v; cout < q(1.); +Quaternion p,r(q); +p=r; +q+=p; +q-=r; +r=p+q; +r+=Quaternion(0,8,8,8); +r+= 2.; +r/=4.; +cout< s(-1,2,1,2); +Quaternion t=r*s.conjugate(); +cout <tt = t.inverse(); +cout < rotmat; +quat2rotmat(r,rotmat); +cout << rotmat[0][1]< rotmat2(3,3),rotmat3(3,3); +Quaternion > rotmatder; +rotmatder[0].resize(3,3); +rotmatder[1].resize(3,3); +rotmatder[2].resize(3,3); +rotmatder[3].resize(3,3); +normquat2rotmatder(r,rotmatder); +cout << rotmatder; +normquat2rotmat(r,rotmat2); +normquat2rotmat(-r,rotmat3); +cout << rotmat2< rr; +rotmat2normquat(rotmat2,rr); +cout <<"orig "< eul; +r.normquat2euler(eul); +cout< axis={0,1/sqrt(2),1/sqrt(2)}; +Quaternion rrr; +rrr.axis2normquat(axis,0.5); +NRVec axis2(3); +double angle; +rrr.normquat2axis(&axis2[0],angle); +cout <<"back angle "< vec={1,2,3}; +Quaternion qvec(vec); +Quaternion rvec = qvec.rotateby(rrr); +Quaternion rvec2 = qvec.rotateby(rrr.conjugate()); +cout < rrvec = rvec.rotateby(rrr.conjugate()); +cout < rotvec; +rrr.rotate(rotvec,vec); +} + + } diff --git a/vecmat3.h b/vecmat3.h index 4f978e2..72cf24c 100644 --- a/vecmat3.h +++ b/vecmat3.h @@ -28,42 +28,144 @@ #include #include +//forward declaration +template class Mat3; + template class Vec3 { + friend class Mat3; public: //just plain old data T q[3]; // Vec3(void) {}; Vec3(const T x, const T u=0, const T v=0) {q[0]=x; q[1]=u; q[2]=v;}; //Vec3 from real(s) - explicit Vec3(const T* x) {memcpy(q,x,3*sizeof(T));} + Vec3(const T* x) {memcpy(q,x,3*sizeof(T));} + + //get pointer to data transparently + inline operator const T*() const {return q;}; + inline operator T*() {return q;}; //compiler generates default copy constructor and assignment operator //formal indexing - const T operator[](const int i) const {return this->q[i];}; - T& operator[](const int i) {return this->q[i];}; + inline const T operator[](const int i) const {return q[i];}; + inline T& operator[](const int i) {return q[i];}; //operations of Vec3s with scalars - Vec3& operator*=(const T rhs) {this->q[0]*=rhs; this->q[1]*=rhs; this->q[2]*=rhs; return *this;}; + Vec3& operator*=(const T rhs) {q[0]*=rhs; q[1]*=rhs; q[2]*=rhs; return *this;}; Vec3& operator/=(const T rhs) {return *this *= ((T)1/rhs);}; const Vec3 operator*(const T rhs) const {return Vec3(*this) *= rhs;}; const Vec3 operator/(const T rhs) const {return Vec3(*this) /= rhs;}; //Vec3 algebra const Vec3 operator-() const {Vec3 r(*this); r.q[0]= -r.q[0]; r.q[1]= -r.q[1]; r.q[2]= -r.q[2]; return r;}; //unary minus - Vec3& operator+=(const Vec3 &rhs) {this->q[0]+=rhs.q[0];this->q[1]+=rhs.q[1];this->q[2]+=rhs.q[2]; return *this;}; - Vec3& operator-=(const Vec3 &rhs) {this->q[0]-=rhs.q[0];this->q[1]-=rhs.q[1];this->q[2]-=rhs.q[2]; return *this;}; + Vec3& operator+=(const Vec3 &rhs) {q[0]+=rhs.q[0];q[1]+=rhs.q[1];q[2]+=rhs.q[2]; return *this;}; + Vec3& operator-=(const Vec3 &rhs) {q[0]-=rhs.q[0];q[1]-=rhs.q[1];q[2]-=rhs.q[2]; return *this;}; const Vec3 operator+(const Vec3 &rhs) const {return Vec3(*this) += rhs;}; const Vec3 operator-(const Vec3 &rhs) const {return Vec3(*this) -= rhs;}; const Vec3 operator*(const Vec3 &rhs) const {Vec3 x; x[0] = q[1]*rhs.q[2]-q[2]*rhs.q[1]; x[1] = q[2]*rhs.q[0]-q[0]*rhs.q[2]; x[2] = q[0]*rhs.q[1]-q[1]*rhs.q[0]; return x;}; //vector product T dot(const Vec3 &rhs) const {return q[0]*rhs.q[0] + q[1]*rhs.q[1] + q[2]*rhs.q[2];}; T normsqr(void) const {return dot(*this);}; - T norm(void) const {return sqrt(this->normsqr());}; - Vec3& normalize(void) {*this /= this->norm(); return *this;}; - }; + T norm(void) const {return sqrt(normsqr());}; + Vec3& normalize(void) {*this /= norm(); return *this;}; + const Vec3 operator*(const Mat3 &rhs) const + { + Vec3 r; + r[0] = q[0]*rhs.q[0][0] + q[1]*rhs.q[1][0] + q[2]*rhs.q[2][0]; + r[1] = q[0]*rhs.q[0][1] + q[1]*rhs.q[1][1] + q[2]*rhs.q[2][1]; + r[2] = q[0]*rhs.q[0][2] + q[1]*rhs.q[1][2] + q[2]*rhs.q[2][2]; + return r; + }; //matrix times vector + +}; + + +template +class Mat3 + { + friend class Vec3; +public: + //just plain old data + T q[3][3]; + // + Mat3(void) {}; + Mat3(const T x) {memset(q,0,9*sizeof(T)); q[0][0]=q[1][1]=q[2][2]=x;}; //scalar matrix + Mat3(const T* x) {memcpy(q,x,9*sizeof(T));} + + //get pointer to data transparently + inline operator const T*() const {return q;}; + inline operator T*() {return q;}; + + //compiler generates default copy constructor and assignment operator + + //formal indexing + inline const T* operator[](const int i) const {return q[i];}; + inline T* operator[](const int i) {return q[i];}; + inline const T operator()(const int i, const int j) const {return q[i][j];}; + inline T& operator()(const int i, const int j) {return q[i][j];}; + + + //operations of Mat3s with scalars + Mat3& operator+=(const T rhs) {q[0][0]+=rhs; q[1][1]+=rhs; q[2][2]+=rhs; return *this;}; + Mat3& operator-=(const T rhs) {q[0][0]-=rhs; q[1][1]-=rhs; q[2][2]-=rhs; return *this;}; + const Mat3 operator+(const T rhs) const {return Mat3(*this) += rhs;}; + const Mat3 operator-(const T rhs) const {return Mat3(*this) -= rhs;}; + Mat3& operator*=(const T rhs) {q[0][0]*=rhs; q[0][1]*=rhs; q[0][2]*=rhs; q[1][0]*=rhs; q[1][1]*=rhs; q[1][2]*=rhs; q[2][0]*=rhs; q[2][1]*=rhs; q[2][2]*=rhs; return *this;}; + Mat3& operator/=(const T rhs) {return *this *= ((T)1/rhs);}; + const Mat3 operator*(const T rhs) const {return Mat3(*this) *= rhs;}; + const Mat3 operator/(const T rhs) const {return Mat3(*this) /= rhs;}; + + //Mat3 algebra + const Mat3 operator-() const {return *this * (T)-1;}; //unary minus + Mat3& operator+=(const Mat3 &rhs) {q[0][0]+=rhs.q[0][0];q[0][1]+=rhs.q[0][1];q[0][2]+=rhs.q[0][2]; q[1][0]+=rhs.q[1][0];q[1][1]+=rhs.q[1][1];q[1][2]+=rhs.q[1][2]; q[2][0]+=rhs.q[2][0];q[2][1]+=rhs.q[2][1];q[2][2]+=rhs.q[2][2]; return *this;}; + Mat3& operator-=(const Mat3 &rhs) {q[0][0]-=rhs.q[0][0];q[0][1]-=rhs.q[0][1];q[0][2]-=rhs.q[0][2]; q[1][0]-=rhs.q[1][0];q[1][1]-=rhs.q[1][1];q[1][2]-=rhs.q[1][2]; q[2][0]-=rhs.q[2][0];q[2][1]-=rhs.q[2][1];q[2][2]-=rhs.q[2][2]; return *this;}; + 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 + { + Mat3 r; + r[0][0]= q[0][0]*rhs.q[0][0] + q[0][1]*rhs.q[1][0] + q[0][2]*rhs.q[2][0]; + r[0][1]= q[0][0]*rhs.q[0][1] + q[0][1]*rhs.q[1][1] + q[0][2]*rhs.q[2][1]; + r[0][2]= q[0][0]*rhs.q[0][2] + q[0][1]*rhs.q[1][2] + q[0][2]*rhs.q[2][2]; + r[1][0]= q[1][0]*rhs.q[0][0] + q[1][1]*rhs.q[1][0] + q[1][2]*rhs.q[2][0]; + r[1][1]= q[1][0]*rhs.q[0][1] + q[1][1]*rhs.q[1][1] + q[1][2]*rhs.q[2][1]; + r[1][2]= q[1][0]*rhs.q[0][2] + q[1][1]*rhs.q[1][2] + q[1][2]*rhs.q[2][2]; + r[2][0]= q[2][0]*rhs.q[0][0] + q[2][1]*rhs.q[1][0] + q[2][2]*rhs.q[2][0]; + r[2][1]= q[2][0]*rhs.q[0][1] + q[2][1]*rhs.q[1][1] + q[2][2]*rhs.q[2][1]; + r[2][2]= q[2][0]*rhs.q[0][2] + q[2][1]*rhs.q[1][2] + q[2][2]*rhs.q[2][2]; + return r; + }; //matrix product + const Vec3 operator*(const Vec3 &rhs) const + { + Vec3 r; + r[0] = q[0][0]*rhs.q[0] + q[0][1]*rhs.q[1] + q[0][2]*rhs.q[2]; + r[1] = q[1][0]*rhs.q[0] + q[1][1]*rhs.q[1] + q[1][2]*rhs.q[2]; + r[2] = q[2][0]*rhs.q[0] + q[2][1]*rhs.q[1] + q[2][2]*rhs.q[2]; + return r; + }; //matrix times vector + T trace() const {return q[0][0]+q[1][1]+q[2][2];}; + T determinant() const {return q[0][0]*(q[2][2]*q[1][1]-q[2][1]*q[1][2])-q[1][0]*(q[2][2]*q[0][1]-q[2][1]*q[0][2])+q[2][0]*(q[1][2]*q[0][1]-q[1][1]*q[0][2]); };//determinant + void 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;}; + const Mat3 transpose() const {Mat3 r(*this); r.transposeme(); return r;}; + const Mat3 inverse() const + { + Mat3 r; + r[0][0]= q[2][2]*q[1][1]-q[2][1]*q[1][2]; + r[0][1]= -q[2][2]*q[0][1]+q[2][1]*q[0][2]; + r[0][2]= q[1][2]*q[0][1]-q[1][1]*q[0][2]; + r[1][0]= -q[2][2]*q[1][0]+q[2][0]*q[1][1]; + r[1][1]= q[2][2]*q[0][0]-q[2][0]*q[0][2]; + r[1][2]= -q[1][2]*q[0][0]+q[1][0]*q[0][2]; + 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(); + }; +}; + //stream I/O @@ -84,6 +186,31 @@ s << x.q[2]; return s; } +template +std::istream& operator>>(std::istream &s, Mat3 &x) +{ +s >> x.q[0][0]; +s >> x.q[0][1]; +s >> x.q[0][2]; +s >> x.q[1][0]; +s >> x.q[1][1]; +s >> x.q[1][2]; +s >> x.q[2][0]; +s >> x.q[2][1]; +s >> x.q[2][2]; +return s; +} + +template +std::ostream& operator<<(std::ostream &s, const Mat3 &x) { +s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]<