Subversion Repositories gelsvn

#include "Vec3f.h"
#include "Quaternion.h"

namespace CGLA 
{

        void Quaternion::make_rot(const Mat4x4f& m)
        {
                float trace = m[0][0] + m[1][1] + m[2][2]  + 1.0f;

                //If the trace of the matrix is greater than zero, then
                //perform an "instant" calculation.
                if ( trace > TINY ) 
                {
                        float S = sqrt(trace) * 2.0f;
                        qv = Vec3f(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );
                        qv /= S;
                        qw = 0.25f * S;
                }
                else
                {
                        //If the trace of the matrix is equal to zero (or negative...) then identify
                        //which major diagonal element has the greatest value.
                        //Depending on this, calculate the following:

                        if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )  {        // Column 0: 
                                float S  = sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2] ) * 2.0f;
                                qv[0] = 0.25f * S;
                                qv[1] = (m[1][0] + m[0][1] ) / S;
                                qv[2] = (m[0][2] + m[2][0] ) / S;
                                qw = (m[2][1] - m[1][3] ) / S;
                        } else if ( m[1][1] > m[2][2] ) {                       // Column 1: 
                                float S  = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0f;
                                qv[0] = (m[1][0] + m[0][1] ) / S;
                                qv[1] = 0.25f * S;
                                qv[2] = (m[2][1] + m[1][2] ) / S;
                                qw = (m[0][2] - m[2][0] ) / S;
                        } else {                                                // Column 2:
                                float S  = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0f;
                                qv[0] = (m[0][2] + m[2][0] ) / S;
                                qv[1] = (m[2][1] + m[1][2] ) / S;
                                qv[2] = 0.25f * S;
                                qw = (m[1][0] - m[0][1] ) / S;
                        }
                }
                //The quaternion is then defined as:
                //  Q = | X Y Z W |
        }

        void Quaternion::make_rot(float angle, const Vec3f& v)
  {
    angle = angle/2;
    qv = CGLA::normalize(v);
    qv *= sin(angle);
    qw  = cos(angle);
  };

  void Quaternion::make_rot(const Vec3f& _v0, const Vec3f& _v1)
  {
    Vec3f v0 = CGLA::normalize(_v0);
    Vec3f v1 = CGLA::normalize(_v1);
    qv = cross(v0, v1);
    float l = qv.length();
    if(l<TINY)
      qv = Vec3f(1,0,0);
    else
      qv.normalize();
    float a = acos(dot(v0,v1))/2;
    qw  = cos(a);
    qv *= sin(a);       
  };

  void Quaternion::get_rot(float& angle, Vec3f& v) 
  {
    angle=2*acos(qw);

    if (angle<TINY) 
      v = Vec3f(1,0,0);
    else 
      v = qv / sin(angle);

    if (angle>M_PI)
      v = -v;

    v.normalize();
  }

  Mat3x3f Quaternion::get_mat3x3f() const
  {
    float s=2/norm();
    float m[9] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]), 
                  s*(qv[0]*qv[1]-qw*qv[2]), 
                  s*(qv[0]*qv[2]+qw*qv[1]), 
                  s*(qv[0]*qv[1]+qw*qv[2]), 
                  1 - s*(qv[0]*qv[0]+qv[2]*qv[2]), 
                  s*(qv[1]*qv[2]-qw*qv[0]), 
                  s*(qv[0]*qv[2]-qw*qv[1]), 
                  s*(qv[1]*qv[2]+qw*qv[0]), 
                  1 - s*(qv[0]*qv[0]+qv[1]*qv[1])};
    Mat3x3f mat;
    raw_assign(mat, m);
    return mat;
  }


  //This function just need to call the right initialiser

  Mat4x4f Quaternion::get_mat4x4f() const
  {
    float s=2/norm();
    float m[16] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]), 
                   s*(qv[0]*qv[1]-qw*qv[2]), 
                   s*(qv[0]*qv[2]+qw*qv[1]), 
                   float(0),
                   s*(qv[0]*qv[1]+qw*qv[2]), 
                   1 - s*(qv[0]*qv[0]+qv[2]*qv[2]), 
                   s*(qv[1]*qv[2]-qw*qv[0]), 
                   float(0),
                   s*(qv[0]*qv[2]-qw*qv[1]), 
                   s*(qv[1]*qv[2]+qw*qv[0]), 
                   1 - s*(qv[0]*qv[0]+qv[1]*qv[1]), 
                   float(0),
                   float(0),             
                   float(0),                   
                   float(0),                
                   float(1)};
    Mat4x4f mat;
    raw_assign(mat, m);
    return mat;
  }


  Vec3f Quaternion::apply(const Vec3f& vec) const
  {
    return Vec3f((*this)*Quaternion(vec)*inverse());
  }


}