| 2 |
bj |
1 |
#ifndef __CGLA_QUATERNION_H__
|
|
|
2 |
#define __CGLA_QUATERNION_H__
|
|
|
3 |
|
| 10 |
jab |
4 |
#include "Vec3f.h"
|
|
|
5 |
#include "Vec4f.h"
|
|
|
6 |
#include "Mat3x3f.h"
|
|
|
7 |
#include "Mat4x4f.h"
|
| 2 |
bj |
8 |
|
|
|
9 |
namespace CGLA {
|
|
|
10 |
|
| 89 |
jab |
11 |
/** \brief A Quaterinion class.
|
| 2 |
bj |
12 |
|
| 89 |
jab |
13 |
Quaternions are algebraic entities useful for rotation. */
|
|
|
14 |
|
| 2 |
bj |
15 |
class Quaternion
|
|
|
16 |
{
|
|
|
17 |
public:
|
|
|
18 |
|
|
|
19 |
/// Vector part of quaternion
|
|
|
20 |
Vec3f qv;
|
|
|
21 |
|
|
|
22 |
/// Scalar part of quaternion
|
|
|
23 |
float qw;
|
|
|
24 |
|
|
|
25 |
/// Construct 0 quaternion
|
| 10 |
jab |
26 |
Quaternion(): qw(CGLA_INIT_VALUE) {}
|
| 2 |
bj |
27 |
|
|
|
28 |
/// Construct quaternion from vector and scalar
|
|
|
29 |
Quaternion(const Vec3f _qv, float _qw=1) :
|
|
|
30 |
qv(_qv) , qw(_qw) {}
|
|
|
31 |
|
|
|
32 |
/// Construct quaternion from four scalars
|
|
|
33 |
Quaternion(float x, float y, float z, float _qw) :
|
|
|
34 |
qv(x,y,z), qw(_qw) {}
|
|
|
35 |
|
|
|
36 |
/// Assign values to a quaternion
|
|
|
37 |
void set(float x, float y, float z, float _qw)
|
|
|
38 |
{
|
|
|
39 |
qv.set(x,y,z);
|
|
|
40 |
qw = _qw;
|
|
|
41 |
}
|
|
|
42 |
|
|
|
43 |
/// Get values from a quaternion
|
|
|
44 |
void get(float& x, float& y, float& z, float& _qw) const
|
|
|
45 |
{
|
|
|
46 |
x = qv[0];
|
|
|
47 |
y = qv[1];
|
|
|
48 |
z = qv[2];
|
|
|
49 |
_qw = qw;
|
|
|
50 |
}
|
|
|
51 |
|
|
|
52 |
/// Get a 3x3 rotation matrix from a quaternion
|
|
|
53 |
Mat3x3f get_mat3x3f() const;
|
|
|
54 |
|
|
|
55 |
/// Get a 4x4 rotation matrix from a quaternion
|
|
|
56 |
Mat4x4f get_mat4x4f() const;
|
|
|
57 |
|
|
|
58 |
/// Construct a Quaternion from an angle and axis of rotation.
|
|
|
59 |
void make_rot(float angle, const Vec3f&);
|
|
|
60 |
|
|
|
61 |
/** Construct a Quaternion rotating from the direction given
|
|
|
62 |
by the first argument to the direction given by the second.*/
|
|
|
63 |
void make_rot(const Vec3f&,const Vec3f&);
|
|
|
64 |
|
|
|
65 |
/// Obtain angle of rotation and axis
|
|
|
66 |
void get_rot(float& angle, Vec3f&);
|
|
|
67 |
|
|
|
68 |
/// Multiply two quaternions. (Combine their rotation)
|
|
|
69 |
Quaternion operator *(Quaternion quat) const;
|
|
|
70 |
|
|
|
71 |
/// Multiply scalar onto quaternion.
|
|
|
72 |
Quaternion operator *(float scalar) const;
|
|
|
73 |
|
|
|
74 |
/// Add two quaternions.
|
|
|
75 |
Quaternion operator +(Quaternion quat) const;
|
|
|
76 |
|
|
|
77 |
/// Invert quaternion
|
|
|
78 |
Quaternion inverse() const;
|
|
|
79 |
|
|
|
80 |
/// Return conjugate quaternion
|
|
|
81 |
Quaternion conjugate() const;
|
|
|
82 |
|
|
|
83 |
/// Compute norm of quaternion
|
|
|
84 |
float norm() const;
|
|
|
85 |
|
|
|
86 |
/// Normalize quaternion.
|
|
|
87 |
Quaternion normalize();
|
|
|
88 |
|
|
|
89 |
/// Rotate vector according to quaternion
|
|
|
90 |
Vec3f apply(const Vec3f& vec) const;
|
|
|
91 |
};
|
|
|
92 |
|
|
|
93 |
/// Compare for equality.
|
|
|
94 |
inline bool operator==(const Quaternion& q0, const Quaternion& q1)
|
|
|
95 |
{
|
|
|
96 |
return (q0.qw == q1.qw && q0.qv == q1.qv);
|
|
|
97 |
}
|
|
|
98 |
|
|
|
99 |
/// Print quaternion to stream.
|
|
|
100 |
inline std::ostream& operator<<(std::ostream&os, const Quaternion v)
|
|
|
101 |
{
|
|
|
102 |
os << "[ ";
|
| 45 |
jab |
103 |
for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
|
| 2 |
bj |
104 |
os << " ~ " << v.qw << " ";
|
|
|
105 |
os << "]";
|
|
|
106 |
|
|
|
107 |
return os;
|
|
|
108 |
}
|
|
|
109 |
|
|
|
110 |
inline Quaternion Quaternion::operator *(Quaternion quat) const
|
|
|
111 |
{
|
|
|
112 |
return Quaternion(cross(qv,quat.qv) + quat.qw*qv + qw*quat.qv,
|
|
|
113 |
qw*quat.qw - dot(qv,quat.qv));
|
|
|
114 |
}
|
|
|
115 |
|
|
|
116 |
inline Quaternion Quaternion::operator *(float scalar) const
|
|
|
117 |
{
|
|
|
118 |
return Quaternion(scalar*qv,scalar*qw);
|
|
|
119 |
}
|
|
|
120 |
|
|
|
121 |
/// Multiply scalar onto quaternion
|
|
|
122 |
inline Quaternion operator *(float scalar, Quaternion quat)
|
|
|
123 |
{
|
|
|
124 |
return Quaternion(scalar*quat.qv,scalar*quat.qw);
|
|
|
125 |
}
|
|
|
126 |
|
|
|
127 |
|
|
|
128 |
inline Quaternion Quaternion::operator +(Quaternion quat) const
|
|
|
129 |
{
|
|
|
130 |
return Quaternion(qv + quat.qv,qw + quat.qw);
|
|
|
131 |
}
|
|
|
132 |
|
|
|
133 |
inline float Quaternion::norm() const
|
|
|
134 |
{
|
|
|
135 |
return qv[0]*qv[0] + qv[1]*qv[1] + qv[2]*qv[2] + qw*qw;
|
|
|
136 |
}
|
|
|
137 |
|
|
|
138 |
inline Quaternion Quaternion::normalize()
|
|
|
139 |
{
|
| 5 |
jab |
140 |
return Quaternion(1.0/norm()*(*this));
|
| 2 |
bj |
141 |
}
|
|
|
142 |
|
|
|
143 |
inline Quaternion Quaternion::conjugate() const
|
|
|
144 |
{
|
|
|
145 |
return Quaternion(-qv,qw);
|
|
|
146 |
}
|
|
|
147 |
|
|
|
148 |
inline Quaternion Quaternion::inverse() const
|
|
|
149 |
{
|
| 5 |
jab |
150 |
return Quaternion(1.0/norm()*conjugate());
|
| 2 |
bj |
151 |
}
|
|
|
152 |
|
|
|
153 |
|
|
|
154 |
/** Perform linear interpolation of two quaternions.
|
|
|
155 |
The last argument is the parameter used to interpolate
|
|
|
156 |
between the two first. SLERP - invented by Shoemake -
|
|
|
157 |
is a good way to interpolate because the interpolation
|
|
|
158 |
is performed on the unit sphere.
|
|
|
159 |
*/
|
|
|
160 |
inline Quaternion slerp(Quaternion q0, Quaternion q1, float t)
|
|
|
161 |
{
|
|
|
162 |
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);
|
|
|
163 |
return (q0*sin((1-t)*angle)+q1*sin(t*angle))*(1/sin(angle));
|
|
|
164 |
}
|
|
|
165 |
|
|
|
166 |
}
|
|
|
167 |
#endif
|