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-08 21:32:02 +00:00
parent ef02c16f28
commit 2dd7737825
2 changed files with 44 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
quaternion.cc
View File

@ -27,6 +27,35 @@ using namespace LA_Vecmat3;
//and instantize the templates for the types needed
template<typename T>
Quaternion<T>& Quaternion<T>::normalize(T *getnorm, bool unique_sign)
{
T nn=norm();
if(getnorm) *getnorm=nn;
if(unique_sign && q[0]<0) nn= -nn;
*this /= nn;
return *this;
};
template<>
Quaternion<float> & Quaternion<float>::fast_normalize(bool unique_sign)
{
float nn=fast_sqrtinv(normsqr());
if(unique_sign && q[0]<0) nn= -nn;
*this *= nn;
return *this;
}
template<typename T>
Quaternion<T>& Quaternion<T>::fast_normalize(bool unique_sign)
{
return normalize(NULL,unique_sign);
};
template<typename T>
Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &rhs) const
{
@ -82,6 +111,7 @@ 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];
@ -121,7 +151,7 @@ if(grad)
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][2]= q[1]*f0 + q[2]*f1 + q[3]*f2;
grad[1][3]= -q[0]*f0 - q[3]*f1 + q[2]*f2;
grad[2][0]= q[2]*f0 - q[1]*f1 + q[0]*f2;
@ -376,10 +406,6 @@ r *= ::pow(xnorm,y);
return r;
}
//force instantization
#define INSTANTIZE(T) \
template class Quaternion<T>; \

19
quaternion.h
View File

@ -38,7 +38,7 @@ class Quaternion
public:
//just plain old data
T q[4];
//
//methods
Quaternion(void) {};
Quaternion(const T x, const T u=0, const T v=0, const T w=0) {q[0]=x; q[1]=u; q[2]=v; q[3]=w;}; //quaternion from real(s)
Quaternion(const std::complex<T> &rhs) {q[0]=rhs.real(); q[1]=rhs.imag(); q[2]=0; q[3]=0;} //quaternion from complex
@ -50,7 +50,12 @@ public:
inline const T operator[](const int i) const {return q[i];};
inline T& operator[](const int i) {return q[i];};
//get pointer to data transparently
inline operator const T*() const {return q;};
inline operator T*() {return q;};
//operations of quaternions with scalars
void clear() {memset(q,0,4*sizeof(T));}
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) {q[0]+=rhs; return *this;};
Quaternion& operator-=(const T rhs) {q[0]-=rhs; return *this;};
@ -76,23 +81,25 @@ public:
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(T *getnorm=NULL, bool unique_sign=false) {T nn=norm(); if(getnorm) *getnorm=nn; *this /= nn; if(unique_sign && q[0]<0) *this *= (T)-1; return *this;};
Quaternion& fast_normalize(bool unique_sign=false); //using quick 1/sqrt for floats
Quaternion& normalize(T *getnorm=NULL, bool unique_sign=false);
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, 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
Quaternion rotate_match(T *to, const T *from, const T *match) const; //gradient of quaternion rotation which should "to" = "from" transformed by *this to match a given vector by gradient descent
Quaternion commutator(const Quaternion &rhs) const {return *this * rhs - rhs * *this;}; //could be made more efficient
Quaternion anticommutator(const Quaternion &rhs) const {return *this * rhs + rhs * *this;}; //could be made more efficient
T geodesic_distance(const Quaternion &rhs) const {T t=dot(rhs); return acos(2*t*t-1);}; //length of great arc between two quaternions on the S3 hypersphere
//some conversions (for all 12 cases of euler angles go via rotation matrices), cf. also the 1977 NASA paper
void normquat2eulerzyx(T *eul) const; //corresponds to [meul -r -T xyz -d -t -R] or euler2rotmat(...,"xyz",true,true,true)
void euler2normquat(const T *eul, const char *type);
void normquat2euler(T *eul, const char *type) const;
inline void eulerzyx2normquat(const T *eul) {euler2normquat(eul,"zyx");};
//@quaternion to euler via rotation matrix
void normquat2euler(T *eul, const char *type) const;
void euler2normquat(const T *eul, const char *type);
void axis2normquat(const T *axis, const T &angle);
void normquat2axis(T *axis, T &angle) const;
//C-style IO
void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2],q[3]);};