/*
** 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 .
*/
/* $Header: /Commando/Code/wwmath/quat.cpp 38 8/28/01 10:26a Greg_h $ */
/***********************************************************************************************
*** 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 "quat.h"
#include "matrix3d.h"
#include "matrix4.h"
#include "wwmath.h"
#include
#include
#include
#include
#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 = WWMath::Sin(angle/2);
float c = WWMath::Cos(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 len2=X * X + Y * Y + Z * Z + W * W;
if (0.0f == len2) {
return;
} else {
float inv_mag = WWMath::Inv_Sqrt(len2);
X *= inv_mag;
Y *= inv_mag;
Z *= inv_mag;
W *= inv_mag;
}
}
/***********************************************************************************************
* 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 * WWMath::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(WWMath::Sin(phi / 2.0f));
q[3] = WWMath::Cos(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. *
*=============================================================================================*/
#if 0
#pragma warning (disable : 4725)
#define ARC_TABLE_SIZE_MASK 1023
#define SIN_TABLE_SIZE_MASK 1023
void __cdecl Fast_Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
{
float float_epsilon2=WWMATH_EPSILON * WWMATH_EPSILON;
float HalfOfArcTableSize=float(ARC_TABLE_SIZE/2);
float HalfOfSinTableSize=float(SIN_TABLE_SIZE/2);
const unsigned ARC_TABLE_SIZE_PER_2=ARC_TABLE_SIZE/2;
float beta; // complementary interploation parameter
float theta; // angle between p and q
__asm {
mov esi, p
mov edi, q
fld1 // we'll need 1.0 and 0.0 later
// ----------------------------------------------------------------------------
// 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;
// }
// ----------------------------------------------------------------------------
fld dword ptr [esi] // p.X
fmul dword ptr [edi] // p.X*q.X
fld dword ptr [esi+08h] // p.Y
fmul dword ptr [edi+08h] // p.Y*q.Y
fld dword ptr [esi+04h] // p.Z
fmul dword ptr [edi+04h] // p.Z*q.Z
fld dword ptr [edi+0ch] // p.W
fmul dword ptr [esi+0ch] // p.W*q.W
faddp st(2), st(0) // y+=w
faddp st(2), st(0) // x+=z
faddp st(1),st(0) // x+z + y+w
fst beta
fabs
mov ebx,beta
and ebx,0x80000000
// ----------------------------------------------------------------------------
// if q is very close to p, just linearly interpolate
// between the two.
// if (1.0 - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
// beta = 1.0 - alpha;
// }
// ----------------------------------------------------------------------------
fld st(0) // duplicate st(0), which contains cos_t
fsubr st(0),st(2) // st(2) contains 1.0
fcomp float_epsilon2
fnstsw ax
test ah, 01h
je normal_slerp
fld alpha
fsubr st(0),st(1) // st(1) contains 1.0
fstp beta
jmp done_slerp
normal_slerp:
// ----------------------------------------------------------------------------
// normal slerp!
// else {
// theta = WWMath::Acos(cos_t);
// sin_t = WWMath::Sin(theta);
// oo_sin_t = 1.0 / sin_t;
// beta = WWMath::Sin(theta - alpha*theta) * oo_sin_t;
// alpha = WWMath::Sin(alpha*theta) * oo_sin_t;
// }
// if (qflip) {
// alpha = -alpha;
// }
// ----------------------------------------------------------------------------
fld HalfOfSinTableSize
fld HalfOfArcTableSize
fmul st(0),st(2) // cos_t * (ARC_TABLE_SIZE/2)
fistp theta // convert to integer
mov eax,theta
add eax,ARC_TABLE_SIZE_PER_2
jns no_neg
xor eax,eax
jmp contin
no_neg:
cmp eax,ARC_TABLE_SIZE
jl contin // Note: Use Setcc/Movcc here!!!
mov eax,ARC_TABLE_SIZE_MASK
contin:
fld dword ptr[_FastAcosTable+eax*4]
fst theta
fmul st(0),st(1) // theta * (sin_table_size/2)
fadd st(0),st(1) // theta * (sin_table_size/2) + (sin_table_size/2)
fistp beta // conver to integer
mov ecx,SIN_TABLE_SIZE_MASK
mov eax,beta
and eax,ecx // & SIN_TABLE_SIZE_MASK
fld dword ptr[_FastInvSinTable+eax*4] // 1.0f/sin(theta)
fld theta
fmul alpha // theta*alpha
fld st(0) // duplicate stack head as we need theta*alpha later
fsubr theta // theta-theta*alpha
fmul st(0),st(3) // (theta-theta*alpha)*HalfOfSinTableSize
fadd st(0),st(3) // (theta-theta*alpha)*HalfOfSinTableSize+HalfOfSinTableSize
fistp beta // convert to integer
mov eax,beta
and eax,ecx // & SIN_TABLE_SIZE_MASK
fld dword ptr[_FastSinTable+eax*4] // sin(theta-theta*alpha)
fmul st(0),st(2) // sin(theta-theta*alpha) * oo_sin_t
fstp beta
fmul st(0),st(2) // (theta*alpha)*HalfOfSinTableSize
fadd st(0),st(2) // (theta*alpha)*HalfOfSinTableSize+HalfOfSinTableSize
fistp theta // convert to integer
mov eax,theta
and eax,ecx // & SIN_TABLE_SIZE_MASK
fld dword ptr[_FastSinTable+eax*4] // sin(theta*alpha)
fmul st(0),st(1) // oo_sin_t
fstp alpha
fstp st(0) // pop oo_sin_t
fstp st(0) // pop HalfOfSinTableSize
done_slerp:
test ebx, ebx
je no_negative
fld alpha
fchs
fstp alpha
no_negative:
// ----------------------------------------------------------------------------
// 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;
// ----------------------------------------------------------------------------
fstp st(0) // pop cos_t
fstp st(0) // pop 1.0
fld alpha
fld dword ptr [edi+4] // q.Y
fmul st(0),st(1) // alpha*q.Y
fld dword ptr [edi+8] // q.Z
fmul st(0),st(2) // alpha*q.Z
fld dword ptr [edi+12] // q.W
fmul st(0),st(3) // alpha*q.W
fld dword ptr [edi] // q.X
fmulp st(4),st // alpha*q.X
fld beta
fld dword ptr [esi+4] // p.Y
fmul st(0),st(1) // beta*p.Y
fld dword ptr [esi+8] // p.Z
fmul st(0),st(2) // beta*p.Z
fld dword ptr [esi] // p.X
fmul st(0),st(3) // beta*p.X
fxch st(3) // move beta to top of stack
fmul dword ptr [esi+12] // beta*p.W
faddp st(4),st // w
faddp st(4),st // z
faddp st(4),st // y
faddp st(4),st // x
mov ecx, res
fstp [ecx+12] // w
fstp [ecx+8] // z
fstp [ecx+4] // y
fstp [ecx] // x
}
}
#else
void __cdecl Fast_Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
{
float beta; // complementary interploation parameter
float theta; // angle between p and q
float 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.0f) {
cos_t = -cos_t;
qflip = true;
} else {
qflip = false;
}
if (1.0f - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
// if q is very close to p, just linearly interpolate
// between the two.
beta = 1.0f - alpha;
} else {
theta = WWMath::Fast_Acos(cos_t);
float sin_t = WWMath::Fast_Sin(theta);
oo_sin_t = 1.0f / sin_t;
beta = WWMath::Fast_Sin(theta - alpha*theta) * oo_sin_t;
alpha = WWMath::Fast_Sin(alpha*theta) * oo_sin_t;
}
if (qflip) {
alpha = -alpha;
}
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;
}
#endif // MSC_VER
void Slerp(Quaternion& res, const Quaternion & p,const Quaternion & q,float alpha)
{
float beta; // complementary interploation parameter
float theta; // angle between p and q
//float sin_t
float 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.0f) {
cos_t = -cos_t;
qflip = true;
} else {
qflip = false;
}
if (1.0f - cos_t < WWMATH_EPSILON * WWMATH_EPSILON) {
// if q is very close to p, just linearly interpolate
// between the two.
beta = 1.0f - alpha;
} else {
// normal slerp!
theta = WWMath::Acos(cos_t);
float sin_t = WWMath::Sin(theta);
oo_sin_t = 1.0f / sin_t;
beta = WWMath::Sin(theta - alpha*theta) * oo_sin_t;
alpha = WWMath::Sin(alpha*theta) * oo_sin_t;
}
if (qflip) {
alpha = -alpha;
}
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;
}
/***********************************************************************************************
* 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.0f) {
cos_t = -cos_t;
slerpinfo->Flip = true;
} else {
slerpinfo->Flip = false;
}
if (1.0f - cos_t < SLERP_EPSILON) {
slerpinfo->Linear = true;
slerpinfo->Theta = 0.0f;
slerpinfo->SinT = 0.0f;
} else {
slerpinfo->Linear = false;
slerpinfo->Theta = WWMath::Acos(cos_t);
slerpinfo->SinT = WWMath::Sin(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.0f - alpha;
} else {
// normal slerp!
oo_sin_t = 1.0f / slerpinfo->Theta;
beta = WWMath::Sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
alpha = WWMath::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.0f - alpha;
} else {
// normal slerp!
oo_sin_t = 1.0f / slerpinfo->Theta;
beta = WWMath::Sin(slerpinfo->Theta - alpha*slerpinfo->Theta) * oo_sin_t;
alpha = WWMath::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();
}