*** empty log message ***

quaternion.cc

@@ -17,19 +17,45 @@
 */
 
 #include "quaternion.h"
-#include <math.h>
 
-template<>
+
-double Quaternion<double>::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<typename T>
+const Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &rhs) const 
 {
-return sqrt(this->normsqr());
+return Quaternion<T>
-}
+	(
+	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<float>::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<typename T>
+void  normquat2euler(const Quaternion<T> &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<T>; \
+template void normquat2euler(const Quaternion<T> &q, T (&e)[3]); \
+
+INSTANTIZE(float)
+#ifndef QUAT_NO_DOUBLE
+INSTANTIZE(double)
+#endif

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 <http://www.gnu.org/licenses/>.
 */
+
+//this actually should be compilable separately from LA as well as being a part of LA
+
 #ifndef _QUATERNION_H_
 #define _QUATERNION_H_
 
 #include <stdlib.h>
 #include <iostream>
+#include <complex>
+#include <cstring>
+#include <math.h>
+
 
 template <typename T>
 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<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));}
 
 	//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 
+	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]
-			);
-		};	
 	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<typename T>
+void  normquat2euler(const Quaternion<T> &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<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];
+}
+
+template<typename T, typename M>
+void quat2rotmat(Quaternion<T> 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<typename T, typename M>
+void rotmat2normquat(const M &a, Quaternion<T> &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_ */