third_party/PowerVR_SDK/Tools/PVRTQuaternionX.cpp

/******************************************************************************

 @File         PVRTQuaternionX.cpp

 @Title        PVRTQuaternionX

 @Version      

 @Copyright    Copyright (c) Imagination Technologies Limited.

 @Platform     ANSI compatible

 @Description  Set of mathematical functions for quaternions.

******************************************************************************/
#include "PVRTContext.h"
#include <math.h>
#include <string.h>

#include "PVRTFixedPoint.h"
#include "PVRTQuaternion.h"


/****************************************************************************
** Functions
****************************************************************************/

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionIdentityX
 @Output			qOut	Identity quaternion
 @Description		Sets the quaternion to (0, 0, 0, 1), the identity quaternion.
*****************************************************************************/
void PVRTMatrixQuaternionIdentityX(PVRTQUATERNIONx		&qOut)
{
	qOut.x = PVRTF2X(0.0f);
	qOut.y = PVRTF2X(0.0f);
	qOut.z = PVRTF2X(0.0f);
	qOut.w = PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionRotationAxisX
 @Output			qOut	Rotation quaternion
 @Input				vAxis	Axis to rotate around
 @Input				fAngle	Angle to rotate
 @Description		Create quaternion corresponding to a rotation of fAngle
					radians around submitted vector.
*****************************************************************************/
void PVRTMatrixQuaternionRotationAxisX(
	PVRTQUATERNIONx		&qOut,
	const PVRTVECTOR3x	&vAxis,
	const int			fAngle)
{
	int	fSin, fCos;

	fSin = PVRTXSIN(fAngle>>1);
	fCos = PVRTXCOS(fAngle>>1);

	/* Create quaternion */
	qOut.x = PVRTXMUL(vAxis.x, fSin);
	qOut.y = PVRTXMUL(vAxis.y, fSin);
	qOut.z = PVRTXMUL(vAxis.z, fSin);
	qOut.w = fCos;

	/* Normalise it */
	PVRTMatrixQuaternionNormalizeX(qOut);
}

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionToAxisAngleX
 @Input				qIn		Quaternion to transform
 @Output			vAxis	Axis of rotation
 @Output			fAngle	Angle of rotation
 @Description		Convert a quaternion to an axis and angle. Expects a unit
					quaternion.
*****************************************************************************/
void PVRTMatrixQuaternionToAxisAngleX(
	const PVRTQUATERNIONx	&qIn,
	PVRTVECTOR3x			&vAxis,
	int						&fAngle)
{
	int		fCosAngle, fSinAngle;
	int		temp;

	/* Compute some values */
	fCosAngle	= qIn.w;
	temp		= PVRTF2X(1.0f) - PVRTXMUL(fCosAngle, fCosAngle);
	fAngle		= PVRTXMUL(PVRTXACOS(fCosAngle), PVRTF2X(2.0f));
	fSinAngle	= PVRTF2X(((float)sqrt(PVRTX2F(temp))));

	/* This is to avoid a division by zero */
	if (PVRTABS(fSinAngle)<PVRTF2X(0.0005f))
	{
		fSinAngle = PVRTF2X(1.0f);
	}

	/* Get axis vector */
	vAxis.x = PVRTXDIV(qIn.x, fSinAngle);
	vAxis.y = PVRTXDIV(qIn.y, fSinAngle);
	vAxis.z = PVRTXDIV(qIn.z, fSinAngle);
}

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionSlerpX
 @Output			qOut	Result of the interpolation
 @Input				qA		First quaternion to interpolate from
 @Input				qB		Second quaternion to interpolate from
 @Input				t		Coefficient of interpolation
 @Description		Perform a Spherical Linear intERPolation between quaternion A
					and quaternion B at time t. t must be between 0.0f and 1.0f
					Requires input quaternions to be normalized
*****************************************************************************/
void PVRTMatrixQuaternionSlerpX(
	PVRTQUATERNIONx			&qOut,
	const PVRTQUATERNIONx	&qA,
	const PVRTQUATERNIONx	&qB,
	const int				t)
{
	int		fCosine, fAngle, A, B;

	/* Parameter checking */
	if (t<PVRTF2X(0.0f) || t>PVRTF2X(1.0f))
	{
		_RPT0(_CRT_WARN, "PVRTMatrixQuaternionSlerp : Bad parameters\n");
		qOut.x = PVRTF2X(0.0f);
		qOut.y = PVRTF2X(0.0f);
		qOut.z = PVRTF2X(0.0f);
		qOut.w = PVRTF2X(1.0f);
		return;
	}

	/* Find sine of Angle between Quaternion A and B (dot product between quaternion A and B) */
	fCosine = PVRTXMUL(qA.w, qB.w) +
		PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z);

	if(fCosine < PVRTF2X(0.0f))
	{
		PVRTQUATERNIONx qi;

		/*
			<http://www.magic-software.com/Documentation/Quaternions.pdf>

			"It is important to note that the quaternions q and -q represent
			the same rotation... while either quaternion will do, the
			interpolation methods require choosing one over the other.

			"Although q1 and -q1 represent the same rotation, the values of
			Slerp(t; q0, q1) and Slerp(t; q0,-q1) are not the same. It is
			customary to choose the sign... on q1 so that... the angle
			between q0 and q1 is acute. This choice avoids extra
			spinning caused by the interpolated rotations."
		*/
		qi.x = -qB.x;
		qi.y = -qB.y;
		qi.z = -qB.z;
		qi.w = -qB.w;

		PVRTMatrixQuaternionSlerpX(qOut, qA, qi, t);
		return;
	}

	fCosine = PVRT_MIN(fCosine, PVRTF2X(1.0f));
	fAngle = PVRTXACOS(fCosine);

	/* Avoid a division by zero */
	if (fAngle==PVRTF2X(0.0f))
	{
		qOut = qA;
		return;
	}

	/* Precompute some values */
	A = PVRTXDIV(PVRTXSIN(PVRTXMUL((PVRTF2X(1.0f)-t), fAngle)), PVRTXSIN(fAngle));
	B = PVRTXDIV(PVRTXSIN(PVRTXMUL(t, fAngle)), PVRTXSIN(fAngle));

	/* Compute resulting quaternion */
	qOut.x = PVRTXMUL(A, qA.x) + PVRTXMUL(B, qB.x);
	qOut.y = PVRTXMUL(A, qA.y) + PVRTXMUL(B, qB.y);
	qOut.z = PVRTXMUL(A, qA.z) + PVRTXMUL(B, qB.z);
	qOut.w = PVRTXMUL(A, qA.w) + PVRTXMUL(B, qB.w);

	/* Normalise result */
	PVRTMatrixQuaternionNormalizeX(qOut);
}

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionNormalizeX
 @Modified			quat	Vector to normalize
 @Description		Normalize quaternion.
					Original quaternion is scaled down prior to be normalized in
					order to avoid overflow issues.
*****************************************************************************/
void PVRTMatrixQuaternionNormalizeX(PVRTQUATERNIONx &quat)
{
	PVRTQUATERNIONx	qTemp;
	int				f, n;

	/* Scale vector by uniform value */
	n = PVRTABS(quat.w) + PVRTABS(quat.x) + PVRTABS(quat.y) + PVRTABS(quat.z);
	qTemp.w = PVRTXDIV(quat.w, n);
	qTemp.x = PVRTXDIV(quat.x, n);
	qTemp.y = PVRTXDIV(quat.y, n);
	qTemp.z = PVRTXDIV(quat.z, n);

	/* Compute quaternion magnitude */
	f = PVRTXMUL(qTemp.w, qTemp.w) + PVRTXMUL(qTemp.x, qTemp.x) + PVRTXMUL(qTemp.y, qTemp.y) + PVRTXMUL(qTemp.z, qTemp.z);
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));

	/* Multiply vector components by f */
	quat.x = PVRTXMUL(qTemp.x, f);
	quat.y = PVRTXMUL(qTemp.y, f);
	quat.z = PVRTXMUL(qTemp.z, f);
	quat.w = PVRTXMUL(qTemp.w, f);
}

/*!***************************************************************************
 @Function			PVRTMatrixRotationQuaternionX
 @Output			mOut	Resulting rotation matrix
 @Input				quat	Quaternion to transform
 @Description		Create rotation matrix from submitted quaternion.
					Assuming the quaternion is of the form [X Y Z W]:

						|       2     2									|
						| 1 - 2Y  - 2Z    2XY - 2ZW      2XZ + 2YW		 0	|
						|													|
						|                       2     2					|
					M = | 2XY + 2ZW       1 - 2X  - 2Z   2YZ - 2XW		 0	|
						|													|
						|                                      2     2		|
						| 2XZ - 2YW       2YZ + 2XW      1 - 2X  - 2Y	 0	|
						|													|
						|     0			   0			  0          1  |
*****************************************************************************/
void PVRTMatrixRotationQuaternionX(
	PVRTMATRIXx				&mOut,
	const PVRTQUATERNIONx	&quat)
{
	const PVRTQUATERNIONx *pQ;

#if defined(BUILD_DX11)
	PVRTQUATERNIONx qInv;

	qInv.x = -quat.x;
	qInv.y = -quat.y;
	qInv.z = -quat.z;
	qInv.w =  quat.w;

	pQ = &qInv;
#else
	pQ = &quat;
#endif

    /* Fill matrix members */
	mOut.f[0] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->y, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1);
	mOut.f[1] = (PVRTXMUL(pQ->x, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->w)<<1);
	mOut.f[2] = (PVRTXMUL(pQ->x, pQ->z)<<1) + (PVRTXMUL(pQ->y, pQ->w)<<1);
	mOut.f[3] = PVRTF2X(0.0f);

	mOut.f[4] = (PVRTXMUL(pQ->x, pQ->y)<<1) + (PVRTXMUL(pQ->z, pQ->w)<<1);
	mOut.f[5] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1);
	mOut.f[6] = (PVRTXMUL(pQ->y, pQ->z)<<1) - (PVRTXMUL(pQ->x, pQ->w)<<1);
	mOut.f[7] = PVRTF2X(0.0f);

	mOut.f[8] = (PVRTXMUL(pQ->x, pQ->z)<<1) - (PVRTXMUL(pQ->y, pQ->w)<<1);
	mOut.f[9] = (PVRTXMUL(pQ->y, pQ->z)<<1) + (PVRTXMUL(pQ->x, pQ->w)<<1);
	mOut.f[10] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->y, pQ->y)<<1);
	mOut.f[11] = PVRTF2X(0.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function			PVRTMatrixQuaternionMultiplyX
 @Output			qOut	Resulting quaternion
 @Input				qA		First quaternion to multiply
 @Input				qB		Second quaternion to multiply
 @Description		Multiply quaternion A with quaternion B and return the
					result in qOut.
					Input quaternions must be normalized.
*****************************************************************************/
void PVRTMatrixQuaternionMultiplyX(
	PVRTQUATERNIONx			&qOut,
	const PVRTQUATERNIONx	&qA,
	const PVRTQUATERNIONx	&qB)
{
	PVRTVECTOR3x	CrossProduct;

	/* Compute scalar component */
	qOut.w = PVRTXMUL(qA.w, qB.w) -
				   (PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z));

	/* Compute cross product */
	CrossProduct.x = PVRTXMUL(qA.y, qB.z) - PVRTXMUL(qA.z, qB.y);
	CrossProduct.y = PVRTXMUL(qA.z, qB.x) - PVRTXMUL(qA.x, qB.z);
	CrossProduct.z = PVRTXMUL(qA.x, qB.y) - PVRTXMUL(qA.y, qB.x);

	/* Compute result vector */
	qOut.x = PVRTXMUL(qA.w, qB.x) + PVRTXMUL(qB.w, qA.x) + CrossProduct.x;
	qOut.y = PVRTXMUL(qA.w, qB.y) + PVRTXMUL(qB.w, qA.y) + CrossProduct.y;
	qOut.z = PVRTXMUL(qA.w, qB.z) + PVRTXMUL(qB.w, qA.z) + CrossProduct.z;

	/* Normalize resulting quaternion */
	PVRTMatrixQuaternionNormalizeX(qOut);
}

/*****************************************************************************
 End of file (PVRTQuaternionX.cpp)
*****************************************************************************/