#include "Quat.h" // ------------------------------------------------------------------------------------------ Quat::Quat( float s, float vx, float vy, float vz ) { this->s = s; this->vx = vx; this->vy = vy; this->vz = vz; this->mat[0][3] = this->mat[1][3] = this->mat[2][3] = 0.; this->mat[3][0] = this->mat[3][1] = this->mat[3][2] = 0.; this->mat[3][3] = 1.; this->tpose[0][3] = this->tpose[1][3] = this->tpose[2][3] = 0.; this->tpose[3][0] = this->tpose[3][1] = this->tpose[3][2] = 0.; this->tpose[3][3] = 1.; } void Quat::setIdentity() { // cos( 0./2. ) = 1. // sin( 0./2. ) = 0. this->s = 1.; this->vx = 0.; this->vy = 0.; this->vz = 0.; } char * Quat::toString() { sprintf( str, "%8.3f %8.3f %8.3f %8.3f", s, vx, vy, vz ); return str; } void Quat::unitize() { // NOTE: this normalizes the vector portion, so this must be done // *before* the sin(th/2) is multiplied in float len = sqrt( vx*vx + vy*vy + vz*vz ); if( len > 0. ) { vx /= len; vy /= len; vz /= len; } } Quat operator+( const Quat& p, const Quat& q ) { Quat r = Quat( p.s+q.s, p.vx+q.vx, p.vy+q.vy, p.vz+q.vz ); return r; } Quat operator-( const Quat& p ) { Quat r = p; r.vx = - r.vx; r.vy = - r.vy; r.vz = - r.vz; return r; } Quat operator*( const Quat& p, const Quat& q ) { Quat r; // scalar term: r.s = p.s * q.s - p.vx * q.vx - p.vy * q.vy - p.vz * q.vz; // vector terms: r.vx = p.s * q.vx + p.vx * q.s + p.vy * q.vz - p.vz * q.vy; r.vy = p.s * q.vy + p.vy * q.s + p.vz * q.vx - p.vx * q.vz; r.vz = p.s * q.vz + p.vz * q.s + p.vx * q.vy - p.vy * q.vx; return r; } Quat operator*( float f, const Quat& p ) { Quat q; q.s = f * p.s; q.vx = f * p.vx; q.vy = f * p.vy; q.vz = f * p.vz; return q; } Quat operator*( const Quat& p, float f ) { return f * p; } // ------------------------------------------------------------------------------------------ Rotate::Rotate( float angr, float vx, float vy, float vz ) : Quat( 0., vx, vy, vz ) { float sn; // sine of half-angle unitize(); this->s = cos( angr/2. ); sn = sin( angr/2. ); this->vx *= sn; this->vy *= sn; this->vz *= sn; } // another form of the constructor // this is especially made for reading successive mouse locations off a virtual trackball Rotate::Rotate( float ax, float ay, float az, float bx, float by, float bz, float scale ) : Quat( 0., 0., 0., 0. ) { float la, lb; // lengths of a and b vectors float angr; // angle between a and b in radians float sn; // sine of half-angle // v = a X b: this->vx = ay*bz - az*by; this->vy = az*bx - ax*bz; this->vz = ax*by - ay*bx; unitize(); // get the angle between the vectors: la = sqrt( ax*ax + ay*ay + az*az ); lb = sqrt( bx*bx + by*by + bz*bz ); angr = acos( ( ax*bx + ay*by + az*bz ) / ( la*lb ) ); angr *= scale; // setup the quaternions: this->s = cos( angr/2. ); sn = sin( angr/2. ); this->vx *= sn; this->vy *= sn; this->vz *= sn; } float Rotate::getAng() { return 2. * acos( s ); } void Rotate::getAxis( float& vx, float& vy, float& vz ) { float sn; // sine of the half-angle sn = sqrt( 1. - this->s * this->s ); if( sn != 0. ) { vx = this->vx / sn; vy = this->vy / sn; vz = this->vz / sn; } else { vx = vy = vz = 0.; vz = 1.; } } float * Rotate::getAxis() { static float v[3]; this->getAxis( v[0], v[1], v[2] ); return v; } void Rotate::getAngAxis( float& angr, float& vx, float& vy, float& vz ) { float sn; // sine of the half-angle angr = 2. * acos( this->s ); sn = sqrt( 1. - this->s * this->s ); if( sn != 0. ) { vx = this->vx / sn; vy = this->vy / sn; vz = this->vz / sn; } else { vx = vy = 0.; vz = 1.; } } float * Rotate::getMatrix() { mat[0][0] = 1. - 2.*vy*vy - 2.*vz*vz; mat[0][1] = 2.*vx*vy - 2.*s*vz; mat[0][2] = 2.*vx*vz + 2.*s*vy; mat[1][0] = 2.*vx*vy + 2.*s*vz; mat[1][1] = 1. - 2.*vx*vx - 2.*vz*vz; mat[1][2] = 2.*vy*vz - 2.*s*vx; mat[2][0] = 2.*vx*vz - 2.*s*vy; mat[2][1] = 2.*vy*vz + 2.*s*vx; mat[2][2] = 1. - 2.*vx*vx - 2.*vy*vy; return &mat[0][0]; } float * Rotate::getOpenglMatrix() { int i, j; (void) getMatrix(); // forces conversion for( i = 0; i < 3; i++ ) for( j = 0; j < 3; j++ ) tpose[i][j] = mat[j][i]; return &tpose[0][0]; } void Rotate::printMatrix() { int i; (void) getMatrix(); // forces conversion for( i=0; i<4; i++ ) { fprintf( stderr, "\t%8.3f %8.3f %8.3f %8.3f\n", mat[i][0], mat[i][1], mat[i][2], mat[i][3] ); } } char * Rotate::toString() { float angr, ax, ay, az; getAngAxis( angr, ax, ay, az ); sprintf( str, "%8.3f degrees about ( %8.3f %8.3f %8.3f )", angr/0.01745, ax, ay, az ); return str; } Rotate& Rotate::operator=( const Quat& q ) { this->s = q.s; this->vx = q.vx; this->vy = q.vy; this->vz = q.vz; return *this; } Rotate operator*( const Rotate& p, const Rotate& q ) { Rotate r; r = (Quat)p * (Quat)q; return r; } Rotate Slerp( float t, const Quat& p, const Quat& q ) { float angr; // angle between p and q in radians float c, s; // cosine and sine of the angle between p and q float cp, cq; // coefficients to multiply quaternions p and q Rotate r; // dot product to get the angle between p and q: c = p.s*q.s + p.vx*q.vx + p.vy*q.vy + p.vz*q.vz; angr = acos( c ); // sine of that angle: s = sin( angr ); // if the sine is 0., then p == q: if( s == 0. ) { r = p; return r; } // do spherical interpolation: cp = sin( (1.-t)*angr ) / s; cq = sin( t *angr ) / s; r = cp*p + cq*q; return r; } const float EPSILON = { 0.0001f }; int operator==( Rotate& p, Rotate& q ) { float *pm = p.getMatrix(); float *qm = q.getMatrix(); for( int i = 0; i < 16; i++, pm++, qm++ ) { if( fabs( *pm - *qm ) > EPSILON ) return 0; } return ( !0 ); } int operator!=( Rotate& p, Rotate& q ) { return ! ( p == q ); } // ------------------------------------------------------------------------------------------ Point::Point( float px = 0., float py = 0., float pz = 0. ) : Quat( 0., px, py, pz ) { } char * Point::toString() { sprintf( str, "%8.3f %8.3f %8.3f", vx, vy, vz ); return str; } Point operator*( const Rotate& p, const Point& q ) { Quat rr; rr = (Quat)p * (Quat)q * (Quat)(-p); Point r = Point( rr.vx, rr.vy, rr.vz ); return r; } #define TEST #ifdef TEST int main( int argc, char *argv[ ] ) { Rotate r1 = Rotate( 45.*D2R, 0., 0., 1. ); Rotate r2 = Rotate( 90.*D2R, 0., 0., 1. ); Rotate r3 = Rotate( 90.*D2R, 1., 0., 0. ); Rotate r4 = r2 * r1; Rotate r5 = r3 * r2 * r1; fprintf( stderr, "r1 = %s\n", r1.toString() ); fprintf( stderr, "r2 = %s\n", r2.toString() ); fprintf( stderr, "r2*r1 = %s\n", r4.toString() ); fprintf( stderr, "r3 = %s\n", r3.toString() ); fprintf( stderr, "r3*r2*r1 = %s\n", r5.toString() ); fprintf( stderr, "\nr3*r2*r1 matrix =\n" ); r5.printMatrix(); Point p1 = Point( 1., 1., 0. ); Point p2 = r4 * p1; fprintf( stderr, "Original point = %s\n", p1.toString() ); fprintf( stderr, "Transformed point = %s\n", p2.toString() ); // try interpolating from r1 to r5: const int N = 10; float dt = 1. / (float)( N - 1 ); float t = 0.; for( int i = 0; i < N; i++, t += dt ) { Rotate r15 = Slerp( t, r1, r5 ); fprintf( stderr, "%2d %5.3f %s\n", i, t, r15.toString() ); } } #ifdef SHOW_WHAT_RESULTS_SHOULD_BE r1 = 45.000 degrees about ( 0.000 0.000 1.000 ) r2 = 90.000 degrees about ( 0.000 0.000 1.000 ) r2*r1 = 135.000 degrees about ( 0.000 0.000 1.000 ) r3 = 90.000 degrees about ( 1.000 0.000 0.000 ) r3*r2*r1 = 148.606 degrees about ( 0.281 -0.678 0.679 ) r3*r2*r1 matrix = -0.707 -0.707 0.000 0.000 0.000 0.000 -1.000 0.000 0.707 -0.707 0.000 0.000 0.000 0.000 0.000 1.000 Original point = 1.000 1.000 0.000 Transformed point = -1.414 0.001 0.000 0 0.000 45.000 degrees about ( 0.000 0.000 1.000 ) 1 0.111 53.754 degrees about ( 0.080 -0.194 0.978 ) 2 0.222 64.007 degrees about ( 0.136 -0.328 0.935 ) 3 0.333 75.145 degrees about ( 0.175 -0.423 0.889 ) 4 0.444 86.824 degrees about ( 0.204 -0.493 0.846 ) 5 0.556 98.848 degrees about ( 0.226 -0.546 0.807 ) 6 0.667 111.101 degrees about ( 0.244 -0.588 0.771 ) 7 0.778 123.508 degrees about ( 0.258 -0.623 0.739 ) 8 0.889 136.021 degrees about ( 0.270 -0.652 0.708 ) 9 1.000 148.606 degrees about ( 0.281 -0.678 0.679 ) #endif #endif