918 lines
33 KiB
C++
918 lines
33 KiB
C++
/*
|
|
** 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/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 <stdio.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 = 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();
|
|
}
|
|
|
|
|