CGII/framework/include/cgv/math/quat.h

#pragma once

#include <iostream>
#include <cmath>
#include <cassert>
#include <cgv/math/vec.h>
#include <cgv/math/mat.h>

namespace cgv {
namespace math {

/**
* Quaterions are stored in vectors in the following order:
* [w, x, y, z]  -> w + x*i + y*j + z*k
*/


// returns the product of quaternion v and q
template <typename T>
vec<T> quat_multiply(const vec<T>& v,const vec<T>& q)
{ 
	assert(v.dim() == 4);
	assert(q.dim() == 4);
	vec<T> r(4);
	r(0) = v[0]*q(0) - v[1]*q(1) - v[2]*q(2) - v[3]*q(3);
	r(1) = v[0]*q(1) + v[1]*q(0) + v[2]*q(3) - v[3]*q(2);	 
	r(2) = v[0]*q(2) + v[2]*q(0) + v[3]*q(1) - v[1]*q(3);
	r(3) = v[0]*q(3) + v[3]*q(0) + v[1]*q(2) - v[2]*q(1);
	return r;
}

///returns a quaterion q defined by the given axis and angle
template <typename T>
vec<T> axis_angle_2_quat(vec<T> axis, T angle)
{
	angle = (T)angle*3.14159/360.0;
	T s = (T)sinf((float)angle );
	axis.normalize();
	
	vec<T> q(4);
	
	q(0) = cos(angle );
	q(1) = axis(0) * s;
	q(2) = axis(1) * s;
	q(3) = axis(2) * s;
	return q;
}

///extract the axis and angle in degree from quaternion q
template <typename T>
void quat_2_axis_angle(const vec<T>& q,vec<T>& axis, T& angle)
{
	axis.resize(3);
	T s = sqrt((q(1) * q(1)) + (q(2) * q(2)) + (q(3) * q(3)));

	if (s == 0) 
	{
	 	axis(0) = 1;
		axis(1) = 0;
		axis(2) = 0;
		angle = 0;

	} 
	else 
	{
		axis(0) = q(1) / s;
		axis(1) = q(2) / s;
		axis(2) = q(3) / s;
		angle = 2.0 * acos(q(0));
	}
}


///returns the rotation of  vector v by quaternion q
template <typename T>
vec<T> quat_rotate(const vec<T>&q, const vec<T>& v)
{
	
	assert(v.dim() >= 3);
	assert(q.dim() == 4);
	
	vec<T> pos(v.dim());

	T w =  - q(1)*v(0) - q(2)*v(1) - q(3)*v(2);
	T x =    q(0)*v(0) + q(2)*v(2) - q(3)*v(1);
	T y =    q(0)*v(1) + q(3)*v(0) - q(1)*v(2);
	T z =    q(0)*v(2) + q(1)*v(1) - q(2)*v(0);
	
	pos(0) = -w*q(1) + x*q(0) - y*q(3) + z*q(2);
	pos(1) = -w*q(2) + y*q(0) - z*q(1) + x*q(3);
	pos(2) = -w*q(3) + z*q(0) - x*q(2) + y*q(1);
	int i;
	for(i = 3; i < v.dim(); i++)
		pos(i) = v(i);
	return pos;

}

///returns the conjugate quaternion q
template <typename T>
vec<T> quat_conj(const vec<T>& q)
{
	vec<T> r(4);
	r(0) =  q(0);
	r(1) = -q(1);
	r(2) = -q(2);
	r(3) = -q(3);
}

///returns  a normalize version of quaternion q
template <typename T>
vec<T> quat_normalize(const vec<T>& q)
{
	T magnitude = sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));

	vec<T> r(4);
	r(0)= q(0) / magnitude;
	r(1)= q(1) / magnitude;
	r(2)= q(2) / magnitude;
	r(3)= q(3) / magnitude;	
	return r;
}

///return  the inverse of quaterion q
template <typename T>
vec<T> quat_inv(const vec<T>& q)
{
	T magnitude = sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));

	vec<T> r(4);
	r(0)= q(0) / magnitude;
	r(1)= -q(1) / magnitude;
	r(2)= -q(2) / magnitude;
	r(3)= -q(3) / magnitude;
	return r;
}

///convert unit quaternion to 3x3 rotation matrix
template <typename T>
mat<T> quat_2_mat_33(const vec<T>& q)
{
	assert(q.dim() == 4);
	mat<T> m(3,3);
	T xx      = q[1] * q[1];
	T xy      = q[1] * q[2];
	T xz      = q[1] * q[3];
	T xw      = q[1] * q[0];
	T yy      = q[2] * q[2];
	T yz      = q[2] * q[3];
	T yw      = q[2] * q[0];
	T zz      = q[3] * q[3];
	T zw      = q[3] * q[0];
	m(0,0)  = 1 - 2 * ( yy + zz );
	m(0,1)  =     2 * ( xy - zw );
	m(0,2)  =     2 * ( xz + yw );
	m(1,0)  =     2 * ( xy + zw );
	m(1,1)  = 1 - 2 * ( xx + zz );
	m(1,2)  =     2 * ( yz - xw );
	m(2,0)  =     2 * ( xz - yw );
	m(2,1)  =     2 * ( yz + xw );
	m(2,2)  = 1 - 2 * ( xx + yy );
	return m;
}

///convert unit quaternion to 4x4 rotation matrix
template <typename T>
mat<T> quat_2_mat_44(const vec<T>& q) 
{
	assert(q.dim() == 4);
	
	mat<T> m(4,4);
	T xx      = q[1] * q[1];
	T xy      = q[1] * q[2];
	T xz      = q[1] * q[3];
	T xw      = q[1] * q[0];
	T yy      = q[2] * q[2];
	T yz      = q[2] * q[3];
	T yw      = q[2] * q[0];
	T zz      = q[3] * q[3];
	T zw      = q[3] * q[0];
	m(0,0)  = 1 - 2 * ( yy + zz );
	m(0,1)  =     2 * ( xy - zw );
	m(0,2)  =     2 * ( xz + yw );
	m(1,0)  =     2 * ( xy + zw );
	m(1,1)  = 1 - 2 * ( xx + zz );
	m(1,2)  =     2 * ( yz - xw );
	m(2,0)  =     2 * ( xz - yw );
	m(2,1)  =     2 * ( yz + xw );
	m(2,2) = 1 - 2 * ( xx + yy );
	m(3,0) = m(3,1) = m(3,2) = m(0,3) = m(1,3) = m(2,3) = 0;
	m(3,3) = 1;
	return m;
	
}

	
	/*
/// A column vector class.
template <typename T>
class quat
{	

private:
	///pointer to _data storage
	T v[4];
	
public:

	quat()
	{
		v[0]    = 0;
		v[1]    = 0;
		v[2]    = 0;
		v[3]    = 1;
	}

	quat(const T& x,const T& y,const T& z,const T w)
	{
		v[0]    = x;
		v[1]    = y;
		v[2]    = z;
		v[3]    = w;
		
	}

	quat(const vec<T>& axis,const T angle)
	{
		set(axis,angle);
	}


	quat(const quat &q)
	{
		for (int i=0; i<4; i++)
			v[i]=q(i);
	}

	quat& operator=(const quat &rhs) 
	{
		if (this != &rhs)
		{
			for (int i=0; i<4; i++)
				v[i]=rhs(i);
		}
		return *this;
	}

	void conjugate()
    {
		v[0]    = -v[0];
		v[1]    = -v[1];
		v[2]    = -v[2];
    }

	quat<T>& operator*=(const quat<T>& q)
    {
		T w = v[3]*q(3) - v[0]*q(0) - v[1]*q(1) - v[2]*q(2);
		T x = v[3]*q(0) + v[0]*q(3) + v[1]*q(2) - v[2]*q(1);
		T y = v[3]*q(1) + v[1]*q(3) + v[2]*q(0) - v[0]*q(2);
		T z = v[3]*q(2) + v[2]*q(3) + v[0]*q(1) - v[1]*q(0);
		v[0]=x;
		v[1]=y;
		v[2]=z;
		v[3]=w;
		
		return *this;

    }

	const quat<T> operator*(const quat<T>& q)const
    {
		
		quat<T> r =*this;
		r*=q;
		return r;

    }


	T length()
	{
		T l =0;
		for(int i = 0; i < 4;i++)
			l += v[i]*v[i];
		return sqrt(l);
	}

	T sqr_length()
	{
		T l =0;
		for(int i = 0; i < 4;i++)
			l += v[i]*v[i];
		return l;
	}


	mat<T> to_mat_33() const
	{
		mat<T> mat(3,3);
		T xx      = v[0] * v[0];
		T xy      = v[0] * v[1];
		T xz      = v[0] * v[2];
		T xw      = v[0] * v[3];
		T yy      = v[1] * v[1];
		T yz      = v[1] * v[2];
		T yw      = v[1] * v[3];
		T zz      = v[2] * v[2];
		T zw      = v[2] * v[3];
		mat(0,0)  = 1 - 2 * ( yy + zz );
		mat(0,1)  =     2 * ( xy - zw );
		mat(0,2)  =     2 * ( xz + yw );
		mat(1,0)  =     2 * ( xy + zw );
		mat(1,1)  = 1 - 2 * ( xx + zz );
		mat(1,2)  =     2 * ( yz - xw );
		mat(2,0)  =     2 * ( xz - yw );
		mat(2,1)  =     2 * ( yz + xw );
		mat(2,2) = 1 - 2 * ( xx + yy );
		return mat;
	}

	mat<T> to_mat_44() const
	{
		mat<T> mat(4,4);
		T xx      = v[0] * v[0];
		T xy      = v[0] * v[1];
		T xz      = v[0] * v[2];
		T xw      = v[0] * v[3];
		T yy      = v[1] * v[1];
		T yz      = v[1] * v[2];
		T yw      = v[1] * v[3];
		T zz      = v[2] * v[2];
		T zw      = v[2] * v[3];
		mat(0,0)  = 1 - 2 * ( yy + zz );
		mat(0,1)  =     2 * ( xy - zw );
		mat(0,2)  =     2 * ( xz + yw );
		mat(1,0)  =     2 * ( xy + zw );
		mat(1,1)  = 1 - 2 * ( xx + zz );
		mat(1,2)  =     2 * ( yz - xw );
		mat(2,0)  =     2 * ( xz - yw );
		mat(2,1)  =     2 * ( yz + xw );
		mat(2,2) = 1 - 2 * ( xx + yy );
		mat(3,0) = mat(3,1) =mat(3,2)=mat(0,3)=mat(1,3)=mat(2,3) = 0;
		mat(3,3) = 1;
		return mat;
	}

	T& operator()(const int i)	//subscripting
	{
		return v[i];
	}
	
	
	const T&  operator()(const int i) const	//subscripting
	{
		return v[i];
	}

	///cast into non const array 
	operator T*()
	{
		return v;
	}

	///cast into const array
	operator const T*() const
	{
		return v;
	}

	///normalize the vector
	void normalize() 
	{
		T l =0;
		for(int i = 0; i < 4;i++)
			l += v[i]*v[i];
		l=(T)1.0/sqrt(l);
		for(unsigned i = 0; i<4; i++)
			 operator()(i)=l*operator()(i);
	}

	void set(const T& x, const T&y, const T&z,const T&w)
	{
		v[0]=x;
		v[1]=y;
		v[2]=z;
		v[3]=w;
		
	}

	
	void set(vec<T> axis, const T angle) 
	{
		axis.normalize();
		T sin_a = (T)sin( angle / (T)2.0 );
		T cos_a = (T)cos( angle / (T)2.0 );
		v[0]    = axis.x() * sin_a;
		v[1]    = axis.y() * sin_a;
		v[2]    = axis.z() * sin_a;
		v[3]    = cos_a;

	}

	void invert()
	{
		
		conjugate();
		normalize();
		
	}

	void rotate(vec<T>& pos) const
	{
		
		T w =  - v[0]*pos(0) - v[1]*pos(1) - v[2]*pos(2);
		T x = v[3]*pos(0)  + v[1]*pos(2) - v[2]*pos(1);
		T y = v[3]*pos(1)  + v[2]*pos(0) - v[0]*pos(2);
		T z = v[3]*pos(2)  + v[0]*pos(1) - v[1]*pos(0);
	
		pos(0) = -w*v[0] + x*v[3] - y*v[2] + z*v[1];
		pos(1) = -w*v[1] + y*v[3] - z*v[0] + x*v[2];
		pos(2) = -w*v[2] + z*v[3] - x*v[1] + y*v[0];
		
	}

};

template<typename T>
vec<T> rotate(const quat<T>& q,const vec<T>& pos)
{
	vec<T> v = pos;
	q.rotate(v);
	return v;
}

template<typename T>
quat<T> normalize(const quat<T>& q)
{
	quat<T> r = q;
	r.normalize();
	return r;
}

//output of a vector
template<typename T>
std::ostream& operator<<(std::ostream& out, const quat<T>& q)
{

	for (unsigned i=0;i<3;++i)
	{
		out << q(i)<<"\t";	
	}
	out << q(3);
	return out;

}

//input of a vector
template<typename T>
std::istream& operator>>(std::istream& in, quat<T>& q)
{

	for (unsigned i=0;i<4;++i)
	{
		in >> q(i);	
	}
	
	return in;

}

*/


}

}