//--------------------------------------------------------------------------- // BasicTypes.cpp // // (C) COPYRIGHT John Henckel, mailto:henckel@iname.com Aug 1998 // // Permission to use, copy, modify, distribute and sell this software // and its documentation for any purpose is hereby granted without fee, // provided that the above copyright notice appear in all copies. // // Visit my website! http://www.GeoCities.com/Paris/6502 #include "stdafx.h" #include "BasicTypes.h" #ifdef _DEBUG #define new DEBUG_NEW #undef THIS_FILE static char THIS_FILE[] = __FILE__; #endif //--------------------------------------------------------------------------- GLdouble Quat::matrix[16]; //--------------------------------------------------------------------------- // Construct a quaternion as the product of two other quaternions. // The formula is // [ w1w2 - x1x2 - y1y2 - z1z2, // w1x2 + x1w2 + y1z2 - z1y2, // w1y2 - x1z2 + y1w2 + z1x2, // w1z2 + x1y2 - y1x2 + z1w2 ]. Quat::Quat(const Quat& p, const Quat& q) { w = p.w*q.w - p.x*q.x - p.y*q.y - p.z*q.z; x = p.w*q.x + p.x*q.w + p.y*q.z - p.z*q.y; y = p.w*q.y - p.x*q.z + p.y*q.w + p.z*q.x; z = p.w*q.z + p.x*q.y - p.y*q.x + p.z*q.w; } //--------------------------------------------------------------------------- // Construct a quaternion from a ROTATION ANGLE and VECTOR // // If the rotation is (t, x, y, z) then the quaternion is // [ cos(t/2), sin(t/2)*x, sin(t/2)*y, sin(t/2)*z ]. // // NOTICE (ax, ay, az) must be a UNIT VECTOR or else results are undefined. Quat::Quat(double theta, double ax, double ay, double az) { double s = sin(theta/2); w = cos(theta/2); x = s*ax; y = s*ay; z = s*az; } //--------------------------------------------------------------------------- // Return an OpenGL 4x4 column-major rotation matrix // // Notice, the returned memory is REUSED. It is shared and not thread safe. GLdouble* Quat::AsMatrix() const { double ww = w*w, wx = 2.0*w*x, wy = 2.0*w*y, wz = 2.0*w*z; double xx = x*x, xy = 2.0*x*y, xz = 2.0*x*z; double yy = y*y, yz = 2.0*y*z; double zz = z*z; matrix[ 0] = ww + xx - yy - zz; matrix[ 1] = xy - wz; matrix[ 2] = xz + wy; matrix[ 3] = 0.0; matrix[ 4] = xy + wz; matrix[ 5] = ww - xx + yy - zz; matrix[ 6] = yz - wx; matrix[ 7] = 0.0; matrix[ 8] = xz - wy; matrix[ 9] = yz + wx; matrix[10] = ww - xx - yy + zz; matrix[11] = 0.0; matrix[12] = 0.0; matrix[13] = 0.0; matrix[14] = 0.0; matrix[15] = ww + xx + yy + zz; // this should be 1.0 return matrix; } //--------------------------------------------------------------------------- void Quat::operator *= (const Quat& q) { double xx = w*q.x + x*q.w + y*q.z - z*q.y; double yy = w*q.y - x*q.z + y*q.w + z*q.x; double zz = w*q.z + x*q.y - y*q.x + z*q.w; w = w*q.w - x*q.x - y*q.y - z*q.z; x = xx; y = yy; z = zz; } //--------------------------------------------------------------------------- // This is the spherical linear interpolation (SLERP) void Quat::Slerp(const Quat& p, double alpha, int spins) { double beta = 1.0 - alpha; // linear interpolation double cos_t = Dot(p); if (1.0 - cos_t > EPSILON) { // A and B are not very close double sg = (cos_t < 0.0) ? -1.0 : 1.0; cos_t *= sg; double theta = acos(cos_t); double phi = theta + spins * M_PI; // theta plus spins double sin_t = sin(theta); beta = sin(theta - alpha*phi) / sin_t; alpha = sg * sin(alpha*phi) / sin_t; } /* interpolate */ x = beta*x + alpha*p.x; y = beta*y + alpha*p.y; z = beta*z + alpha*p.z; w = beta*w + alpha*p.w; } //--------------------------------------------------------------------------- // Convert the quaternion to the angular velocity vector // // The direction of the result is (x,y,z) and the magnitude is equal to // the rate of spin (possibly negative). Vect Quat::SpinVector() const { double h = sqrt(x*x + y*y + z*z); if (h < EPSILON) return Vect(); double t = 2*atan2(h,w)/h; return Vect(x*t, y*t, z*t); } //--------------------------------------------------------------------------- // Rotate the vector using a quaternion. void Vect::Rotate(const Quat& p) { GLdouble* m = p.AsMatrix(); double x1 = x*m[0] + y*m[1] + z*m[2]; double y1 = x*m[4] + y*m[5] + z*m[6]; z = x*m[8] + y*m[9] + z*m[10]; x = x1; y = y1; } //--------------------------------------------------------------------------- // Return the smallest angle between two vectors. // The result is always between -pi and pi. double Vect::AngleTo(const Vect& v) { double len = Length() * v.Length(); if (len < EPSILON) return 0; double sn = Cross(v).Length() / len; // sine (non-negative) double cs = Dot(v) / len; // cosine return 2*atan2(sn,cs); } //--------------------------------------------------------------------------- // Returns a unit vector that is a suitable axis for rotating from one // vector to another. Returns (0,1,0) if either vector has zero len. Vect Vect::AxisTo(const Vect& v) { double len = Length() * v.Length(); if (len < EPSILON) return Vect(0,1,0); Vect axis = Cross(v); axis.Normalize(); return axis; } //--------------------------------------------------------------------------- // Matrix constructor Matrix::Matrix(int numrow, int numcol) : numr(numrow), numc(numcol) { m = new double* [numr]; int i; for (i=0; i