diff --git a/quaternion.cc b/quaternion.cc index b329415..d71f91b 100644 --- a/quaternion.cc +++ b/quaternion.cc @@ -18,6 +18,8 @@ #include "quaternion.h" +using namespace LA_Quaternion; + //do not replicate this code in each object file, therefore not in .h //and instantize the templates for the types needed @@ -154,7 +156,6 @@ 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 Quaternion::normquat2euler(T *e) const { @@ -184,7 +185,6 @@ axis[0]= q[1]*s; axis[1]= q[2]*s; axis[2]= q[3]*s; } -#endif @@ -192,18 +192,10 @@ axis[2]= q[3]*s; #define INSTANTIZE(T) \ template class Quaternion; \ -#define INSTANTIZE2(T) \ - INSTANTIZE(float) #ifndef QUAT_NO_DOUBLE INSTANTIZE(double) #endif -#ifndef AVOID_GONIOM_FUNC -INSTANTIZE2(float) -#ifndef QUAT_NO_DOUBLE -INSTANTIZE2(double) -#endif -#endif diff --git a/quaternion.h b/quaternion.h index 1fa7491..79f106e 100644 --- a/quaternion.h +++ b/quaternion.h @@ -21,13 +21,16 @@ #ifndef _QUATERNION_H_ #define _QUATERNION_H_ + #ifndef AVOID_STDSTREAM #include #endif +#include #include #include #include +namespace LA_Quaternion { template class Quaternion @@ -73,17 +76,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(bool unique_sign=false) {*this /= norm(); if(unique_sign && q[0]<0) *this *= (T)-1; return *this;}; + 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 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 *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 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 //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; + + //C-style IO + void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2],q[3]);}; + void sprintf(char *f, const char *format) const {::sprintf(f,format,q[0],q[1],q[2],q[3]);}; + int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0],q[1],q[2],q[3]);}; + int sscanf(char *f, const char *format) const {return ::sscanf(f,format,q[0],q[1],q[2],q[3]);}; }; @@ -243,7 +254,61 @@ if(a[0][1]-a[1][0]<0) q[3] = -q[3]; +//Functions - cf. https://en.wikipedia.org/wiki/Quaternion +template +Quaternion exp(const Quaternion &x) +{ +Quaternion r; +T vnorm = sqrt(x[1]*x[1]+x[2]*x[2]+x[3]*x[3]); +r[0] = cos(vnorm); +vnorm = sin(vnorm)/vnorm; +r[1] = x[1] * vnorm; +r[2] = x[2] * vnorm; +r[3] = x[3] * vnorm; +r*= ::exp(x[0]); +return r; +} + + +template +Quaternion log(const Quaternion &x) +{ +Quaternion r; +T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; +T xnorm = vnorm + x[0]*x[0]; +vnorm = sqrt(vnorm); +xnorm = sqrt(xnorm); +r[0] = ::log(xnorm); +T tmp = acos(x[0]/xnorm)/vnorm; +r[1] = x[1] * tmp; +r[2] = x[2] * tmp; +r[3] = x[3] * tmp; +return r; +} + + +template +Quaternion pow(const Quaternion &x, const T &y) +{ +Quaternion r; +T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3]; +T xnorm = vnorm + x[0]*x[0]; +vnorm = sqrt(vnorm); +xnorm = sqrt(xnorm); +T phi = acos(x[0]/xnorm); +r[0] = cos(y*phi); +T tmp = sin(y*phi)/vnorm; +r[1] = x[1] * tmp; +r[2] = x[2] * tmp; +r[3] = x[3] * tmp; +r *= ::pow(xnorm,y); +return r; +} + + +} //namespace + #endif /* _QUATERNION_H_ */ diff --git a/t.cc b/t.cc index 9ea432f..1585fa2 100644 --- a/t.cc +++ b/t.cc @@ -24,9 +24,12 @@ #include "quaternion.h" using namespace std; +using namespace LA_Vecmat3; +using namespace LA_Quaternion; using namespace LA; + extern void test(const NRVec &x); @@ -1883,7 +1886,7 @@ cout< rotmat; quat2rotmat(r,rotmat); cout << rotmat[0][1]< rotmat2(3,3),rotmat3(3,3); Quaternion > rotmatder; rotmatder[0].resize(3,3); @@ -1921,6 +1924,11 @@ Quaternion rrvec = rvec.rotateby(rrr.conjugate()); cout < rotvec; rrr.rotate(rotvec,vec); +Quaternion qq={1.5,2,-3,2.123}; +cout << " test "< #include +#include + +namespace LA_Vecmat3 { //forward declaration template class Mat3; @@ -81,7 +84,11 @@ public: 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 - + //C-style IO + void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2]);}; + void sprintf(char *f, const char *format) const {::sprintf(f,format,q[0],q[1],q[2]);}; + int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0],q[1],q[2]);}; + int sscanf(char *f, const char *format) const {return ::sscanf(f,format,q[0],q[1],q[2]);}; }; @@ -166,6 +173,10 @@ public: r[2][2]= q[1][1]*q[0][0]-q[1][0]*q[0][1]; return r/determinant(); }; + //C-style IO + void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0][0],q[0][1],q[0][2]); ::fprintf(f,format,q[1][0],q[1][1],q[1][2]); ::fprintf(f,format,q[2][0],q[2][1],q[2][2]);}; + int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);}; + }; @@ -213,6 +224,6 @@ return s; } #endif - +}//namespace #endif /* _VECMAT3_H_ */