2
3D - C++ Class Library for 3D Transformations
3
Copyright (C) 1996-1998 Gino van den Bergen
5
This library is free software; you can redistribute it and/or
6
modify it under the terms of the GNU Library General Public
7
License as published by the Free Software Foundation; either
8
version 2 of the License, or (at your option) any later version.
10
This library is distributed in the hope that it will be useful,
11
but WITHOUT ANY WARRANTY; without even the implied warranty of
12
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13
Library General Public License for more details.
15
You should have received a copy of the GNU Library General Public
16
License along with this library; if not, write to the Free
17
Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19
Please send remarks, questions and bug reports to gino@win.tue.nl,
22
Department of Mathematics and Computing Science
23
Eindhoven University of Technology
24
P.O. Box 513, 5600 MB Eindhoven, The Netherlands
27
#ifndef _QUATERNION_H_
28
#define _QUATERNION_H_
33
class Quaternion : public Tuple4 {
36
Quaternion(const float v[4]) : Tuple4(v) {}
37
Quaternion(const double v[4]) : Tuple4(v) {}
38
Quaternion(Scalar x, Scalar y, Scalar z, Scalar w) : Tuple4(x, y, z, w) {}
39
Quaternion(const Vector& axis, Scalar angle) { setRotation(axis, angle); }
40
Quaternion(Scalar yaw, Scalar pitch, Scalar roll) {
41
setEuler(yaw, pitch, roll);
44
void setRotation(const Vector& axis, Scalar angle) {
45
Scalar d = axis.length();
47
Scalar s = sin(angle / 2) / d;
48
setValue(axis[X] * s, axis[Y] * s, axis[Z] * s, cos(angle / 2));
51
void setEuler(Scalar yaw, Scalar pitch, Scalar roll) {
52
Scalar cosYaw = cos(yaw / 2);
53
Scalar sinYaw = sin(yaw / 2);
54
Scalar cosPitch = cos(pitch / 2);
55
Scalar sinPitch = sin(pitch / 2);
56
Scalar cosRoll = cos(roll / 2);
57
Scalar sinRoll = sin(roll / 2);
58
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
59
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
60
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
61
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
64
Quaternion& operator+=(const Quaternion& q);
65
Quaternion& operator-=(const Quaternion& q);
66
Quaternion& operator*=(const Quaternion& q);
67
Quaternion& operator*=(Scalar s);
68
Quaternion& operator/=(Scalar s);
70
Scalar length2() const;
71
Scalar length() const;
73
bool approxZero() const;
76
Quaternion normalized() const;
79
Quaternion conjugate() const;
82
Quaternion inverse() const;
84
static Quaternion random();
87
Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
88
Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
89
Quaternion operator-(const Quaternion& q);
90
Quaternion operator*(const Quaternion& q1, const Quaternion& q2);
91
Quaternion operator*(const Quaternion& q, Scalar s);
92
Quaternion operator/(const Quaternion& q, Scalar s);
94
Scalar dot(const Quaternion& q1, const Quaternion& q2);
96
Scalar length2(const Quaternion& q);
97
Scalar length(const Quaternion& q);
99
bool approxZero(const Quaternion& q);
100
bool approxEqual(const Quaternion& q1, const Quaternion& q2);
104
inline Quaternion& Quaternion::operator+=(const Quaternion& q) {
105
comp[X] += q[X]; comp[Y] += q[Y]; comp[Z] += q[Z]; comp[W] += q[W];
109
inline Quaternion& Quaternion::operator-=(const Quaternion& q) {
110
comp[X] -= q[X]; comp[Y] -= q[Y]; comp[Z] -= q[Z]; comp[W] -= q[W];
114
inline Quaternion& Quaternion::operator*=(const Quaternion& q) {
115
setValue(comp[W] * q[X] + comp[X] * q[W] + comp[Y] * q[Z] - comp[Z] * q[Y],
116
comp[W] * q[Y] + comp[Y] * q[W] + comp[Z] * q[X] - comp[X] * q[Z],
117
comp[W] * q[Z] + comp[Z] * q[W] + comp[X] * q[Y] - comp[Y] * q[X],
118
comp[W] * q[W] - comp[X] * q[X] - comp[Y] * q[Y] - comp[Z] * q[Z]);
122
inline Quaternion& Quaternion::operator*=(Scalar s) {
123
comp[X] *= s; comp[Y] *= s; comp[Z] *= s; comp[W] *= s;
127
inline Quaternion& Quaternion::operator/=(Scalar s) {
129
return *this *= 1 / s;
132
inline Quaternion operator+(const Quaternion& q1, const Quaternion& q2) {
133
return Quaternion(q1[X] + q2[X], q1[Y] + q2[Y], q1[Z] + q2[Z], q1[W] + q2[W]);
136
inline Quaternion operator-(const Quaternion& q1, const Quaternion& q2) {
137
return Quaternion(q1[X] - q2[X], q1[Y] - q2[Y], q1[Z] - q2[Z], q1[W] - q2[W]);
140
inline Quaternion operator-(const Quaternion& q) {
141
return Quaternion(-q[X], -q[Y], -q[Z], -q[W]);
144
inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
146
Quaternion(q1[W] * q2[X] + q1[X] * q2[W] + q1[Y] * q2[Z] - q1[Z] * q2[Y],
147
q1[W] * q2[Y] + q1[Y] * q2[W] + q1[Z] * q2[X] - q1[X] * q2[Z],
148
q1[W] * q2[Z] + q1[Z] * q2[W] + q1[X] * q2[Y] - q1[Y] * q2[X],
149
q1[W] * q2[W] - q1[X] * q2[X] - q1[Y] * q2[Y] - q1[Z] * q2[Z]);
152
inline Quaternion operator*(const Quaternion& q, Scalar s) {
153
return Quaternion(q[X] * s, q[Y] * s, q[Z] * s, q[W] * s);
156
inline Quaternion operator*(Scalar s, const Quaternion& q) { return q * s; }
158
inline Quaternion operator/(const Quaternion& q, Scalar s) {
163
inline Scalar dot(const Quaternion& q1, const Quaternion& q2) {
164
return q1[X] * q2[X] + q1[Y] * q2[Y] + q1[Z] * q2[Z] + q1[W] * q2[W];
167
inline Scalar Quaternion::length2() const { return dot(*this, *this); }
168
inline Scalar Quaternion::length() const { return sqrt(length2()); }
170
inline bool Quaternion::approxZero() const { return length2() < EPSILON2; }
172
inline void Quaternion::normalize() { *this /= length(); }
173
inline Quaternion Quaternion::normalized() const { return *this / length(); }
175
inline void Quaternion::conjugate() {
176
comp[X] = -comp[X]; comp[Y] = -comp[Y]; comp[Z] = -comp[Z];
179
inline Quaternion Quaternion::conjugate() const {
180
return Quaternion(-comp[X], -comp[Y], -comp[Z], comp[W]);
183
inline void Quaternion::invert() {
188
inline Quaternion Quaternion::inverse() const {
189
return conjugate() / length2();
192
inline Scalar length2(const Quaternion& q) { return q.length2(); }
193
inline Scalar length(const Quaternion& q) { return q.length(); }
195
inline bool approxZero(const Quaternion& q) { return q.approxZero(); }
196
inline bool approxEqual(const Quaternion& q1, const Quaternion& q2) {
197
return approxZero(q1 - q2);
200
// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
202
inline Quaternion Quaternion::random() {
204
Scalar r1 = sqrt(1 - x0), r2 = sqrt(x0);
205
Scalar t1 = TWO_PI * rnd(), t2 = TWO_PI * rnd();
206
Scalar c1 = cos(t1), s1 = sin(t1);
207
Scalar c2 = cos(t2), s2 = sin(t2);
208
return Quaternion(s1 * r1, c1 * r1, s2 * r2, c2 * r2);