Renegade/Code/Tools/pluglib/w3dquat.cpp

/*
**	Command & Conquer Renegade(tm)
**	Copyright 2025 Electronic Arts 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/>.
*/

/* $Header: /Commando/Code/Tools/max2w3d/w3dquat.cpp 29    2/03/00 4:55p Jason_a $ */
/*********************************************************************************************** 
 ***                            Confidential - Westwood Studios                              *** 
 *********************************************************************************************** 
 *                                                                                             * 
 *                 Project Name : Voxel Technology                                             * 
 *                                                                                             * 
 *                    File Name : QUAT.CPP                                                     * 
 *                                                                                             * 
 *                   Programmer : Greg Hjelstrom                                               * 
 *                                                                                             * 
 *                   Start Date : 02/24/97                                                     * 
 *                                                                                             * 
 *                  Last Update : February 28, 1997 [GH]                                       * 
 *                                                                                             * 
 *---------------------------------------------------------------------------------------------* 
 * Functions:                                                                                  * 
 *   Quaternion::Quaternion -- constructor                                                     * 
 *   Quaternion::Set -- Set the quaternion                                                     * 
 *   Quaternion::operator= -- Assignment operator                                              * 
 *   Quaternion::Make_Closest -- Use nearest representation to the given quaternion.           * 
 *   Trackball -- Computes a "trackball" quaternion given 2D mouse coordinates                 * 
 *   Axis_To_Quat -- Creates a quaternion given an axis and angle of rotation                  * 
 *   Slerp -- Spherical Linear interpolation!                                                  * 
 *   Build_Quaternion -- Creates a quaternion from a Matrix                                    * 
 *   Build_Matrix -- Creates a Matrix from a Quaternion                                        * 
 *   Normalize -- normalizes a quaternion                                                      * 
 *   Quaternion::Quaternion -- constructor                                                     *
 *   Slerp_Setup -- Get ready to call "Cached_Slerp"                                           *
 *   Cached_Slerp -- Quaternion slerping, optimized with cached values                         *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */


#include "w3dquat.h"
#include "matrix3d.h"
#include "matrix4.h"
#include "wwmath.h"

#include <stdio.h>
//#include <iostream.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#define SLERP_EPSILON		0.001

static int _nxt[3] = { 1 , 2 , 0 };


// ------------------------------------------------------------
// 	local functions
// ------------------------------------------------------------
static float project_to_sphere(float,float,float);


/***********************************************************************************************
 * Quaternion::Quaternion -- constructor                                                       *
 *                                                                                             *
 * constructs a quaternion from the given axis and angle of rotation (in RADIANS of course)    *
 *                                                                                             *
 * INPUT:                                                                                      *
 * axis - axis of the rotation                                                                 *
 * angle - rotation angle                                                                      * 
 *                                                                                             *
 * OUTPUT:                                                                                     *
 *                                                                                             *
 * WARNINGS:                                                                                   *
 *                                                                                             *
 * HISTORY:                                                                                    *
 *   12/10/97   GTH : Created.                                                                 *
 *=============================================================================================*/
Quaternion::Quaternion(const Vector3 & axis,float angle)
{
	float s = sinf(angle/2);
	float c = cosf(angle/2);
	X = s * axis.X;
	Y = s * axis.Y;
	Z = s * axis.Z;
	W = c;
}


/*********************************************************************************************** 
 * Quaternion::Normalize -- Normalize to a unit quaternion                                     * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/24/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
void Quaternion::Normalize()
{
	float mag = WWMath::Sqrt(X * X + Y * Y + Z * Z + W * W);

	if (0.0f == mag) {
		return;
	} else {
		X /= mag;
		Y /= mag;
		Z /= mag;
		W /= mag;
	}
}

/*********************************************************************************************** 
 * Quaternion::operator= -- Assignment operator                                                * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/24/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion & Quaternion::operator = (const Quaternion & source)
{
  X = source[0];
  Y = source[1];
  Z = source[2];
  W = source[3];

  return *this;
}

/*********************************************************************************************** 
 * Q::Make_Closest -- Use nearest representation to the given quaternion.                      * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion & Quaternion::Make_Closest(const Quaternion & qto)
{
	float cos_t = qto.X * X + qto.Y * Y + qto.Z * Z + qto.W * W;

	// if we are on opposite hemisphere from qto, negate ourselves
	if (cos_t < 0.0) {
		X = -X;
		Y = -Y;
		Z = -Z;
		W = -W;
	}
	
	return *this;
}

/*********************************************************************************************** 
 * Trackball -- Computes a "trackball" quaternion given 2D mouse coordinates                   * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 * x0,y0 - x1,y1 - "normalized" mouse coordinates for the mouse movement                       * 
 * sphsize - size of the trackball sphere                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 * a quaternion representing the rotation of a trackball                                       * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion Trackball(float x0, float y0, float x1, float y1, float sphsize)
{
	Vector3	a;
	Vector3	p1;
	Vector3	p2;
	Vector3	d;	

	float phi,t;

	if ((x0 == x1) && (y0 == y1)) {
		return Quaternion(0.0f, 0.0f, 0.0f, 1.0f);	// Zero rotation
	}


	// Compute z coordinates for projection of p1 and p2 to
	// deformed sphere
	p1[0] = x0;
	p1[1] = y0;
	p1[2] = project_to_sphere(sphsize, x0, y0);

	p2[0] = x1;
	p2[1] = y1;
	p2[2] = project_to_sphere(sphsize, x1, y1);


	// Find their cross product
	Vector3::Cross_Product(p2,p1,&a);

	// Compute how much to rotate
	d = p1 - p2;
	t = d.Length() / (2.0f * sphsize);

	// Avoid problems with out of control values
	if (t >  1.0f) t =  1.0f;
	if (t < -1.0f) t = -1.0f;
	phi = 2.0f * asin(t);

	return Axis_To_Quat(a, phi);
}


/*********************************************************************************************** 
 * Axis_To_Quat -- Creates a quaternion given an axis and angle of rotation                    * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion Axis_To_Quat(const Vector3 &a, float phi)
{
	Quaternion q;
	Vector3 tmp = a;  

	tmp.Normalize();
	q[0] = tmp[0];
	q[1] = tmp[1];
	q[2] = tmp[2];

	q.Scale(sinf(phi / 2.0f));
	q[3] =  cosf(phi / 2.0f);

	return q;
}

/*********************************************************************************************** 
 * Slerp -- Spherical Linear interpolation!                                                    * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *  p - start quaternion                                                                       * 
 *  q - end quaternion                                                                         * 
 *  alpha - interpolating parameter                                                            * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion Slerp(const Quaternion & p,const Quaternion & q,float alpha)
{		
	float beta;				// complementary interploation parameter
	float theta;				// angle between p and q
	float sin_t,cos_t; 		// sine, cosine of theta
	float oo_sin_t;
	int qflip;					// use flip of q?
	
	// cos theta = dot product of p and q
	cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;

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

	if (1.0 - cos_t < SLERP_EPSILON) {

		// if q is very close to p, just linearly interpolate
		// between the two.
		beta = 1.0 - alpha;

	} else {

		// normal slerp!
		theta = acos(cos_t);
		sin_t = sinf(theta);
		oo_sin_t = 1.0 / sin_t;
		beta = sinf(theta - alpha*theta) * oo_sin_t;
		alpha = sinf(alpha*theta) * oo_sin_t;
	}

	if (qflip) {
		alpha = -alpha;
	}

	Quaternion res;
	res.X = beta*p.X + alpha*q.X;
	res.Y = beta*p.Y + alpha*q.Y;
	res.Z = beta*p.Z + alpha*q.Z;
	res.W = beta*p.W + alpha*q.W;

	return res;
}


/***********************************************************************************************
 * Slerp_Setup -- Get ready to call "Cached_Slerp"                                             *
 *                                                                                             *
 * INPUT:                                                                                      *
 *                                                                                             *
 * OUTPUT:                                                                                     *
 *                                                                                             *
 * WARNINGS:                                                                                   *
 *                                                                                             *
 * HISTORY:                                                                                    *
 *   2/27/98    GTH : Created.                                                                 *
 *=============================================================================================*/
void Slerp_Setup(const Quaternion & p,const Quaternion & q,SlerpInfoStruct * slerpinfo)
{
	float cos_t;
	
	assert(slerpinfo != NULL);

	// cos theta = dot product of p and q
	cos_t = p.X * q.X + p.Y * q.Y + p.Z * q.Z + p.W * q.W;

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

	if (1.0 - cos_t < SLERP_EPSILON) {

		slerpinfo->Linear = true;
		slerpinfo->Theta = 0.0f;
		slerpinfo->SinT = 0.0f;

	} else {

		slerpinfo->Linear = false;
		slerpinfo->Theta = acos(cos_t);
		slerpinfo->SinT = sinf(slerpinfo->Theta);
	
	}
}

/***********************************************************************************************
 * Cached_Slerp -- Quaternion slerping, optimized with cached values                           *
 *                                                                                             *
 * INPUT:                                                                                      *
 *                                                                                             *
 * OUTPUT:                                                                                     *
 *                                                                                             *
 * WARNINGS:                                                                                   *
 *                                                                                             *
 * HISTORY:                                                                                    *
 *   2/27/98    GTH : Created.                                                                 *
 *=============================================================================================*/
Quaternion Cached_Slerp(const Quaternion & p,const Quaternion & q,float alpha,SlerpInfoStruct * slerpinfo)
{
	float beta;				// complementary interploation parameter
	float oo_sin_t;

	if (slerpinfo->Linear) {

		// if q is very close to p, just linearly interpolate
		// between the two.
		beta = 1.0 - alpha;

	} else {

		// normal slerp!
		oo_sin_t = 1.0 / slerpinfo->Theta;
		beta = sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
		alpha = sin(alpha*slerpinfo->Theta) * oo_sin_t;
	}

	if (slerpinfo->Flip) {
		alpha = -alpha;
	}

	Quaternion res;
	res.X = beta*p.X + alpha*q.X;
	res.Y = beta*p.Y + alpha*q.Y;
	res.Z = beta*p.Z + alpha*q.Z;
	res.W = beta*p.W + alpha*q.W;

	return res;
}

void Cached_Slerp(const Quaternion & p,const Quaternion & q,float alpha,SlerpInfoStruct * slerpinfo,Quaternion * set_q)
{
	float beta;				// complementary interploation parameter
	float oo_sin_t;

	if (slerpinfo->Linear) {

		// if q is very close to p, just linearly interpolate
		// between the two.
		beta = 1.0 - alpha;

	} else {

		// normal slerp!
		oo_sin_t = 1.0 / slerpinfo->Theta;
		beta = sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
		alpha = sin(alpha*slerpinfo->Theta) * oo_sin_t;
	}

	if (slerpinfo->Flip) {
		alpha = -alpha;
	}

	set_q->X = beta*p.X + alpha*q.X;
	set_q->Y = beta*p.Y + alpha*q.Y;
	set_q->Z = beta*p.Z + alpha*q.Z;
	set_q->W = beta*p.W + alpha*q.W;
}

/*********************************************************************************************** 
 * Build_Quaternion -- Creates a quaternion from a Matrix                                      * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *   Matrix MUST NOT have scaling!                                                             *
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Quaternion Build_Quaternion(const Matrix3D & mat)
{
	float tr,s;
	int i,j,k;
	Quaternion q;

	// sum the diagonal of the rotation matrix
	tr = mat[0][0] + mat[1][1] + mat[2][2];
	
	if (tr > 0.0f) {

		s = sqrt(tr + 1.0);
		q[3] = s * 0.5;
		s = 0.5 / s;

		q[0] = (mat[2][1] - mat[1][2]) * s;
		q[1] = (mat[0][2] - mat[2][0]) * s;
		q[2] = (mat[1][0] - mat[0][1]) * s;

	} else {

		i=0;
		if (mat[1][1] > mat[0][0]) i = 1;
		if (mat[2][2] > mat[i][i]) i = 2;
		j = _nxt[i];
		k = _nxt[j];

		s = sqrt((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);

		q[i] = s * 0.5;
		if (s != 0.0) {
			s = 0.5 / s;
		}

		q[3] = 	( mat[k][j] - mat[j][k] ) * s;
		q[j] =	( mat[j][i] + mat[i][j] ) * s;    
		q[k] =	( mat[k][i] + mat[i][k] ) * s;

	}

	return q;
}

Quaternion Build_Quaternion(const Matrix3 & mat)
{
	float tr,s;
	int i,j,k;
	Quaternion q;

	// sum the diagonal of the rotation matrix
	tr = mat[0][0] + mat[1][1] + mat[2][2];
	
	if (tr > 0.0) {

		s = sqrt(tr + 1.0);
		q[3] = s * 0.5;
		s = 0.5 / s;

		q[0] = (mat[2][1] - mat[1][2]) * s;
		q[1] = (mat[0][2] - mat[2][0]) * s;
		q[2] = (mat[1][0] - mat[0][1]) * s;
	
	} else {

		i = 0;
		if (mat[1][1] > mat[0][0]) i = 1;
		if (mat[2][2] > mat[i][i]) i = 2;

		j = _nxt[i];
		k = _nxt[j];

		s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);

		q[i] =	s * 0.5;
		
		if (s != 0.0) {
			s = 0.5/s;
		}

		q[3] = 	( mat[k][j] - mat[j][k] ) * s;
		q[j] =	( mat[j][i] + mat[i][j] ) * s;    
		q[k] =	( mat[k][i] + mat[i][k] ) * s;
	}
	
	return q;
}

Quaternion Build_Quaternion(const Matrix4 & mat)
{
	float tr,s;
	int i,j,k;
	Quaternion q;

	// sum the diagonal of the rotation matrix
	tr = mat[0][0] + mat[1][1] + mat[2][2];
	
	if (tr > 0.0) {

		s = sqrt(tr + 1.0);
		q[3] = s * 0.5;
		s = 0.5 / s;

		q[0] = (mat[2][1] - mat[1][2]) * s;
		q[1] = (mat[0][2] - mat[2][0]) * s;
		q[2] = (mat[1][0] - mat[0][1]) * s;
	
	} else {

		i = 0; 
		if (mat[1][1] > mat[0][0]) i = 1;
		if (mat[2][2] > mat[i][i]) i = 2;

		j = _nxt[i];
		k = _nxt[j];

		s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);

		q[i] =	s * 0.5;
		if (s != 0.0) {
			s = 0.5/s;
		}
		q[3] = 	( mat[k][j] - mat[j][k] ) * s;
		q[j] =	( mat[j][i] + mat[i][j] ) * s;    
		q[k] =	( mat[k][i] + mat[i][k] ) * s;
	}
	
	return q;
}


/*********************************************************************************************** 
 * Build_Matrix -- Creates a Matrix from a Quaternion                                          * 
 *                                                                                             * 
 * INPUT:                                                                                      * 
 *                                                                                             * 
 * OUTPUT:                                                                                     * 
 *                                                                                             * 
 * WARNINGS:                                                                                   * 
 *                                                                                             * 
 * HISTORY:                                                                                    * 
 *   02/28/1997 GH  : Created.                                                                 * 
 *=============================================================================================*/
Matrix3	Build_Matrix3(const Quaternion & q)
{
	Matrix3 m;

	m[0][0] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
	m[0][1] = (float)(2.0 * (q[0] * q[1] - q[2] * q[3]));
	m[0][2] = (float)(2.0 * (q[2] * q[0] + q[1] * q[3]));

	m[1][0] = (float)(2.0 * (q[0] * q[1] + q[2] * q[3]));
	m[1][1] = (float)(1.0 - 2.0f * (q[2] * q[2] + q[0] * q[0]));
	m[1][2] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));

	m[2][0] = (float)(2.0 * (q[2] * q[0] - q[1] * q[3]));
	m[2][1] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
	m[2][2] =(float)(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));

	return m;
}

Matrix3D Build_Matrix3D(const Quaternion & q)
{
	Matrix3D m;

	// initialize the rotation sub-matrix
	m[0][0] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
	m[0][1] = (float)(2.0 * (q[0] * q[1] - q[2] * q[3]));
	m[0][2] = (float)(2.0 * (q[2] * q[0] + q[1] * q[3]));

	m[1][0] = (float)(2.0 * (q[0] * q[1] + q[2] * q[3]));
	m[1][1] = (float)(1.0 - 2.0f * (q[2] * q[2] + q[0] * q[0]));
	m[1][2] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));

	m[2][0] = (float)(2.0 * (q[2] * q[0] - q[1] * q[3]));
	m[2][1] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
	m[2][2] =(float)(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));

	// no translation
	m[0][3] = m[1][3] = m[2][3] = 0.0f;

	return m;
}

Matrix4 Build_Matrix4(const Quaternion & q)
{
	Matrix4 m;

	// initialize the rotation sub-matrix
	m[0][0] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
	m[0][1] = (float)(2.0 * (q[0] * q[1] - q[2] * q[3]));
	m[0][2] = (float)(2.0 * (q[2] * q[0] + q[1] * q[3]));

	m[1][0] = (float)(2.0 * (q[0] * q[1] + q[2] * q[3]));
	m[1][1] = (float)(1.0 - 2.0f * (q[2] * q[2] + q[0] * q[0]));
	m[1][2] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));

	m[2][0] = (float)(2.0 * (q[2] * q[0] - q[1] * q[3]));
	m[2][1] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
	m[2][2] = (float)(1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]));

	// no translation
	m[0][3] = m[1][3] = m[2][3] = 0.0f;

	// last row
	m[3][0] = m[3][1] = m[3][2] = 0.0f;
	m[3][3] = 1.0f;
	return m;
}
  
void Quaternion::Rotate_X(float theta)
{
	// TODO: optimize this 
	*this = (*this) * Quaternion(Vector3(1,0,0),theta);
}

void Quaternion::Rotate_Y(float theta)
{
	// TODO: optimize this 
	*this = (*this) * Quaternion(Vector3(0,1,0),theta);
}

void Quaternion::Rotate_Z(float theta)
{
	// TODO: optimize this 
	*this = (*this) * Quaternion(Vector3(0,0,1),theta);
}

float project_to_sphere(float r, float x, float y)
{
	const float SQRT2 = 1.41421356f;
	float t, z;
	float d = WWMath::Sqrt(x * x + y * y);

	if (d < r * (SQRT2/(2.0f)))			// inside sphere
		z = WWMath::Sqrt(r * r - d * d);
	else {								// on hyperbola
		t = r / SQRT2;
		z = t * t / d;
	}

	return z;
}


void Quaternion::Randomize(void)
{
	X = ((float) (rand() & 0xFFFF)) / 65536.0f;
	Y = ((float) (rand() & 0xFFFF)) / 65536.0f;
	Z = ((float) (rand() & 0xFFFF)) / 65536.0f;
	W = ((float) (rand() & 0xFFFF)) / 65536.0f;
	
	Normalize();
}