/* * Quaternion.cpp * * Copyright (C) 1999 Stephen F. White *  * 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 2 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 (see the file "COPYING" for details); if  * not, write to the Free Software Foundation, Inc., 675 Mass Ave,  * Cambridge, MA 02139, USA. */#include <math.h>#include "stdafx.h"#include "Quaternion.h"Quaternion::Quaternion(const Vec3f &axis, float angle){    float	    s = (float) sin(angle * 0.5f);    x = axis.x * s;    y = axis.y * s;    z = axis.z * s;    w = (float) cos(angle * 0.5f);}/* from The Matrix and Quaternions FAQhttp://skal.planet-d.net/demo/matrixfaq.htm#Q48Feel free to distribute or copy this FAQ as you please.Q48. How do I convert a rotation matrix to a quaternion?--------------------------------------------------------        |  0  1  2  3 |        |             |        |  4  5  6  7 |    m = |             |        |  8  9 10 11 |        |             |        | 12 13 14 15 |  A rotation may be converted back to a quaternion through the use of  the following algorithm:  The process is performed in the following stages, which are as follows:    Calculate the trace of the matrix T from the equation:                2     2     2      T = 4 - 4x  - 4y  - 4z                 2    2    2        = 4( 1 -x  - y  - z )        = mat[0] + mat[5] + mat[10] + 1    If the trace of the matrix is greater than zero, then    perform an "instant" calculation.      S = 0.5 / sqrt(T)      W = 0.25 / S      X = ( mat[9] - mat[6] ) * S      Y = ( mat[2] - mat[8] ) * S      Z = ( mat[4] - mat[1] ) * S    If the trace of the matrix is less than or equal to zero    then identify which major diagonal element has the greatest    value.    Depending on this value, calculate the following:      Column 0:        S  = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2;        Qx = 0.5 / S;        Qy = (mr[1] + mr[4] ) / S;        Qz = (mr[2] + mr[8] ) / S;        Qw = (mr[6] + mr[9] ) / S;      Column 1:        S  = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2;        Qx = (mr[1] + mr[4] ) / S;        Qy = 0.5 / S;        Qz = (mr[6] + mr[9] ) / S;        Qw = (mr[2] + mr[8] ) / S;      Column 2:        S  = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2;        Qx = (mr[2] + mr[8] ) / S;        Qy = (mr[6] + mr[9] ) / S;        Qz = 0.5 / S;        Qw = (mr[1] + mr[4] ) / S;     The quaternion is then defined as:       Q = | Qx Qy Qz Qw |*/Quaternion::Quaternion(const Matrix &matrix4){    const float* mat=&(*matrix4);    float trace=mat[0]+mat[5]+mat[10]+1;        if (trace>0)       {       float s = 0.5 / sqrt(trace);       x = ( mat[9] - mat[6] ) * s;       y = ( mat[2] - mat[8] ) * s;       z = ( mat[4] - mat[1] ) * s;       w = 0.25 / s;       }          else       {       int column_mindiag=0;       float mindiag=mat[0];       if (mat[5]<mindiag)          {          mindiag=mat[5];          column_mindiag=1;          }       if (mat[10]<mindiag)          {          mindiag=mat[10];          column_mindiag=2;          }       switch (column_mindiag)          {          float s;          case 0:             s  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;                 x = 0.5 / s;             y = (mat[1] + mat[4] ) / s;             z = (mat[2] + mat[8] ) / s;             w = (mat[6] + mat[9] ) / s;             break;          case 1:             s  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;             x = (mat[1] + mat[4] ) / s;             y = 0.5 / s;             z = (mat[6] + mat[9] ) / s;             w = (mat[2] + mat[8] ) / s;             break;          case 2:             s  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;             x = (mat[2] + mat[8] ) / s;             y = (mat[6] + mat[9] ) / s;             z = 0.5 / s;             w = (mat[1] + mat[4] ) / s;             break;          }       }}QuaternionQuaternion::operator *(const Quaternion &q2) const{    const Quaternion   &q1 = *this;    Quaternion	    r;    r.x = q2.w * q1.x + q2.x * q1.w + q2.y * q1.z - q2.z * q1.y;    r.y = q2.w * q1.y + q2.y * q1.w + q2.z * q1.x - q2.x * q1.z;    r.z = q2.w * q1.z + q2.z * q1.w + q2.x * q1.y - q2.y * q1.x;    r.w = q2.w * q1.w - q2.x * q1.x - q2.y * q1.y - q2.z * q1.z;    return r;}Vec3fQuaternion::operator *(const Vec3f &v) const{    Quaternion	    r(conj() * Quaternion(v.x, v.y, v.z, 0.0f) * *this);    return Vec3f(r.x, r.y, r.z);}floatQuaternion::norm() const{    return (float) sqrt(x * x + y * y + z * z + w * w);}QuaternionQuaternion::conj() const{    return Quaternion(-x, -y, -z, w);}QuaternionQuaternion::operator *(float f) const{    return Quaternion(f * x, f * y, f * z, f * w);}voidQuaternion::normalize(){    float    rlen = norm();     if (rlen>0.000000005)         rlen=1/rlen;     else         rlen=1;    x *= rlen;    y *= rlen;    z *= rlen;    w *= rlen;}Vec3f operator *(const Vec3f &v,const Quaternion &q){     return q * v; }