Subversion Repositories gelsvn

#include "CGLA.h"
#include "Vec3d.h"
#include "Vec4d.h"
#include "Quatd.h"
#include <cmath>

using namespace std;

namespace CGLA 
{
  inline Mat3x3d Quatd::get_mat3x3d() const
  {
    double s = 2/norm();
    // note that the all q_*q_ are used twice (optimize)
    return Mat3x3d(Vec3d(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])),
                   Vec3d(      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])),
                   Vec3d(      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 Mat4x4d Quatd::get_mat4x4d() const
  {
    double s = 2/norm();
    // note that the all q_*q_ are used twice (optimize?)
    return Mat4x4d(Vec4d(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),
                   Vec4d(      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),
                   Vec4d(      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),
                   Vec4d(0.0, 0.0, 0.0, 1.0));
  }

  inline void Quatd::get_rot(double& angle, Vec3d& v) 
  {
    angle = 2*acos(qw);

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

    if(angle > M_PI)
      v = -v;

    v.normalize();
  }

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

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

  inline Quatd slerp(Quatd q0, Quatd q1, double t)
  {
    double 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));
  }
}