wfmath 1.0.3
A math library for the Worldforge system.
quaternion.cpp
1// quaternion.cpp (Quaternion implementation)
2//
3// The WorldForge Project
4// Copyright (C) 2002 The WorldForge Project
5//
6// This program is free software; you can redistribute it and/or modify
7// it under the terms of the GNU General Public License as published by
8// the Free Software Foundation; either version 2 of the License, or
9// (at your option) any later version.
10//
11// This program is distributed in the hope that it will be useful,
12// but WITHOUT ANY WARRANTY; without even the implied warranty of
13// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14// GNU General Public License for more details.
15//
16// You should have received a copy of the GNU General Public License
17// along with this program; if not, write to the Free Software
18// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
19//
20// For information about WorldForge and its authors, please contact
21// the Worldforge Web Site at http://www.worldforge.org.
22//
23
24// Author: Ron Steinke
25
26// Some code here was taken from the quaternion implementation is
27// eris. Some of the other algorithms are based on information
28// found here <http://www.cs.ualberta.ca/~andreas/math/matrfaq_latest.html>
29// and here <http://www.cs.berkeley.edu/~laura/cs184/quat/quaternion.html>.
30
31#ifdef HAVE_CONFIG_H
32#include "config.h"
33#endif
34
35#include "quaternion.h"
36#include "error.h"
37#include "rotmatrix.h"
38
39#include <cmath>
40
41#include <cassert>
42
43namespace WFMath {
44
45static_assert(std::is_standard_layout<Quaternion>::value, "Quaternion should be standard layout.");
46static_assert(std::is_trivially_copyable<Quaternion>::value, "Quaternion should be trivially copyable.");
47
48
50 CoordType x_in,
51 CoordType y_in,
52 CoordType z_in)
53 : m_w(0), m_vec(), m_valid(true), m_age(1)
54{
55 CoordType norm = std::sqrt(w_in*w_in + x_in*x_in + y_in*y_in + z_in*z_in);
56
57 m_w = w_in / norm;
58 m_vec[0] = x_in / norm;
59 m_vec[1] = y_in / norm;
60 m_vec[2] = z_in / norm;
61 m_vec.setValid();
62}
63
65{
66 static Quaternion ident = Quaternion(Identity());
67 return ident;
68}
69
70
71// The equality functions regard q and -q as equal, since they
72// correspond to the same rotation matrix. We consider the form
73// of the quaternion with w > 0 canonical.
74
75bool Quaternion::isEqualTo(const Quaternion &q, CoordType epsilon) const
76{
77 // Since the sum of squares is 1, the magnitude of the largest
78 // element must be between 1 and 0.5, so we don't need to scale epsilon.
79
80 assert(epsilon > 0);
81
82 //If anyone is invalid they are never equal
83 if (!q.m_valid || !m_valid) {
84 return false;
85 }
86
87 if(std::fabs(m_w - q.m_w) <= epsilon) {
88 int i;
89 for(i = 0; i < 3; ++i)
90 if(std::fabs(m_vec[i] - q.m_vec[i]) > epsilon)
91 break; // try again with swapped signs
92 if(i == 3) // got through the loop
93 return true;
94 }
95
96 // This makes q == -q true
97 if(std::fabs(m_w + q.m_w) <= epsilon) {
98 for(int i = 0; i < 3; ++i)
99 if(std::fabs(m_vec[i] + q.m_vec[i]) > epsilon)
100 return false;
101 return true;
102 }
103
104 return false;
105}
106
107// order of multiplication vs sense of rotation
108//
109// v.rotate(q1).rotate(q2) is the same as v.rotate(q1 * q2),
110// the same as with matrices
111
112Quaternion& Quaternion::operator*= (const Quaternion& rhs)
113{
114 m_valid = m_valid && rhs.m_valid;
115 m_age = m_age + rhs.m_age;
116 checkNormalization();
117
118 CoordType old_w = m_w;
119 m_w = m_w * rhs.m_w - Dot(m_vec, rhs.m_vec);
120 m_vec = old_w * rhs.m_vec + rhs.m_w * m_vec - Cross(m_vec, rhs.m_vec);
121
122 return *this;
123}
124
125Quaternion& Quaternion::operator/= (const Quaternion& rhs)
126{
127 m_valid = m_valid && rhs.m_valid;
128 m_age = m_age + rhs.m_age;
129 checkNormalization();
130
131 CoordType old_w = m_w;
132 m_w = m_w * rhs.m_w + Dot(m_vec, rhs.m_vec);
133 m_vec = rhs.m_w * m_vec - old_w * rhs.m_vec + Cross(m_vec, rhs.m_vec);
134
135 return *this;
136}
137
139{
140 RotMatrix<3> m_tmp;
141 bool not_flip = !m.parity();
142
143 m_valid = m.isValid();
144 m_vec.setValid(m.isValid());
145
146 if(!not_flip)
147 m_tmp = Prod(m, RotMatrix<3>().mirrorX());
148
149 const RotMatrix<3> &m_ref = not_flip ? m : m_tmp;
150
151 CoordType s;
152 const int nxt[3] = {1, 2, 0};
153 CoordType tr = m_ref.trace();
154
155 // check the diagonal
156 if (tr > 0.0) {
157 s = std::sqrt(tr + 1.0f);
158 m_w = (s / 2.0f);
159 s = (0.5f / s);
160
161 m_vec[0] = (m_ref.elem(2, 1) - m_ref.elem(1, 2)) * s;
162 m_vec[1] = (m_ref.elem(0, 2) - m_ref.elem(2, 0)) * s;
163 m_vec[2] = (m_ref.elem(1, 0) - m_ref.elem(0, 1)) * s;
164 } else {
165 // diagonal is negative
166 int i = 0;
167
168 if (m_ref.elem(1, 1) > m_ref.elem(0, 0)) i = 1;
169 if (m_ref.elem(2, 2) > m_ref.elem(i, i)) i = 2;
170
171 int j = nxt[i], k = nxt[j];
172
173 s = std::sqrt (1.0f + m_ref.elem(i, i) - m_ref.elem(j, j) - m_ref.elem(k, k));
174 m_vec[i] = -(s * 0.5f);
175
176 assert("sqrt() returns positive" && s > 0.0);
177 s = (0.5f / s);
178
179 m_w = (m_ref.elem(k, j) - m_ref.elem(j, k)) * s;
180 m_vec[j] = (m_ref.elem(i, j) + m_ref.elem(j, i)) * s;
181 m_vec[k] = (m_ref.elem(i, k) + m_ref.elem(k, i)) * s;
182 }
183
184 m_age = m.age();
185
186 return not_flip;
187}
188
190{
191 Quaternion q(m_valid);
192 q.m_w = m_w;
193 q.m_vec = -m_vec;
194 q.m_age = m_age; // no multiplication was done, so roundoff error does not increase
195 return q;
196}
197
199{
200 // FIXME find a more efficient way to do this
201 Quaternion tmp;
202 tmp.fromRotMatrix(m);
203 *this *= tmp;
204 return *this;
205}
206
208{
209 if (axis < 0 || axis > 2) {
210 m_valid = false;
211 return *this;
212 }
213
214 CoordType half_angle = angle / 2;
215
216 m_w = std::cos(half_angle);
217 for(int i = 0; i < 3; ++i)
218 // Note sin() only called once
219 m_vec[i] = (i == axis) ? std::sin(half_angle) : 0;
220 m_vec.setValid();
221
222 m_valid = true;
223 m_age = 1;
224
225 return *this;
226}
227
229{
230 CoordType axis_mag = axis.mag();
231 CoordType half_angle = angle / 2;
232
233 if (axis_mag < numeric_constants<CoordType>::epsilon()) {
234 m_valid = false;
235 return *this;
236 }
237
238 m_w = std::cos(half_angle);
239 m_vec = axis * (std::sin(half_angle) / axis_mag);
240
241 m_valid = axis.isValid();
242 m_age = 1;
243
244 return *this;
245}
246
248{
249 CoordType axis_mag = axis.mag();
250 CoordType half_angle = axis_mag / 2;
251
252 if (axis_mag < numeric_constants<CoordType>::epsilon()) {
253 m_valid = false;
254 return *this;
255 }
256
257 m_w = std::cos(half_angle);
258 m_vec = axis * (std::sin(half_angle) / axis_mag);
259
260 m_valid = axis.isValid();
261 m_age = 1;
262
263 return *this;
264}
265
267{
268 CoordType mag_prod = std::sqrt(from.sqrMag() * to.sqrMag());
269 CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;
270
271 if (mag_prod < numeric_constants<CoordType>::epsilon()) {
272 m_valid = false;
273 return *this;
274 }
275
276 // antiparallel vectors
277 if(ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) // same check as used in the RotMatrix function
278 throw ColinearVectors<3>(from, to);
279
280 // cosine of half the angle
281 m_w = std::sqrt(ctheta_plus_1 / 2.f);
282
283 // vector in direction of axis, magnitude of cross product is proportional to
284 // the sin of the angle, divide to make the magnitude the sin of half the angle,
285 // sin(x) = 2sin(x/2)cos(x/2), so sin(x/2) = sin(x)/(2cos(x/2))
286 m_vec = Cross(from, to) / (2 * mag_prod * m_w);
287
288 m_valid = from.isValid() && to.isValid();
289 m_age = 1;
290
291 return *this;
292}
293
294Quaternion& Quaternion::rotation(const Vector<3>& from, const Vector<3>& to, const Vector<3>& fallbackAxis)
295{
296 CoordType mag_prod = std::sqrt(from.sqrMag() * to.sqrMag());
297 CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;
298
299 if (mag_prod < numeric_constants<CoordType>::epsilon()) {
300 m_valid = false;
301 return *this;
302 }
303
304 // antiparallel vectors
305 if(ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) { // same check as used in the RotMatrix function
307 } else {
308 // cosine of half the angle
309 m_w = std::sqrt(ctheta_plus_1 / 2.f);
310
311 // vector in direction of axis, magnitude of cross product is proportional to
312 // the sin of the angle, divide to make the magnitude the sin of half the angle,
313 // sin(x) = 2sin(x/2)cos(x/2), so sin(x/2) = sin(x)/(2cos(x/2))
314 m_vec = Cross(from, to) / (2 * mag_prod * m_w);
315 m_valid = from.isValid() && to.isValid();
316 m_age = 1;
317 }
318
319 return *this;
320}
321
323{
324 // Assume that we're not too far off, and compute the norm
325 // only to linear order in the difference from 1.
326 // If q.sqrMag() = 1 + x, q.mag() = 1 + x/2 = (q.SqrMag() + 1)/2
327 // to linear order.
328 CoordType norm = (m_w * m_w + m_vec.sqrMag() + 1)/2;
329 m_w /= norm;
330 m_vec /= norm;
331 m_age = 1;
332}
333
334}
A normalized quaternion.
Definition: quaternion.h:36
Quaternion inverse() const
returns the inverse of the Quaternion
Definition: quaternion.cpp:189
void normalize()
normalize to remove accumulated round-off error
Definition: quaternion.cpp:322
bool fromRotMatrix(const RotMatrix< 3 > &m)
set a Quaternion's value from a RotMatrix
Definition: quaternion.cpp:138
Quaternion & rotate(const RotMatrix< 3 > &)
Rotate quaternion using the matrix.
Definition: quaternion.cpp:198
static const Quaternion & IDENTITY()
Definition: quaternion.cpp:64
Quaternion & rotation(int axis, CoordType angle)
sets the Quaternion to a rotation by angle around axis
Definition: quaternion.cpp:207
Quaternion()
Construct a Quaternion.
Definition: quaternion.h:51
bool parity() const
Get the parity of the matrix.
Definition: rotmatrix.h:154
CoordType trace() const
Get the trace of the matrix.
CoordType elem(const int i, const int j) const
get the (i, j) element of the matrix
Definition: rotmatrix.h:112
unsigned age() const
current round-off age
Definition: rotmatrix.h:201
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
Definition: vector.h:154
CoordType mag() const
The magnitude of a vector.
Definition: vector.h:217
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:220
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
Definition: vector.cpp:102
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
An error thrown by certain functions when passed parallel vectors.
Definition: error.h:37