From 2dd7737825a47e3224953ab7c6f6a39e7b18089b Mon Sep 17 00:00:00 2001
From: jiri <jiri>
Date: Wed, 8 Jan 2020 21:32:02 +0000
Subject: [PATCH] *** empty log message ***

---
 quaternion.cc | 36 +++++++++++++++++++++++++++++++-----
 quaternion.h  | 19 +++++++++++++------
 2 files changed, 44 insertions(+), 11 deletions(-)

diff --git a/quaternion.cc b/quaternion.cc
index 8333fa4..be772bd 100644
--- a/quaternion.cc
+++ b/quaternion.cc
@@ -27,6 +27,35 @@ using namespace LA_Vecmat3;
 //and instantize the templates for the types needed
 
 
+template<typename T>
+Quaternion<T>& Quaternion<T>::normalize(T *getnorm, bool unique_sign) 
+{
+T nn=norm(); 
+if(getnorm) *getnorm=nn; 
+if(unique_sign && q[0]<0)  nn= -nn;
+*this /= nn; 
+return *this;
+};
+
+template<>
+Quaternion<float> & Quaternion<float>::fast_normalize(bool unique_sign)
+{
+float nn=fast_sqrtinv(normsqr()); 
+if(unique_sign && q[0]<0)  nn= -nn;       
+*this *= nn;
+return *this;
+}
+
+template<typename T>
+Quaternion<T>& Quaternion<T>::fast_normalize(bool unique_sign)  
+{
+return normalize(NULL,unique_sign);
+};
+
+
+
+
+
 template<typename T>
 Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &rhs) const 
 {
@@ -82,6 +111,7 @@ to[1] = (q00+q22-q11-q33) * from[1];
 to[2] = (q00+q33-q11-q22) * from[2];
 }
 
+
 T q01= q[0]*q[1];
 T q02= q[0]*q[2];
 T q03= q[0]*q[3];
@@ -121,7 +151,7 @@ if(grad)
 
         grad[1][0]= -q[3]*f0 + q[0]*f1 + q[1]*f2;
         grad[1][1]=  q[2]*f0 - q[1]*f1 + q[0]*f2;
-        grad[1][2]=  q[1]*f0 + q[2]*f1 + q[3]*2;
+        grad[1][2]=  q[1]*f0 + q[2]*f1 + q[3]*f2;
         grad[1][3]= -q[0]*f0 - q[3]*f1 + q[2]*f2;
 
         grad[2][0]=  q[2]*f0 - q[1]*f1 + q[0]*f2;
@@ -376,10 +406,6 @@ r *= ::pow(xnorm,y);
 return r;
 }
 
-
-
-
-
 //force instantization
 #define INSTANTIZE(T) \
 template class Quaternion<T>; \
diff --git a/quaternion.h b/quaternion.h
index 5408889..dd2205a 100644
--- a/quaternion.h
+++ b/quaternion.h
@@ -38,7 +38,7 @@ class Quaternion
 public:
 	//just plain old data
 	T q[4];
-	//
+	//methods
 	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
@@ -50,7 +50,12 @@ public:
 	inline const T operator[](const int i) const {return q[i];};
 	inline T& operator[](const int i) {return q[i];};
 
+	//get pointer to data transparently
+        inline operator const T*() const {return q;};
+        inline operator T*() {return q;};
+
 	//operations of quaternions with scalars
+	void clear() {memset(q,0,4*sizeof(T));}
 	Quaternion& operator=(const T x) {q[0]=x; memset(&q[1],0,3*sizeof(T)); return *this;};  //quaternion from real 
 	Quaternion& operator+=(const T rhs) {q[0]+=rhs; return *this;};
 	Quaternion& operator-=(const T rhs) {q[0]-=rhs; return *this;};
@@ -76,23 +81,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(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& fast_normalize(bool unique_sign=false); //using quick 1/sqrt for floats
+	Quaternion& normalize(T *getnorm=NULL, bool unique_sign=false);
 	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<T> *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 rotate_match(T *to, const T *from, const T *match) const; //gradient of quaternion rotation which should "to" = "from" transformed by *this to match a given vector by gradient descent
 	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
+	T geodesic_distance(const Quaternion &rhs) const {T t=dot(rhs); return acos(2*t*t-1);}; //length of great arc between two quaternions on the S3 hypersphere
 
 	//some conversions (for all 12 cases of euler angles go via rotation matrices), cf. also the 1977 NASA paper
 	void normquat2eulerzyx(T *eul) const;  //corresponds to [meul -r -T xyz -d -t -R] or  euler2rotmat(...,"xyz",true,true,true)
-	void euler2normquat(const T *eul, const char *type);
-	void normquat2euler(T *eul, const char *type) const; 
 	inline void eulerzyx2normquat(const T *eul) {euler2normquat(eul,"zyx");};  
-	//@quaternion to euler via rotation matrix
+	void normquat2euler(T *eul, const char *type) const; 
+	void euler2normquat(const T *eul, const char *type);
 	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]);};