jiri/LA_library
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
jiri 2020-01-04 20:28:03 +00:00
parent 3b469ad619
commit 2ab28bb5e8
4 changed files with 366 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
quaternion.cc
View File

@ -38,8 +38,39 @@ return Quaternion<T>
//basically the same code as in normquat2rotmat, but avoiding extra storage //basically the same code as in normquat2rotmat, but avoiding extra storage
template<typename T> template<typename T>
void Quaternion<T>::rotate(T *to, const T *from) const void Quaternion<T>::rotate(T *to, const T *from, Quaternion<T> *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] + 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[2]+q[0]*q[3]) * from[1] +
2*(q[1]*q[3]-q[0]*q[2]) * from[2]; 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] + 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[2]*q[3]-q[0]*q[1]) * from[1] +
(2*q[0]*q[0]-1+2*q[3]*q[3]) * from[2]; (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; return r;
} }
template<typename T>
Quaternion<T> Quaternion<T>::rotate_match(T *to, const T *from, const T *match) const
{
Quaternion<T> grad[3];
Quaternion<T>::rotate(to, from, grad);
Quaternion<T> 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 //optionally skip this for microcontrollers if not needed
//note that C++ standard headers should use float version of the functions for T=float //note that C++ standard headers should use float version of the functions for T=float
#ifndef AVOID_GONIOM_FUNC #ifndef AVOID_GONIOM_FUNC
template<typename T> template<typename T>
void normquat2euler(const Quaternion<T> &q, T *e) void Quaternion<T>::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[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]); 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<typename T> template<typename T>
void axis2normquat(const T *axis, const T &angle, Quaternion<T> &q) void Quaternion<T>::axis2normquat(const T *axis, const T &angle)
{ {
T a = (T).5*angle; T a = (T).5*angle;
q[0]=cos(a); q[0]=cos(a);
@ -86,7 +149,7 @@ q[3]=axis[2]*s;
} }
template<typename T> template<typename T>
void normquat2axis(const Quaternion<T> &q, T *axis, T &angle) void Quaternion<T>::normquat2axis(T *axis, T &angle) const
{ {
T s = sqrt(q[1]*q[1] + q[2]*q[2] +q[3]*q[3]); T s = sqrt(q[1]*q[1] + q[2]*q[2] +q[3]*q[3]);
angle = 2*atan2(s,q[0]); angle = 2*atan2(s,q[0]);
@ -104,9 +167,6 @@ axis[2]= q[3]*s;
template class Quaternion<T>; \ template class Quaternion<T>; \
#define INSTANTIZE2(T) \ #define INSTANTIZE2(T) \
template void normquat2euler(const Quaternion<T> &q, T *e); \
template void axis2normquat(const T *axis, const T &angle, Quaternion<T> &q); \
template void normquat2axis(const Quaternion<T> &q, T *axis, T &angle); \
INSTANTIZE(float) INSTANTIZE(float)

129
quaternion.h
View File

@ -43,14 +43,14 @@ public:
//compiler generates default copy constructor and assignment operator //compiler generates default copy constructor and assignment operator
//formal indexing //formal indexing
const T operator[](const int i) const {return this->q[i];}; inline const T operator[](const int i) const {return q[i];};
T& operator[](const int i) {return this->q[i];}; inline T& operator[](const int i) {return q[i];};
//operations of quaternions with scalars //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 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) {q[0]+=rhs; return *this;};
Quaternion& operator-=(const T rhs) {this->q[0]-=rhs; return *this;}; Quaternion& operator-=(const T rhs) {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; q[1]*=rhs; q[2]*=rhs; q[3]*=rhs; return *this;};
Quaternion& operator/=(const T rhs) {return *this *= ((T)1/rhs);}; 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;};
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 //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 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) {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) {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;};
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 {return Quaternion(*this) -= rhs;}; const Quaternion operator-(const Quaternion &rhs) const {return Quaternion(*this) -= rhs;};
const Quaternion operator*(const Quaternion &rhs) const; 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& conjugateme(void) {q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; return *this;}
Quaternion conjugate(void) const {return Quaternion(*this).conjugateme();} 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 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 norm(void) const {return sqrt(this->normsqr());}; T normsqr(void) const {return dot(*this);};
Quaternion& normalize(bool unique_sign=false) {*this /= this->norm(); if(unique_sign && q[0]<0) *this *= (T)-1; return *this;}; T norm(void) const {return sqrt(normsqr());};
Quaternion inverse(void) const {return Quaternion(*this).conjugateme()/this->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();}; const Quaternion operator/(const Quaternion &rhs) const {return *this * rhs.inverse();};
Quaternion rotateby(const Quaternion &rhs); //conjugation-rotation of this by NORMALIZED rhs 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<T> *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<typename T>
void normquat2euler(const Quaternion<T> &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 //conversion between quanternions and rotation matrices
// //
template<typename T, typename M> template<typename T, typename M>
void normquat2rotmat(const Quaternion<T> &q, M &a) void normquat2rotmat(const Quaternion<T> &q, M &a)
{ {
a[0][0] = 2*q[0]*q[0]-1+2*q[1]*q[1]; //some explicit common subexpression optimizations
a[0][1] = 2*(q[1]*q[2]+q[0]*q[3]); {
a[0][2] = 2*(q[1]*q[3]-q[0]*q[2]); T q00= q[0]*q[0];
a[1][0] = 2*(q[1]*q[2]-q[0]*q[3]); T q11= q[1]*q[1];
a[1][1] = 2*q[0]*q[0]-1+2*q[2]*q[2]; T q22= q[2]*q[2];
a[1][2] = 2*(q[2]*q[3]+q[0]*q[1]); T q33= q[3]*q[3];
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[0][0] = q00+q11-q22-q33;
a[2][2] = 2*q[0]*q[0]-1+2*q[3]*q[3]; 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<typename T, typename M> template<typename T, typename M>
@ -127,6 +147,57 @@ if(!already_normalized) q.normalize();
normquat2rotmat(q,a); normquat2rotmat(q,a);
} }
//derivative of the rotation matrix by quaternion elements
template<typename T, typename M>
void normquat2rotmatder(const Quaternion<T> &q, Quaternion<M> &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 //normalized quaternion from rotation matrix
//convention compatible with the paper on MEMS sensors by Sebastian O.H. Madgwick //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<typename T>
void axis2normquat(const T *axis, const T &angle, Quaternion<T> &q);
template<typename T>
void normquat2axis(const Quaternion<T> &q, T *axis, T &angle);
#endif

68
t.cc
View File

@ -20,6 +20,8 @@
#include <time.h> #include <time.h>
#include "la.h" #include "la.h"
#include "vecmat3.h"
#include "quaternion.h"
using namespace std; using namespace std;
using namespace LA; using namespace LA;
@ -1848,7 +1850,7 @@ double det=determinant_destroy(a);
cout << "det= "<<det<<endl; cout << "det= "<<det<<endl;
} }
if(1) if(0)
{ {
bitvector v(3); bitvector v(3);
v.assign(0,0); v.assign(0,0);
@ -1858,5 +1860,69 @@ cin >>v;
cout <<v; cout <<v;
} }
if(1)
{
Quaternion<double> q(1.);
Quaternion<double> p,r(q);
p=r;
q+=p;
q-=r;
r=p+q;
r+=Quaternion<double>(0,8,8,8);
r+= 2.;
r/=4.;
cout<<r<<endl;
Quaternion<double> s(-1,2,1,2);
Quaternion<double> t=r*s.conjugate();
cout <<t<<" "<<t[0]<<" "<<t.norm()<<endl;
Quaternion<double>tt = t.inverse();
cout <<tt<<" "<<t*tt<<endl;
r=s/t;
r.normalize();
cout<<r<<endl;
Mat3<double> rotmat;
quat2rotmat(r,rotmat);
cout << rotmat[0][1]<<endl;
r.normalize(true);
NRMat<double> rotmat2(3,3),rotmat3(3,3);
Quaternion<NRMat<double> > 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<<endl;
cout << rotmat3<<endl;
Quaternion<double> rr;
rotmat2normquat(rotmat2,rr);
cout <<"orig "<<r<<endl;
cout <<"reconstruct "<<rr<<endl;
cout <<"diff " <<r-rr<<endl;
Vec3<double> eul;
r.normquat2euler(eul);
cout<<eul[0]<<" " <<eul[1]<<" "<<eul[2]<<endl;
Vec3<double> axis={0,1/sqrt(2),1/sqrt(2)};
Quaternion<double> rrr;
rrr.axis2normquat(axis,0.5);
NRVec<double> axis2(3);
double angle;
rrr.normquat2axis(&axis2[0],angle);
cout <<"back angle "<<angle<<" "<<axis2<<endl;
Vec3<double> vec={1,2,3};
Quaternion<double> qvec(vec);
Quaternion<double> rvec = qvec.rotateby(rrr);
Quaternion<double> rvec2 = qvec.rotateby(rrr.conjugate());
cout <<rvec<<endl;
cout <<rvec2<<endl;
Quaternion<double> rrvec = rvec.rotateby(rrr.conjugate());
cout <<rrvec<<endl;
Vec3<double> rotvec;
rrr.rotate(rotvec,vec);
}
} }

145
vecmat3.h
View File

@ -28,42 +28,144 @@
#include <cstring> #include <cstring>
#include <math.h> #include <math.h>
//forward declaration
template <typename T> class Mat3;
template <typename T> template <typename T>
class Vec3 class Vec3
{ {
friend class Mat3<T>;
public: public:
//just plain old data //just plain old data
T q[3]; T q[3];
// //
Vec3(void) {}; 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) 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 //compiler generates default copy constructor and assignment operator
//formal indexing //formal indexing
const T operator[](const int i) const {return this->q[i];}; inline const T operator[](const int i) const {return q[i];};
T& operator[](const int i) {return this->q[i];}; inline T& operator[](const int i) {return q[i];};
//operations of Vec3s with scalars //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);}; 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;};
const Vec3 operator/(const T rhs) const {return Vec3(*this) /= rhs;}; const Vec3 operator/(const T rhs) const {return Vec3(*this) /= rhs;};
//Vec3 algebra //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 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) {q[0]+=rhs.q[0];q[1]+=rhs.q[1];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;};
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 {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 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 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 normsqr(void) const {return dot(*this);};
T norm(void) const {return sqrt(this->normsqr());}; T norm(void) const {return sqrt(normsqr());};
Vec3& normalize(void) {*this /= this->norm(); return *this;}; Vec3& normalize(void) {*this /= norm(); return *this;};
}; const Vec3 operator*(const Mat3<T> &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 <typename T>
class Mat3
{
friend class Vec3<T>;
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<T> operator*(const Vec3<T> &rhs) const
{
Vec3<T> 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 //stream I/O
@ -84,6 +186,31 @@ s << x.q[2];
return s; return s;
} }
template <typename T>
std::istream& operator>>(std::istream &s, Mat3<T> &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 <typename T>
std::ostream& operator<<(std::ostream &s, const Mat3<T> &x) {
s << x.q[0][0]<<" "<< x.q[0][1]<<" " << x.q[0][2]<<std::endl;
s << x.q[1][0]<<" "<< x.q[1][1]<<" " << x.q[1][2]<<std::endl;
s << x.q[2][0]<<" "<< x.q[2][1]<<" " << x.q[2][2]<<std::endl;
return s;
}
#endif /* _VECMAT3_H_ */ #endif /* _VECMAT3_H_ */