*** empty log message ***
This commit is contained in:
jiri
parent
3b469ad619
commit
2ab28bb5e8
|
|
@ -38,8 +38,39 @@ return Quaternion<T>
|
|||
|
||||
//basically the same code as in normquat2rotmat, but avoiding extra storage
|
||||
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] +
|
||||
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<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
|
||||
//note that C++ standard headers should use float version of the functions for T=float
|
||||
#ifndef AVOID_GONIOM_FUNC
|
||||
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[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>
|
||||
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;
|
||||
q[0]=cos(a);
|
||||
|
|
@ -86,7 +149,7 @@ q[3]=axis[2]*s;
|
|||
}
|
||||
|
||||
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]);
|
||||
angle = 2*atan2(s,q[0]);
|
||||
|
|
@ -104,9 +167,6 @@ axis[2]= q[3]*s;
|
|||
template class Quaternion<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)
|
||||
|
|
|
|||
129
quaternion.h
129
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<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
|
||||
//
|
||||
template<typename T, typename M>
|
||||
void normquat2rotmat(const Quaternion<T> &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<typename T, typename M>
|
||||
|
|
@ -127,6 +147,57 @@ if(!already_normalized) q.normalize();
|
|||
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
|
||||
//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
68
t.cc
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <time.h>
|
||||
#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= "<<det<<endl;
|
||||
}
|
||||
|
||||
if(1)
|
||||
if(0)
|
||||
{
|
||||
bitvector v(3);
|
||||
v.assign(0,0);
|
||||
|
|
@ -1858,5 +1860,69 @@ cin >>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
145
vecmat3.h
|
|
@ -28,42 +28,144 @@
|
|||
#include <cstring>
|
||||
#include <math.h>
|
||||
|
||||
//forward declaration
|
||||
template <typename T> class Mat3;
|
||||
|
||||
|
||||
template <typename T>
|
||||
class Vec3
|
||||
{
|
||||
friend class Mat3<T>;
|
||||
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<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
|
||||
|
|
@ -84,6 +186,31 @@ s << x.q[2];
|
|||
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_ */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue