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-01 09:22:55 +00:00
parent c466dfadf8
commit 8529a746df
2 changed files with 57 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
quaternion.cc
View File

@ -35,17 +35,45 @@ return Quaternion<T>
);
};
template<typename T>
Quaternion<T> Quaternion<T>::rotateby(const Quaternion<T> &rhs, bool rhs_is_normalized)
{
return rhs * *this * rhs.inverse(); //inefficient reference implementation
}
//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)[3])
void normquat2euler(const Quaternion<T> &q, T *e)
{
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[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)
{
T a = (T).5*angle;
q[0]=cos(a);
T s=sin(a);
q[1]=axis[0]*s;
q[2]=axis[1]*s;
q[3]=axis[2]*s;
}
template<typename T>
void normquat2axis(const Quaternion<T> &q, T *axis, T &angle)
{
T s = sqrt(q[1]*q[1] + q[2]*q[2] +q[3]*q[3]);
angle = 2*atan2(s,q[0]);
s= 1/s;
axis[0]= q[1]*s;
axis[1]= q[2]*s;
axis[2]= q[3]*s;
}
#endif
@ -53,9 +81,22 @@ e[2]= atan2(2*q[2]*q[3]-2*q[0]*q[1],2*q[0]*q[0]+2*q[3]*q[3]-1);
//force instantization
#define INSTANTIZE(T) \
template class Quaternion<T>; \
template void normquat2euler(const Quaternion<T> &q, T (&e)[3]); \
#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)
#ifndef QUAT_NO_DOUBLE
INSTANTIZE(double)
#endif
#ifndef AVOID_GONIOM_FUNC
INSTANTIZE2(float)
#ifndef QUAT_NO_DOUBLE
INSTANTIZE2(double)
#endif
#endif

16
quaternion.h
View File

@ -38,7 +38,7 @@ public:
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
explicit Quaternion(const T* x) {memcpy(q,x,4*sizeof(T));}
explicit Quaternion(const T* x, const int shift=1) {q[0]=0; memcpy(q+shift,x,(4-shift)*sizeof(T));} //for shift=1 quaternion from xyz vector
//compiler generates default copy constructor and assignment operator
@ -71,6 +71,8 @@ public:
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();};
const Quaternion operator/(const Quaternion &rhs) const {return *this * rhs.inverse();};
Quaternion rotateby(const Quaternion &rhs, bool rhs_is_normalized=true); //conjugation-rotation of this by rhs
void rotate(T *rhs) const; //rotate xyz vector by *this
};
@ -97,7 +99,7 @@ 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 (&e)[3]);
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 LA matrix
@ -163,6 +165,16 @@ if(a[0][1]-a[1][0]<0) q[3] = -q[3];
}
//rotation about unit vector axis 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
#endif /* _QUATERNION_H_ */