Webcam/include/framework/math/quaternion.cpp

/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "math/quaternion.h"
		#include "math/matrix3.h"
		#include "metafile/nml.h"

		using namespace GS;


//------------------------------------------------------------------------------
Quaternion			Quaternion::Slerp(float t, const Quaternion &a, const Quaternion &b)
{
	float norm = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;

	bool bFlip = false;

	if	(norm < 0.0f)
	{
		norm = -norm;
		bFlip = true;
	}

	float inv_d;
	if	(1.0f - norm < 0.000001f)
		inv_d = 1.0f - t;

	else
	{
		float theta = Math::ACos(norm);
		float s = 1.f / Math::Sin(theta);

		inv_d = Math::Sin((1.0f - t) * theta) * s;
		t = Math::Sin(t * theta) * s;
	}

	if	(bFlip)
		t = -t;

	return Quaternion(inv_d * a.x + t * b.x, inv_d * a.y + t * b.y, inv_d * a.z + t * b.z, inv_d * a.w + t * b.w);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
float				Quaternion::Distance(const Quaternion &a, const Quaternion &b)
{
	const float	dx = a.x - b.x, dy = a.y - b.y, dz = a.z - b.z, dw = a.w - b.w;
	return Math::Sqrt((dx * dx) + (dy * dy) + (dz * dz) + (dw * dw));
}
Quaternion			Quaternion::Inverse() const
{
	const float	norm = w * w + x * x + y * y + z * z;
	if	(norm > 0)
	{
		const float	inorm = 1.f / norm;
		return Quaternion(x * -inorm, y * -inorm, z * -inorm, w * inorm);
	}
	return *this;
}
Quaternion			Quaternion::Normalize() const
{
	float d = Math::Sqrt(x * x + y * y + z * z + w * w);
	if	(!d)
		return Quaternion(1, 1, 1, 1);

	float k = 1.f / d;
	return Quaternion(x * k, y * k, z * k, w * k);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Quaternion			Quaternion::LookAt(const Vector4 &at)
{	return Quaternion::FromMatrix3(Matrix3::FromOrthonormalBasis(at));	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Quaternion			Quaternion::FromMatrix3(const Matrix3 &m)
{
    // From "Quaternion Calculus and Fast Animation".
	float x, y, z, w;
	float trace = m.m[0][0] + m.m[1][1] + m.m[2][2];

	if	(trace > 0.0)
	{
		// |w| > 1/2, may as well choose w > 1/2
		float root = Math::Sqrt(trace + 1.0f);  // 2w
		w = 0.5f * root;
		root = 0.5f / root;  // 1/(4w)
		x = (m.m[2][1] - m.m[1][2]) * root;
		y = (m.m[0][2] - m.m[2][0]) * root;
		z = (m.m[1][0] - m.m[0][1]) * root;
	}
	else
	{
		// |w| <= 1/2
		static size_t inext[3] = { 1, 2, 0 };
		size_t i = 0;
		if	(m.m[1][1] > m.m[0][0])
			i = 1;
		if	(m.m[2][2] > m.m[i][i])
			i = 2;
		size_t j = inext[i];
		size_t k = inext[j];

		float root = Math::Sqrt(m.m[i][i] - m.m[j][j] - m.m[k][k] + 1.0f);
		float *quat[3] = { &x, &y, &z };
		*quat[i] = 0.5f * root;
		root = 0.5f / root;
		w = (m.m[k][j] - m.m[j][k]) * root;
		*quat[j] = (m.m[j][i] + m.m[i][j]) * root;
		*quat[k] = (m.m[k][i] + m.m[i][k]) * root;
	}
	return Quaternion(x, y, z, w);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Quaternion			Quaternion::FromAxisAngle(float a, float _x, float _y, float _z)
{
	float sn = Math::Sin(a * 0.5f), cs = Math::Cos(a * 0.5f);
	return Quaternion(_x * sn, _y * sn, _z * sn, cs).Normalize();
}
Quaternion			Quaternion::FromEuler(float _x, float _y, float _z, Math::rOrder rorder)
{
	Quaternion	qx(Quaternion::FromAxisAngle(_x, 1, 0, 0)),
				qy(Quaternion::FromAxisAngle(_y, 0, 1, 0)),
				qz(Quaternion::FromAxisAngle(_z, 0, 0, 1)),
				q;

	switch (rorder)
	{
		case Math::rOrder_ZYX:	q = qz * qy * qx;	break;
		case Math::rOrder_YZX:	q = qy * qz * qx;	break;
		case Math::rOrder_ZXY:	q = qz * qx * qy;	break;
		case Math::rOrder_XZY:	q = qx * qz * qy;	break;
		default:
		case Math::rOrder_YXZ:	q = qy * qx * qz;	break;
		case Math::rOrder_XYZ:	q = qx * qy * qz;	break;
		case Math::rOrder_XY:	q = qx * qy;		break;
	}
	return q.Normalize();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Quaternion::AsMatrix3() const
{
	float sqw = w * w, sqx = x * x, sqy = y * y, sqz = z * z;

	Matrix3 m;

	float invs = 1.f / (sqx + sqy + sqz + sqw);
	m.m[0][0] = ( sqx - sqy - sqz + sqw) * invs;		// Since sqw + sqx + sqy + sqz = 1 / invs * invs.
	m.m[1][1] = (-sqx + sqy - sqz + sqw) * invs;
	m.m[2][2] = (-sqx - sqy + sqz + sqw) * invs;

	float tmp1 = x * y;
	float tmp2 = z * w;
	m.m[1][0] = 2.f * (tmp1 + tmp2) * invs;
	m.m[0][1] = 2.f * (tmp1 - tmp2) * invs;

	tmp1 = x * z;
	tmp2 = y * w;
	m.m[2][0] = 2.f * (tmp1 - tmp2) * invs;
	m.m[0][2] = 2.f * (tmp1 + tmp2) * invs;
	tmp1 = y * z;
	tmp2 = x * w;
	m.m[2][1] = 2.f * (tmp1 + tmp2) * invs;
	m.m[1][2] = 2.f * (tmp1 - tmp2) * invs;  

	return m;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
NML::Tag			*Quaternion::AsMetaTag(const char *id) const
{
	NML::Tag *root = new NML::Tag(id ? id : "Quaternion");

	if	(root)
	{
		root->AddChild("X", x);
		root->AddChild("Y", y);
		root->AddChild("Z", z);
		root->AddChild("W", w);
	}
	return root;
}
bool				Quaternion::FromMetaTag(NML::Tag &tag)
{
	NML::Tag *t;
	List <NML::Tag *> ::Iterator i(tag.GetTags().GetRoot());

	t = i.ObjectPtr();
	if	(!t)	return false;
	x = t->GetReal();
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	y = t->GetReal();
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	z = t->GetReal();
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	w = t->GetReal();

	return true;
}
//------------------------------------------------------------------------------