// Copyright (C) Liang Yi, 2004 // Last modified: 2004.8. Hangzhou
#ifndef rotation_header
#define rotation_header
#include "matrix.h"
Const Double delta_ROT = 1.0e-10;
Template
Class quaternion;
Template
Class Rotation;
Template
Quaternion
RotationToquaternion
Const rotation
& r)
// r
Be normalized
{
T cosvalue, sinvalue, theta;
Theta = R.angle / 180.0F * m_pi;
Cosvalue = T (COS (Theta / 2.0F));
SINVALUE = T (SIN (Theta / 2.0F));
Return quaternion
(SINVALUE * R.AXIS.X,
SINVALUE * R.AXIS.Y,
SINVALUE * R.AXIS.Z,
CosValue);
}
Template
Rotation
QuaternionTorotation
Const quaternion
& q)
// q
Be normalized
{
IF ((Q.W> 1-Delta_ROT) && (Q.W <1 Delta_ROT) ||
((q.w> -1-delta_rot) && (q.w <-1 delta_rot)))))))
{
Return Rotation
();
}
T sinvalue, haldheta;
HALFTHETA = T (ACOS (Q.W));
SINVALUE = T (Halftheta);
IF (sinvalue { Return Rotation (); } Return Rotation (Q.X / SINVALUE, Q.Y / SINVALUE, Q.Z / SINVALUE, Halftheta * 2.0F * 180.0F / 3.14159F); } Template Void QuaternionTomatrix (T M [16], Const Quaternion & q) // q Be normalized { T M [4] [4]; T WX, WY, WZ, XX, YY, YZ, XY, XZ, ZZ, X2, Y2, Z2; // Calculate Coefficients X2 = Q.X * 2.0F; Y2 = Q.Y * 2.0F; Z2 = q.z * 2.0F; XX = q.x * x2; xy = q.x * y2; xz = q.x * z2; YY = q.y * y2; yz = q.y * z2; zz = q.z * z2; WX = q.w * x2; wy = q.w * y2; wz = q.w * z2; M [0] [0] = 1.0F - (YY ZZ); M [0] [1] = XY WZ; M [0] [2] = xz - wy; M [0] [3] = 0.0F; M [1] [0] = XY - WZ; M [1] [1] = 1.0F - (XX ZZ); M [1] [2] = YZ WX; M [1] [3] = 0.0F; M [2] [0] = XZ WY; M [2] [1] = yz - wx; M [2] [2] = 1.0F - (xx yy); M [2] [3] = 0.0F; M [3] [0] = 0; M [3] [1] = 0; M [3] [2] = 0; M [3] [3] = 1; For (SIZE_T I = 0; i <4; i ) For (SIZE_T J = 0; J <4; J ) M [i * 4 j] = m [i] [j]; } Template Void QuaternionTomatrix_opengl (T M [16], Const Quaternion & q) { T M [4] [4]; // Column Major Format (OpenGL Format) T WX, WY, WZ, XX, YY, YZ, XY, XZ, ZZ, X2, Y2, Z2; // Calculate Coefficients X2 = Q.X * 2.0F; Y2 = Q.Y * 2.0F; Z2 = q.z * 2.0F; XX = q.x * x2; xy = q.x * y2; xz = q.x * z2; YY = q.y * y2; yz = q.y * z2; zz = q.z * z2; WX = q.w * x2; wy = q.w * y2; wz = q.w * z2; M [0] [0] = 1.0F - (YY ZZ); M [0] [1] = xy - wz; M [0] [2] = XZ WY; M [0] [3] = 0.0F; M [1] [0] = XY WZ; M [1] [1] = 1.0F - (XX ZZ); M [1] [2] = yz - wx; M [1] [3] = 0.0F; M [2] [0] = xz - wy; M [2] [1] = YZ WX; M [2] [2] = 1.0F - (xx yy); M [2] [3] = 0.0F; M [3] [0] = 0; M [3] [1] = 0; M [3] [2] = 0; M [3] [3] = 1; For (int i = 0; i <4; i ) For (int J = 0; j <4; j ) M [i * 4 j] = m [i] [j]; // column major format (OpenGL Format) } Template Quaternion Vectortoquaternion Const Vector3D & v) { Return quaternion (V.x, v.y, v.z, 0); } Template Vector3D QuaternionTovector Const quaternion Q) { Return Vector3D (Q.X, Q.Y, Q.Z); } // v After the Q transform is V ', the function returns V' Template Vector3D Vectortransform Const Vector3D & V, Const quaternion & Q) { IF ((Q.W> 1-Delta_ROT) && (Q.W <1 Delta_ROT)) || (Q.W> -1-Delta_ROT) && (Q.W <-1 Delta_ROT)) Return V; Quaternion A, B, C; A = vectortoquaternion (v); B = q; B.INVERSE () C = q * a * b; Return QuaternionTovector (C); } Template Class Rotation { PUBLIC: Vector3D Axis; TANGLE; PUBLIC: Rotation (T x = 0, T y = 0, T z = 1.0, t_angle = 0) { Axis.x = x; Axis.y = y; Axis.z = z; Angle = _angle; } Rotation (Vector3D) & v, t_angle = 0) { AXIS = V; Angle = _angle; } } Template Class quaternion { PUBLIC: T x, y, z, w; PUBLIC: Quaternion (t _x = 0, t _y = 0, t_z = 1, t_w = 0) { X = _x; Y = _y; z = _z; W = _w; } T MAGNITUDE () { RETURN SQRT (x * x y * y z * z w * w); } Void Normalize (Quaternion & OUT { T mgnitude (); IF (m> 0) { Out.x = x / m; Out.y = y / m; OUT.Z = z / m; Out.w = w / m; } } void normalize () { Normalize (* this); } Void Inverse (Quaternion & OUT { T mgnitude (); IF (m> 0) { Out.x = -x / m; Out.y = -y / m; OUT.Z = -Z / m; Out.w = -w / m; } } Void inVerse () { INVERSE (* THIS); } Void Conjugate (Quaternion & OUT { Out.x = -x; Out.y = -y; Out.z = -z; } Void conjugate () { CONJUGATE (* this); } Quaternion Operator (Quaternion & q) { Return quaternion (x Q.X, Y Q.Y, Z Q.Z, W Q.W); } Quaternion Operator - Quaternion & q) { Return quaternion (x-q.x, y-q.y, z-q.z, w-q.w); } Quaternion Operator * ( Const quaternion & q) { Return quaternion (w * q.x x * q.w y * q.z - z * q.y, w * q.y y * q.w z * q.x - x * q.z, W * q.z z * q.w x * q.y - y * q.x, w * q.w - x * q.x - y * q.y - z * q.z); } Quaternion Operator * (t s) { Return quaternion (x * s, y * s, z * s, w * s); } // Nick Bobick write // Purpose: Spherical Linear Interpolation Between Two Quaternions // Arguments: Two quaternions, blend factor Quaternion Slerp Const quaternion & from, Const quaternion & to, t t) { T TO1 [4]; T Omega, Cosom, Sinom, Scale0, Scale1; // Calc Cosine Cosom = from.x * to.x from.y * to.y from.z * to.z from.w * to.w; // Adjust Signs (if Necessary) IF (cosom <0.0) {cosom = -cosom; TO1 [0] = - to.x; TO1 [1] = - to.y; TO1 [2] = - to.z; TO1 [3] = - to.w; } else { TO1 [0] = TO.X; TO1 [1] = to.y; TO1 [2] = to.z; TO1 [3] = to.w; } // Calculate Coefficients IF ((1.0 - Cosom)> Delta_ROT) { // Standard Case (Slerp) Omega = T (ACOS (Cosom)); Sinom = T (Sin (OMEGA)); Scale0 = T (SIN ((1.0 - t) * omega) / sinom); Scale1 = T (SIN (T * Omega) / Sinom); } else { // "from" and "to" quaternions are very close close // ... SO We can do a linear interpolation Scale0 = 1.0F - t; Scale1 = T; } // Calculate Final Values Return (scale0 * from.x scale1 * to1 [0], Scale0 * from.y scale1 * to1 [1], Scale0 * from.z scale1 * TO1 [2], Scale0 * from.w scale1 * to1 [3]); } } #ENDIF
