710 lines
28 KiB
C++
710 lines
28 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/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();
|
||
|
|
}
|
||
|
|
|
||
|
|
|