fixed some issues

added new pose/turn detections
new helper classes
define-flags for libEigen

math/Quaternion.h

@@ -0,0 +1,197 @@
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "Matrix3.h"
+
+class Quaternion {
+
+public:
+
+	float w;
+	float x, y, z;
+
+	/** empty ctor */
+	Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {
+		;
+	}
+
+	/** valued ctor */
+	Quaternion(float w, float x, float y, float z) :  w(w), x(x), y(y), z(z) {
+		;
+	}
+
+	/** construct from Euler angles in radians */
+	static Quaternion fromEuler(float x, float y, float z) {
+		Quaternion q;
+		q.setEuler(x,y,z);
+		return q;
+	}
+
+	static Quaternion fromAxisAngle(float angle, float x, float y, float z) {
+		const float qx = x * std::sin(angle/2);
+		const float qy = y * std::sin(angle/2);
+		const float qz = z * std::sin(angle/2);
+		const float qw = std::cos(angle/2);
+		return Quaternion(qw,	qx,qy,qz);
+	}
+
+
+	void normalize() {
+		*this *= 1.0 / std::sqrt( x*x + y*y + z*z + w*w );
+	}
+
+	inline float dotProduct(const Quaternion& q2) const {
+		return (x * q2.x) + (y * q2.y) + (z * q2.z) + (w * q2.w);
+	}
+
+
+	/** linear interpolation between q1 and q2 */
+	static Quaternion lerp( Quaternion q1, Quaternion q2, float time) {
+		const float scale = 1.0f - time;
+		return (q1*scale) + (q2*time);
+	}
+
+	/** spherical interpolation between q1 and q2 */
+	static Quaternion slerp( Quaternion q1, Quaternion q2, float time, float threshold = 0.05)  {
+
+		float angle = q1.dotProduct(q2);
+
+		// make sure we use the short rotation
+		if (angle < 0.0f) {
+			q1 *= -1.0f;
+			angle *= -1.0f;
+		}
+
+		if (angle <= (1-threshold)) {
+			// spherical interpolation
+			const float theta = acosf(angle);
+			const float invsintheta = 1.0/(sinf(theta));
+			const float scale = sinf(theta * (1.0f-time)) * invsintheta;
+			const float invscale = sinf(theta * time) * invsintheta;
+			return (q1*scale) + (q2*invscale);
+		} else {
+			// linear interpolation
+			return Quaternion::lerp(q1,q2,time);
+		}
+
+	}
+
+
+	/** multiply by scalar */
+	Quaternion& operator *= (float val) {
+		x *= val;
+		y *= val;
+		z *= val;
+		w *= val;
+		return *this;
+	}
+
+	/** multiply by scalar */
+	Quaternion operator * (float val) const {
+		return Quaternion(w*val, x*val, y*val, z*val);
+	}
+
+	/** multiply by other quaterion */
+	Quaternion& operator *= (const Quaternion& other) {
+		return (*this = other * (*this));
+	}
+
+	/** multiply by other quaterion */
+	Quaternion operator * (const Quaternion& other) const {
+		Quaternion tmp;
+		tmp.w = (other.w * w) - (other.x * x) - (other.y * y) - (other.z * z);
+		tmp.x = (other.w * x) + (other.x * w) + (other.y * z) - (other.z * y);
+		tmp.y = (other.w * y) + (other.y * w) + (other.z * x) - (other.x * z);
+		tmp.z = (other.w * z) + (other.z * w) + (other.x * y) - (other.y * x);
+		return tmp;
+	}
+
+	/** add two quaternions */
+	Quaternion operator + (const Quaternion& b) const {
+		return Quaternion(w+b.w,		x+b.x, y+b.y, z+b.z);
+	}
+
+
+	/** set from euler angles (in radians) */
+	void setEuler(float x, float y, float z) {
+
+		double angle;
+
+		angle = x * 0.5;
+		const double sr = std::sin(angle);
+		const double cr = std::cos(angle);
+
+		angle = y * 0.5;
+		const double sp = std::sin(angle);
+		const double cp = std::cos(angle);
+
+		angle = z * 0.5;
+		const double sy = std::sin(angle);
+		const double cy = std::cos(angle);
+
+		const double cpcy = cp * cy;
+		const double spcy = sp * cy;
+		const double cpsy = cp * sy;
+		const double spsy = sp * sy;
+
+		this->x = (float)(sr * cpcy - cr * spsy);
+		this->y = (float)(cr * spcy + sr * cpsy);
+		this->z = (float)(cr * cpsy - sr * spcy);
+		this->w = (float)(cr * cpcy + sr * spsy);
+		normalize();
+
+	}
+
+	/** convert to euler angles */
+	Vector3 toEuler() const {
+
+		Vector3 res;
+
+		const double sqw = w*w;
+		const double sqx = x*x;
+		const double sqy = y*y;
+		const double sqz = z*z;
+		const double test = 2.0 * (y*w - x*z);
+
+		if (near(test, 1.0)) {
+			// heading = rotation about z-axis
+			res.z = (float) (-2.0*atan2(x, w));
+			// bank = rotation about x-axis
+			res.x = 0;
+			// attitude = rotation about y-axis
+			res.y = (float) (M_PI/2.0);
+		} else if (near(test, -1.0)) {
+			// heading = rotation about z-axis
+			res.z = (float) (2.0*std::atan2(x, w));
+			// bank = rotation about x-axis
+			res.x = 0;
+			// attitude = rotation about y-axis
+			res.y = (float) (M_PI/-2.0);
+		} else {
+			// heading = rotation about z-axis
+			res.z = (float) std::atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw));
+			// bank = rotation about x-axis
+			res.x = (float) std::atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw));
+			// attitude = rotation about y-axis
+			res.y = (float) std::asin( clamp(test, -1.0, 1.0) );
+		}
+
+		return res;
+
+	}
+
+private:
+
+	static inline bool near(double d1, double d2) {
+		return std::abs(d1-d2) < 0.00001;
+	}
+
+	static inline double clamp(double val, double min, double max) {
+		if (val < min) {return min;}
+		if (val > max) {return max;}
+		return val;
+	}
+
+};
+
+#endif // QUATERNION_H