From c466dfadf8e5e16738dad3fa0301c641a095658b Mon Sep 17 00:00:00 2001 From: jiri Date: Tue, 31 Dec 2019 17:49:39 +0000 Subject: [PATCH] *** empty log message *** --- quaternion.cc | 42 +++++++++++++++++----- quaternion.h | 96 ++++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 117 insertions(+), 21 deletions(-) diff --git a/quaternion.cc b/quaternion.cc index 1f70de8..dd0516b 100644 --- a/quaternion.cc +++ b/quaternion.cc @@ -17,19 +17,45 @@ */ #include "quaternion.h" -#include -template<> -double Quaternion::norm(void) const + +//do not replicate this code in each object file, therefore not in .h +//and instantize the templates for the types needed + + +template +const Quaternion Quaternion::operator*(const Quaternion &rhs) const { -return sqrt(this->normsqr()); -} +return Quaternion + ( + this->q[0]*rhs.q[0]-this->q[1]*rhs.q[1]-this->q[2]*rhs.q[2]-this->q[3]*rhs.q[3], + this->q[0]*rhs.q[1]+this->q[1]*rhs.q[0]+this->q[2]*rhs.q[3]-this->q[3]*rhs.q[2], + this->q[0]*rhs.q[2]+this->q[2]*rhs.q[0]+this->q[3]*rhs.q[1]-this->q[1]*rhs.q[3], + this->q[0]*rhs.q[3]+this->q[3]*rhs.q[0]+this->q[1]*rhs.q[2]-this->q[2]*rhs.q[1] + ); +}; -template<> -float Quaternion::norm(void) const + +//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 normquat2euler(const Quaternion &q, T (&e)[3]) { -return sqrtf(this->normsqr()); +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); } +#endif +//force instantization +#define INSTANTIZE(T) \ +template class Quaternion; \ +template void normquat2euler(const Quaternion &q, T (&e)[3]); \ + +INSTANTIZE(float) +#ifndef QUAT_NO_DOUBLE +INSTANTIZE(double) +#endif diff --git a/quaternion.h b/quaternion.h index 280bece..9b9b6fb 100644 --- a/quaternion.h +++ b/quaternion.h @@ -15,11 +15,18 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ + +//this actually should be compilable separately from LA as well as being a part of LA + #ifndef _QUATERNION_H_ #define _QUATERNION_H_ #include #include +#include +#include +#include + template class Quaternion @@ -29,13 +36,15 @@ public: T q[4]; // 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 + 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 &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));} //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];}; + T& operator[](const int i) {return this->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 @@ -49,24 +58,17 @@ public: const Quaternion operator/(const T rhs) const {return Quaternion(*this) /= rhs;}; //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;}; 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->q[0]*rhs.q[0]-this->q[1]*rhs.q[1]-this->q[2]*rhs.q[2]-this->q[3]*rhs.q[3], - this->q[0]*rhs.q[1]+this->q[1]*rhs.q[0]+this->q[2]*rhs.q[3]-this->q[3]*rhs.q[2], - this->q[0]*rhs.q[2]+this->q[2]*rhs.q[0]+this->q[3]*rhs.q[1]-this->q[1]*rhs.q[3], - this->q[0]*rhs.q[3]+this->q[3]*rhs.q[0]+this->q[1]*rhs.q[2]-this->q[2]*rhs.q[1] - ); - }; + 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; + 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();}; const Quaternion operator/(const Quaternion &rhs) const {return *this * rhs.inverse();}; }; @@ -93,6 +95,74 @@ return s; } +//"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R] +template +void normquat2euler(const Quaternion &q, T (&e)[3]); + + +//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 + +//conversion between quanternions and rotation matrices +// +template +void normquat2rotmat(const Quaternion &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]; +} + +template +void quat2rotmat(Quaternion q, M &a, const bool already_normalized=false) +{ +if(!already_normalized) q.normalize(); +normquat2rotmat(q,a); +} + + +//normalized quaternion from rotation matrix +//convention compatible with the paper on MEMS sensors by Sebastian O.H. Madgwick +//the rotation matrix correcponds to transpose of (4) in Sarabandi and Thomas paper +//where the method is described + +template +void rotmat2normquat(const M &a, Quaternion &q) +{ +T tr= a[0][0]+a[1][1]+a[2][2]; +if(tr>=0) + { + q[0] = (T).5*sqrt((T)1. +tr); + q[1] = (T).5*sqrt((T)1. +a[0][0]-a[1][1]-a[2][2]); + q[2] = (T).5*sqrt((T)1. -a[0][0]+a[1][1]-a[2][2]); + q[3] = (T).5*sqrt((T)1. -a[0][0]-a[1][1]+a[2][2]); + } +else + { + T a12p = a[0][1]+a[1][0]; + T a12m = a[0][1]-a[1][0]; + T a13p = a[0][2]+a[2][0]; + T a13m = a[0][2]-a[2][0]; + T a23p = a[1][2]+a[2][1]; + T a23m = a[1][2]-a[2][1]; + + q[0] = (T).5*sqrt((a23m*a23m+a13m*a13m+a12m*a12m)/((T)3.-tr)); + q[1] = (T).5*sqrt((a23m*a23m+a12p*a12p+a13p*a13p)/((T)3.-a[0][0]+a[1][1]+a[2][2])); + q[2] = (T).5*sqrt((a13m*a13m+a12p*a12p+a23p*a23p)/((T)3.+a[0][0]-a[1][1]+a[2][2])); + q[3] = (T).5*sqrt((a12m*a12m+a13p*a13p+a23p*a23p)/((T)3.+a[0][0]+a[1][1]-a[2][2])); + } + +if(a[1][2]-a[2][1]<0) q[1] = -q[1]; +if(a[2][0]-a[0][2]<0) q[2] = -q[2]; +if(a[0][1]-a[1][0]<0) q[3] = -q[3]; +} + + #endif /* _QUATERNION_H_ */