CWE-ou-minkata/Sources/Plasma/CoreLib/hsQuat.cpp

/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
#include "HeadSpin.h"
#include "hsQuat.h"
#include "hsMatrix44.h"
#include "hsStream.h"
#include "hsFastMath.h"

//
// Quaternion class.
// For conversion to and from euler angles, see hsEuler.cpp,h.
// 

//
// Construct quat from angle (in radians) and axis of rotation
//
hsQuat::hsQuat(hsScalar rad, const hsVector3* axis)
{
	// hsAssert(rad >= -hsScalarPI && rad <= hsScalarPI, "Quat: Angle should be between -PI and PI");

	fW = hsCosine(rad*0.5f);

	hsScalar s = hsSine(rad*0.5f);
	fX = axis->fX*s;
	fY = axis->fY*s;
	fZ = axis->fZ*s;
}

hsQuat hsQuat::Inverse()
{
	hsQuat q2 = Conjugate();
	hsScalar msInv = 1.0f/q2.MagnitudeSquared();
	return (q2 * msInv);
}

hsPoint3 hsQuat::Rotate(const hsScalarTriple* v)
{
	hsQuat qInv = Inverse();
	hsQuat qVec(v->fX, v->fY, v->fZ, 0);
	hsQuat t = qInv * qVec;
	hsQuat res = (t * (*this));
	//hsAssert(hsABS(res.fW)<1e-5, "Error rotating vector");
	return hsPoint3(res.fX, res.fY, res.fZ);
}

void hsQuat::SetAngleAxis(const hsScalar rad, const hsVector3 &axis)
{
	fW = hsCosine(rad*0.5f);

	hsScalar s = hsSine(rad*0.5f);
	fX = axis.fX*s;
	fY = axis.fY*s;
	fZ = axis.fZ*s;	
}

//
// Might want to Normalize before calling this
//
void hsQuat::GetAngleAxis(hsScalar *rad, hsVector3 *axis) const
{
	hsAssert((fW >= -1) && (fW <= 1), "Invalid acos argument");
	hsScalar ac = hsACosine(fW);
	*rad = 2.0f * ac;

	hsScalar s = hsSine(ac);
	if (s != 0.0f)
	{
		hsScalar invS = 1.0f/s;
		axis->Set(fX*invS, fY*invS, fZ*invS);
	}
	else
		axis->Set(0,0,0);
}

//
//
//
hsScalar hsQuat::MagnitudeSquared()
{
	return (fX*fX + fY*fY + fZ*fZ + fW*fW);
}

//
//
//
hsScalar hsQuat::Magnitude()
{
	return hsSquareRoot(MagnitudeSquared());
}

//
//
//
void hsQuat::Normalize()
{
	hsScalar invMag = 1.0f/Magnitude();
	fX *= invMag;
	fY *= invMag;
	fZ *= invMag;
	fW *= invMag;
}

//
//
//
void hsQuat::NormalizeIfNeeded()
{

	hsScalar magSquared = MagnitudeSquared();
	if (magSquared == 1.0f)
		return;

	hsScalar invMag = 1.0f/hsSquareRoot(magSquared);
	fX *= invMag;
	fY *= invMag;
	fZ *= invMag;
	fW *= invMag;
}

//
// This is for a RHS.
// The quat should be normalized first.
// 
void hsQuat::MakeMatrix(hsMatrix44 *mat) const
{
	// mf horse - this is transpose of both what
	// Gems says and what i'm expecting to come
	// out of it, so i'm flipping it.
	mat->fMap[0][0] = 1.0f - 2.0f*fY*fY - 2.0f*fZ*fZ;
	mat->fMap[0][1] = 2.0f*fX*fY - 2.0f*fW*fZ;
	mat->fMap[0][2] = 2.0f*fX*fZ + 2.0f*fW*fY;
	mat->fMap[0][3] = 0.0f;

	mat->fMap[1][0] = 2.0f*fX*fY + 2.0f*fW*fZ;
	mat->fMap[1][1] = 1.0f - 2.0f*fX*fX - 2.0f*fZ*fZ;
	mat->fMap[1][2] = 2.0f*fY*fZ - 2.0f*fW*fX;
	mat->fMap[1][3] = 0.0f;

	mat->fMap[2][0] = 2.0f*fX*fZ - 2.0f*fW*fY;
	mat->fMap[2][1] = 2.0f*fY*fZ + 2.0f*fW*fX;
	mat->fMap[2][2] = 1.0f - 2.0f*fX*fX - 2.0f*fY*fY;
	mat->fMap[2][3] = 0.0f;

	mat->fMap[3][0] = 0.0f;
	mat->fMap[3][1] = 0.0f;
	mat->fMap[3][2] = 0.0f;
	mat->fMap[3][3] = 1.0f;
 
#if 0
	mat->fMap[0][0] = fW*fW + fX*fX - fY*fY - fZ*fZ;
	mat->fMap[1][0] = 2.0f*fX*fY - 2.0f*fW*fZ;
	mat->fMap[2][0] = 2.0f*fX*fZ + 2.0f*fW*fY;
	mat->fMap[3][0] = 0.0f;

	mat->fMap[0][1] = 2.0f*fX*fY + 2.0f*fW*fZ;
	mat->fMap[1][1] = fW*fW - fX*fX + fY*fY - fZ*fZ;
	mat->fMap[2][1] = 2.0f*fY*fZ - 2.0f*fW*fX;
	mat->fMap[3][1] = 0.0f;

	mat->fMap[0][2] = 2.0f*fX*fZ - 2.0f*fW*fY;
	mat->fMap[1][2] = 2.0f*fY*fZ + 2.0f*fW*fX;
	mat->fMap[2][2] = fW*fW - fX*fX - fY*fY + fZ*fZ;
	mat->fMap[3][2] = 0.0f;

	mat->fMap[0][3] = 0.0f;
	mat->fMap[1][3] = 0.0f;
	mat->fMap[2][3] = 0.0f;
	mat->fMap[3][3] = fW*fW + fX*fX + fY*fY + fZ*fZ;
#endif

	mat->NotIdentity();
}

// Binary operators
hsQuat hsQuat::operator-(const hsQuat &in) const
{
	return hsQuat(fX-in.fX, fY-in.fY, fZ-in.fZ, fW-in.fW);
}

hsQuat hsQuat::operator+(const hsQuat &in) const
{
	return hsQuat(fX+in.fX, fY+in.fY, fZ+in.fZ, fW+in.fW);
}

//
// Return quaternion product (this * in).  Note: order is important!
// To combine rotations, use the product (qSecond * qFirst),
// which gives the effect of rotating by qFirst then qSecond.
//
hsQuat hsQuat::operator*(const hsQuat &in) const
{
	hsQuat ret;
	ret.fW = (fW*in.fW - fX*in.fX - fY*in.fY - fZ*in.fZ);
	ret.fX = (fY*in.fZ - in.fY*fZ + fW*in.fX + in.fW*fX);
	ret.fY = (fZ*in.fX - in.fZ*fX + fW*in.fY + in.fW*fY);
	ret.fZ = (fX*in.fY - in.fX*fY + fW*in.fZ + in.fW*fZ);
	return ret;
}


// I/O
void hsQuat::Read(hsStream *stream)
{
	 fX = stream->ReadSwapFloat();
	 fY = stream->ReadSwapFloat();
	 fZ = stream->ReadSwapFloat();
	 fW = stream->ReadSwapFloat();
}

void hsQuat::Write(hsStream *stream)
{
	 stream->WriteSwapFloat(fX);
	 stream->WriteSwapFloat(fY);
	 stream->WriteSwapFloat(fZ);
	 stream->WriteSwapFloat(fW);
}


#if 0
//
// Interpolate on a sphere.
//
void hsQuat::SetFromSlerp(hsQuat *q1, hsQuat *q2, hsScalar t)
{
	hsAssert(t>=0.0 && t<= 1.0, "Quat slerp param must be between 0 an 1");
	hsScalar theta = hsACosine(q1->Dot(*q2));

	hsScalar st = hsSine(theta);
	assert(st != 0.0);

	hsScalar s1 = hsSine(1.0-t)*theta / st;

	hsScalar s2 = hsSine(t)*theta / st;

	*this = (*q1) * s1 + (*q2) * s2;
}
#else
/*
 * Spherical linear interpolation of unit quaternions with spins
 */

#define EPSILON 1.0E-6 			/* a tiny number */

void hsQuat::SetFromSlerp(const hsQuat &a, const hsQuat &b, hsScalar alpha, int spin)
//	double alpha;			/* interpolation parameter (0 to 1) */
//	Quaternion *a, *b;		/* start and end unit quaternions */
//	int spin;			/* number of extra spin rotations */
{
	hsScalar beta;			/* complementary interp parameter */
	hsScalar theta;			/* angle between A and B */
	hsScalar sin_t, cos_t;		/* sine, cosine of theta */
	hsScalar phi;			/* theta plus spins */
	int bflip;			/* use negation of B? */

	/* cosine theta = dot product of A and B */
	cos_t = a.Dot(b);

	/* if B is on opposite hemisphere from A, use -B instead */
 	if (cos_t < 0.0)
	{
		cos_t = -cos_t;
		bflip = true;
	} 
	else
		bflip = false;

	/* if B is (within precision limits) the same as A,
	 * just linear interpolate between A and B.
	 * Can't do spins, since we don't know what direction to spin.
	 */
	if (1.0 - cos_t < EPSILON) 
	{
		beta = 1.0f - alpha;
 	} else 
	{				/* normal case */
//		hsAssert((cos_t >= -1) && (cos_t <= 1), "Invalid acos argument");
 		theta	= hsACosine(cos_t);
 		phi		= theta + spin * hsScalarPI;
 		sin_t	= hsSine(theta);
		hsAssert(sin_t != 0.0, "Invalid sin value in quat slerp");
 		beta	= hsSine(theta - alpha*phi) / sin_t;
 		alpha	= hsSine(alpha*phi) / sin_t;
 	}

	if (bflip)
		alpha = -alpha;

	/* interpolate */
 	fX = beta*a.fX + alpha*b.fX;
 	fY = beta*a.fY + alpha*b.fY;
 	fZ = beta*a.fZ + alpha*b.fZ;
 	fW = beta*a.fW + alpha*b.fW;
}
#endif

//
//
//
void hsQuat::SetFromMatrix(const hsMatrix44* mat)
{
	hsScalar wSq = 0.25f*(1 + mat->fMap[0][0] + mat->fMap[1][1] + mat->fMap[2][2]);
	if (wSq > EPSILON)
	{
		fW = hsSquareRoot(wSq);
		hsScalar iw4 = 1.0f/(4.0f*fW);
		fX = (mat->fMap[2][1] - mat->fMap[1][2]) * iw4;
		fY = (mat->fMap[0][2] - mat->fMap[2][0]) * iw4;
		fZ = (mat->fMap[1][0] - mat->fMap[0][1]) * iw4;
		return;
	}

	fW = 0;
	hsScalar xSq = -0.5f*(mat->fMap[1][1] + mat->fMap[2][2]);
	if (xSq > EPSILON)
	{
		fX = hsSquareRoot(xSq);
		hsScalar ix2 = 1.0f/(2.0f*fX);
		fY = mat->fMap[1][0] * ix2;
		fZ = mat->fMap[2][0] * ix2;
		return;
	}

	fX = 0;
	hsScalar ySq = 0.5f * (1 - mat->fMap[2][2]);
	if (ySq > EPSILON)
	{
		fY = hsSquareRoot(ySq);
		fZ = mat->fMap[2][1] / (2.0f*fY);
		return;
	}

	fY = 0;
	fZ = 1;
}

// 9/15/03 - Colin
// Changed to not use hsFastMath::InvSqrt, due to errors occuring at some
// specific angles that caused Havok to blow up.
hsQuat hsQuat::QuatFromMatrix44(const hsMatrix44& mat)
{
    /* This algorithm avoids near-zero divides by looking for a large component
     * - first w, then x, y, or z.  When the trace is greater than zero,
     * |w| is greater than 1/2, which is as small as a largest component can be.
     * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
     * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
    hsQuat qu;
	float tr, s;

	const int X = 0;
	const int Y = 1;
	const int Z = 2;
	const int W = 3;
    tr = mat.fMap[X][X] + mat.fMap[Y][Y]+ mat.fMap[Z][Z];
    if (tr >= 0.0) {
		s = float(sqrt(tr + 1.f));
	    qu.fW = 0.5f * s;
	    s = 0.5f / s;
	    qu.fX = (mat.fMap[Z][Y] - mat.fMap[Y][Z]) * s;
	    qu.fY = (mat.fMap[X][Z] - mat.fMap[Z][X]) * s;
	    qu.fZ = (mat.fMap[Y][X] - mat.fMap[X][Y]) * s;
	} else {
	    int h = X;
	    if (mat.fMap[Y][Y] > mat.fMap[X][X]) 
			h = Y;
	    if (mat.fMap[Z][Z] > mat.fMap[h][h]) 
			h = Z;
	    switch (h) {
#define caseMacro(i,j,k,I,J,K) \
	    case I:\
		s = float(sqrt( (mat.fMap[I][I] - (mat.fMap[J][J]+mat.fMap[K][K])) + 1.f )); \
		qu.i = 0.5f * s; \
		s = 0.5f / s; \
		qu.j = (mat.fMap[I][J] + mat.fMap[J][I]) * s; \
		qu.k = (mat.fMap[K][I] + mat.fMap[I][K]) * s; \
		qu.fW = (mat.fMap[K][J] - mat.fMap[J][K]) * s; \
		break
	    caseMacro(fX,fY,fZ,X,Y,Z);
	    caseMacro(fY,fZ,fX,Y,Z,X);
	    caseMacro(fZ,fX,fY,Z,X,Y);
	    }
	}
    return (qu);
}

hsQuat& hsQuat::SetFromMatrix44(const hsMatrix44& mat)
{
	return (*this = QuatFromMatrix44(mat));
}