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 
43 namespace WFMath {
44 
45 static_assert(std::is_standard_layout<Quaternion>::value, "Quaternion should be standard layout.");
46 static_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 
75 bool 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 
112 Quaternion& 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 
125 Quaternion& 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 
294 Quaternion& 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 }
WFMath::Cross
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
WFMath::Quaternion::Identity
Definition: quaternion.h:46
WFMath::Quaternion::rotation
Quaternion & rotation(int axis, CoordType angle)
sets the Quaternion to a rotation by angle around axis
Definition: quaternion.cpp:207
WFMath::Quaternion::rotate
Quaternion & rotate(const RotMatrix< 3 > &)
Rotate quaternion using the matrix.
Definition: quaternion.cpp:198
WFMath::RotMatrix::elem
CoordType elem(const int i, const int j) const
get the (i, j) element of the matrix
Definition: rotmatrix.h:112
WFMath::Vector::setValid
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
Definition: vector.h:154
WFMath::Quaternion::Quaternion
Quaternion()
Construct a Quaternion.
Definition: quaternion.h:51
WFMath::Quaternion::inverse
Quaternion inverse() const
returns the inverse of the Quaternion
Definition: quaternion.cpp:189
WFMath::RotMatrix< 3 >
WFMath::Quaternion::fromRotMatrix
bool fromRotMatrix(const RotMatrix< 3 > &m)
set a Quaternion's value from a RotMatrix
Definition: quaternion.cpp:138
WFMath::ColinearVectors
An error thrown by certain functions when passed parallel vectors.
Definition: error.h:37
WFMath::Vector::sqrMag
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:220
WFMath::Quaternion
A normalized quaternion.
Definition: quaternion.h:36
WFMath::Prod
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition: rotmatrix_funcs.h:72
WFMath::RotMatrix::age
unsigned age() const
current round-off age
Definition: rotmatrix.h:201
WFMath
Generic library namespace.
Definition: shape.h:41
WFMath::Quaternion::IDENTITY
static const Quaternion & IDENTITY()
Definition: quaternion.cpp:64
WFMath::Vector< 3 >
WFMath::Quaternion::normalize
void normalize()
normalize to remove accumulated round-off error
Definition: quaternion.cpp:322
WFMath::CoordType
double CoordType
Basic floating point type.
Definition: const.h:140
WFMath::numeric_constants
Definition: const.h:65
WFMath::RotMatrix::trace
CoordType trace() const
Get the trace of the matrix.
Definition: rotmatrix_funcs.h:332
WFMath::Vector::mag
CoordType mag() const
The magnitude of a vector.
Definition: vector.h:217
WFMath::RotMatrix::parity
bool parity() const
Get the parity of the matrix.
Definition: rotmatrix.h:154