*** empty log message ***

quaternion.cc

@@ -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
+

quaternion.h

@@ -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_ */