Subversion Repositories gelsvn

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
48 jrf 1 
#include "CGLA.h"
2 
#include "Vec3d.h"
3 
#include "Vec4d.h"
4 
#include "Quatd.h"
5 
#include <cmath>
6 
 
7 
using namespace std;
8 
 
9 
namespace CGLA
10 
{
11 
  inline Mat3x3d Quatd::get_mat3x3d() const
12 
  {
13 
    double s = 2/norm();
14 
    // note that the all q_*q_ are used twice (optimize)
15 
    return Mat3x3d(Vec3d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
16 
			       s*(qv[0]*qv[1] - qw*qv[2]),
17 
			       s*(qv[0]*qv[2] + qw*qv[1])),
18 
		   Vec3d(      s*(qv[0]*qv[1] + qw*qv[2]),
19 
			 1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
20 
			       s*(qv[1]*qv[2] - qw*qv[0])),
21 
		   Vec3d(      s*(qv[0]*qv[2] - qw*qv[1]),
22 
			       s*(qv[1]*qv[2] + qw*qv[0]),
23 
			 1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
24 
  }
25 
 
26 
  inline Mat4x4d Quatd::get_mat4x4d() const
27 
  {
28 
    double s = 2/norm();
29 
    // note that the all q_*q_ are used twice (optimize?)
30 
    return Mat4x4d(Vec4d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
31 
			       s*(qv[0]*qv[1] - qw*qv[2]),
32 
			       s*(qv[0]*qv[2] + qw*qv[1]),
33 
			 0.0),
34 
		   Vec4d(      s*(qv[0]*qv[1] + qw*qv[2]),
35 
			 1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
36 
			       s*(qv[1]*qv[2] - qw*qv[0]),
37 
			 0.0),
38 
		   Vec4d(      s*(qv[0]*qv[2] - qw*qv[1]),
39 
			       s*(qv[1]*qv[2] + qw*qv[0]),
40 
			 1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
41 
			 0.0),
42 
		   Vec4d(0.0, 0.0, 0.0, 1.0));
43 
  }
44 
 
45 
  inline void Quatd::get_rot(double& angle, Vec3d& v)
46 
  {
47 
    angle = 2*acos(qw);
48 
 
49 
    if(angle < TINY)
50 
      v = Vec3d(1.0, 0.0, 0.0);
51 
    else
52 
      v = qv*(1/sin(angle));
53 
 
54 
    if(angle > M_PI)
55 
      v = -v;
56 
 
57 
    v.normalize();
58 
  }
59 
 
60 
  inline void Quatd::make_rot(double angle, const Vec3d& v)
61 
  {
62 
    angle /= 2.0;
63 
    qv = CGLA::normalize(v)*sin(angle);
64 
    qw = cos(angle);
65 
  }
66 
 
67 
  inline void Quatd::make_rot(const Vec3d& s, const Vec3d& t)
68 
  {
69 
    double tmp = sqrt(2*(1 + dot(s, t)));
70 
    qv = cross(s, t)*(1.0/tmp);
71 
    qw = tmp/2.0;
72 
  }
73 
 
74 
  inline Quatd slerp(Quatd q0, Quatd q1, double t)
75 
  {
76 
    double angle = acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1]
77 
		       + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
78 
    return (q0*sin((1 - t)*angle) + q1*sin(t*angle))*(1/sin(angle));
79 
  }
80 
}