Subversion Repositories gelsvn

#include "CGLA.h"
#include "Vec3f.h"
#include "Vec4f.h"
#include "Quatf.h"
#include <cmath>

using namespace std;

namespace CGLA 
{
  inline Mat3x3f Quatf::get_mat3x3f() const
  {
    float s = 2/norm();
    // note that the all q_*q_ are used twice (optimize)
    return Mat3x3f(Vec3f(1.0 - 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])),
                   Vec3f(      s*(qv[0]*qv[1] + qw*qv[2]),
                         1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
                               s*(qv[1]*qv[2] - qw*qv[0])),
                   Vec3f(      s*(qv[0]*qv[2] - qw*qv[1]),
                               s*(qv[1]*qv[2] + qw*qv[0]),
                         1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
  }

  inline Mat4x4f Quatf::get_mat4x4f() const
  {
    float s = 2/norm();
    // note that the all q_*q_ are used twice (optimize?)
    return Mat4x4f(Vec4f(1.0 - 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]),
                         0.0),
                   Vec4f(      s*(qv[0]*qv[1] + qw*qv[2]),
                         1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
                               s*(qv[1]*qv[2] - qw*qv[0]),
                         0.0),
                   Vec4f(      s*(qv[0]*qv[2] - qw*qv[1]),
                               s*(qv[1]*qv[2] + qw*qv[0]),
                         1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
                         0.0),
                   Vec4f(0.0, 0.0, 0.0, 1.0));
  }

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

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

    if(angle > M_PI)
      v = -v;

    v.normalize();
  }

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

  inline void Quatf::make_rot(const Vec3f& s, const Vec3f& t)
  {
    float tmp = sqrt(2*(1 + dot(s, t)));
    qv = cross(s, t)*(1.0/tmp);
    qw = tmp/2.0;    
  }

  inline Quatf slerp(Quatf q0, Quatf q1, float t)
  {
    float angle = acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1] 
                       + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
    return (q0*sin((1 - t)*angle) + q1*sin(t*angle))*(1/sin(angle));
  }
}