1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
/************************************************************************
Quaternion class
$Id: quat.cxx 427 2004-09-27 04:45:31Z garland $
************************************************************************/
#include <gfx/quat.h>
namespace gfx
{
// Based on code from the Appendix of
// Quaternion Calculus for Computer Graphics, by Ken Shoemake
// Computes the exponential of a quaternion, assuming scalar part w=0
Quat exp(const Quat& q)
{
double theta = norm(q.vector());
double c = cos(theta);
if( theta > FEQ_EPS )
{
double s = sin(theta) / theta;
return Quat( s*q.vector(), c);
}
else
return Quat(q.vector(), c);
}
// Based on code from the Appendix of
// Quaternion Calculus for Computer Graphics, by Ken Shoemake
// Computes the natural logarithm of a UNIT quaternion
Quat log(const Quat& q)
{
double scale = norm(q.vector());
double theta = atan2(scale, q.scalar());
if( scale > 0.0 ) scale=theta/scale;
return Quat(scale*q.vector(), 0.0);
}
Quat axis_to_quat(const Vec3& a, double phi)
{
Vec3 u = a;
unitize(u);
double s = sin(phi/2.0);
return Quat(u[0]*s, u[1]*s, u[2]*s, cos(phi/2.0));
}
Mat4 quat_to_matrix(const Quat& q)
{
Mat4 M;
const double x = q.vector()[0];
const double y = q.vector()[1];
const double z = q.vector()[2];
const double w = q.scalar();
const double s = 2/norm(q);
M(0,0)=1-s*(y*y+z*z); M(0,1)=s*(x*y-w*z); M(0,2)=s*(x*z+w*y); M(0,3)=0;
M(1,0)=s*(x*y+w*z); M(1,1)=1-s*(x*x+z*z); M(1,2)=s*(y*z-w*x); M(1,3)=0;
M(2,0)=s*(x*z-w*y); M(2,1)=s*(y*z+w*x); M(2,2)=1-s*(x*x+y*y); M(2,3)=0;
M(3,0)=0; M(3,1)=0; M(3,2)=0; M(3,3)=1;
return M;
}
Mat4 unit_quat_to_matrix(const Quat& q)
{
Mat4 M;
const double x = q.vector()[0];
const double y = q.vector()[1];
const double z = q.vector()[2];
const double w = q.scalar();
M(0,0)=1-2*(y*y+z*z); M(0,1)=2*(x*y-w*z); M(0,2)=2*(x*z+w*y); M(0,3)=0;
M(1,0)=2*(x*y+w*z); M(1,1)=1-2*(x*x+z*z); M(1,2)=2*(y*z-w*x); M(1,3)=0;
M(2,0)=2*(x*z-w*y); M(2,1)=2*(y*z+w*x); M(2,2)=1-2*(x*x+y*y); M(2,3)=0;
M(3,0)=0; M(3,1)=0; M(3,2)=0; M(3,3)=1;
return M;
}
Quat slerp(const Quat& from, const Quat& to, double t)
{
const Vec3& v_from = from.vector();
const Vec3& v_to = to.vector();
const double s_from = from.scalar();
const double s_to = to.scalar();
double cosine = v_from*v_to + s_from*s_to;
if( (1+cosine) < FEQ_EPS )
{
// The quaternions are (nearly) diametrically opposed. We
// treat this specially (based on suggestion in Watt & Watt).
//
double A = sin( (1-t)*M_PI/2.0 );
double B = sin( t*M_PI/2.0 );
return Quat( A*v_from[0] + B*(-v_from[1]),
A*v_from[1] + B*(v_from[0]),
A*v_from[2] + B*(-s_from),
A*s_from + B*(v_from[2]) );
}
double A, B;
if( (1-cosine) < FEQ_EPS )
{
// The quaternions are very close. Approximate with normal
// linear interpolation. This is cheaper and avoids division
// by very small numbers.
//
A = 1.0 - t;
B = t;
}
else
{
// This is the normal case. Perform SLERP.
//
double theta = acos(cosine);
double sine = sqrt(1 - cosine*cosine);
A = sin( (1-t)*theta ) / sine;
B = sin( t*theta ) / sine;
}
return Quat( A*v_from + B*v_to, A*s_from + B*s_to);
}
} // namespace gfx
|
