mt
 All Classes Files Functions Enumerations Groups Pages
quaternion.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Adolfo Rodriguez *
3  * adolfo.rodriguez@upc.edu *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program 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 *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
22 
23 // HEADER GUARD
24 #ifndef MT_QUATERNION_H
25 #define MT_QUATERNION_H
26 
27 // C++ STANDARD HEADERS
28 #include <iostream>
29 #include <stdexcept>
30 
31 // PROJECT HEADERS
32 #include <mt/util/assert/assert_template.h>
33 
34 // MT LIBRARY HEADERS
35 #include <mt/scalar.h>
36 #include <mt/vector3.h>
37 
38 
40 
41 namespace mt
42 {
43 
44 
46 
49 
58 
60 {
61 public:
62 
63 // LIFECYCLE
64 
66  Quaternion();
67 
69  Quaternion(const Scalar& x,
70  const Scalar& y,
71  const Scalar& z,
72  const Scalar& w);
73 
75  explicit Quaternion(const Scalar* q);
76 
77  // Compiler generated copy constructor is being used
78 
79  virtual ~Quaternion(){}
80 
81 
82 // OPERATORS
83 
84  // Compiler generated assignment operator is being used
85 
87  Scalar& operator[](size_t n);
88 
90  const Scalar& operator[](size_t n) const;
91 
92  Quaternion& operator+=(const Quaternion& q);
93  Quaternion& operator-=(const Quaternion& q);
94 
96  Quaternion& operator*=(const Quaternion& q);
97 
99  Quaternion& operator*=(const Scalar& s);
100 
102  Quaternion& operator/=(const Scalar& s);
103 
105  Vector3 operator()(const Vector3& v) const;
106 
109  bool operator==(const Quaternion& q) const;
110 
111  bool operator!=(const Quaternion& q) const;
112 
113 // OPERATIONS
114 
116  Scalar dot(const Quaternion& q) const;
117 
119  Scalar length2() const;
120 
122  Scalar length() const;
123 
126 
128  Scalar distance2(const Quaternion& q) const;
129 
131  Scalar distance(const Quaternion& q) const;
132 
134  Scalar angleCos(const Quaternion& q) const;
135 
137  Scalar angle(const Quaternion& q) const;
138 
140  Quaternion conjugate() const;
141 
143  Quaternion inverse() const;
144 
146  Quaternion slerp(const Quaternion& q,
147  const Scalar& t) const;
148 
149 // ACCESS
150 
152  Scalar& at(size_t n);
153 
155  const Scalar& at(size_t n) const;
156 
158  void setValue(const Scalar& x,
159  const Scalar& y,
160  const Scalar& z,
161  const Scalar& w);
162 
164  void setValue(const Scalar* q);
165 
166 
167 protected:
168 
169 // MEMBERS
170 
171 Scalar m_co[4];
172 
173 };
174 
175 
177 
178 // OPERATORS
179 
180 Quaternion operator+(const Quaternion& q);
181 
182 Quaternion operator-(const Quaternion& q);
183 
184 Quaternion operator+(const Quaternion& q1,
185  const Quaternion& q2);
186 
187 Quaternion operator-(const Quaternion& q1,
188  const Quaternion& q2);
189 
190 Quaternion operator*(const Quaternion& q1,
191  const Quaternion& q2);
192 
193 Quaternion operator*(const Quaternion& q,
194  const Vector3& v);
195 
196 Quaternion operator*(const Vector3& v,
197  const Quaternion& q);
198 
199 Quaternion operator*(const Scalar& s,
200  const Quaternion& q);
201 
202 Quaternion operator*(const Quaternion& q,
203  const Scalar& s);
204 
205 Quaternion operator/(const Quaternion& q,
206  const Scalar& s);
207 
208 std::ostream& operator<<(std::ostream& os,
209  const Quaternion& q);
210 
211 
212 // FUNCTIONS
213 
215 Scalar length2(const Quaternion& q);
216 
218 Scalar length(const Quaternion& q);
219 
221 Quaternion normalize(const Quaternion& q);
222 
224 Scalar angleCos(const Quaternion& q1,
225  const Quaternion& q2);
226 
228 Scalar angle(const Quaternion& q1,
229  const Quaternion& q2);
230 
232 Scalar dot(const Quaternion& q1,
233  const Quaternion& q2);
234 
236 Quaternion conjugate(const Quaternion& q);
237 
239 Quaternion inverse(const Quaternion& q);
240 
242 Quaternion slerp(const Quaternion& q1,
243  const Quaternion& q2,
244  const Scalar& t);
245 
246 
248 
249 // LIFECYCLE
250 
252 {
253  setValue(0.0, 0.0, 0.0, 1.0);
254 }
255 
256 
257 inline Quaternion::Quaternion(const Scalar& x,
258  const Scalar& y,
259  const Scalar& z,
260  const Scalar& w)
261 {
262  setValue(x, y, z, w);
263 }
264 
265 
266 inline Quaternion::Quaternion(const Scalar* q)
267 {
268  setValue(q);
269 }
270 
271 
272 // OPERATORS
273 
274 inline Scalar& Quaternion::operator[](size_t n)
275 {
276  return m_co[n];
277 }
278 
279 
280 inline const Scalar& Quaternion::operator[](size_t n) const
281 {
282  return m_co[n];
283 }
284 
285 
286 inline Quaternion& Quaternion::operator+=(const Quaternion& q)
287 {
288  m_co[0] += q[0];
289  m_co[1] += q[1];
290  m_co[2] += q[2];
291  m_co[3] += q[3];
292  return *this;
293 }
294 
295 
296 inline Quaternion& Quaternion::operator-=(const Quaternion& q)
297 {
298  m_co[0] -= q[0];
299  m_co[1] -= q[1];
300  m_co[2] -= q[2];
301  m_co[3] -= q[3];
302  return *this;
303 }
304 
305 
306 inline Quaternion& Quaternion::operator*=(const Scalar& s)
307 {
308  m_co[0] *= s;
309  m_co[1] *= s;
310  m_co[2] *= s;
311  m_co[3] *= s;
312  return *this;
313 }
314 
315 
317 {
318  setValue(m_co[3] * q[0] + m_co[0] * q[3] + m_co[1] * q[2] - m_co[2] * q[1],
319  m_co[3] * q[1] + m_co[1] * q[3] + m_co[2] * q[0] - m_co[0] * q[2],
320  m_co[3] * q[2] + m_co[2] * q[3] + m_co[0] * q[1] - m_co[1] * q[0],
321  m_co[3] * q[3] - m_co[0] * q[0] - m_co[1] * q[1] - m_co[2] * q[2]);
322  return *this;
323 }
324 
325 
326 inline Quaternion& Quaternion::operator/=(const Scalar& s)
327 {
328  m_co[0] /= s;
329  m_co[1] /= s;
330  m_co[2] /= s;
331  m_co[3] /= s;
332  return *this;
333 }
334 
335 
336 inline Vector3 Quaternion::operator()(const Vector3& v) const
337 {
338  const Quaternion q(*this * (v * inverse()));
339  return Vector3(q[0], q[1], q[2]);
340 }
341 
342 
343 inline bool Quaternion::operator==(const Quaternion& q) const
344 {
345  return (distance(q) == Scalar(0.0)) && (angle(q) == Scalar(0.0));
346 }
347 
348 
349 inline bool Quaternion::operator!=(const Quaternion& q) const
350 {
351  return !(*this == q);
352 }
353 
354 
355 // OPERATIONS
356 
357 inline Scalar Quaternion::dot(const Quaternion& q) const
358 {
359  return m_co[0] * q[0] +
360  m_co[1] * q[1] +
361  m_co[2] * q[2] +
362  m_co[3] * q[3];
363 }
364 
365 
366 inline Scalar Quaternion::length2() const
367 {
368  return dot(*this);
369 }
370 
371 
372 inline Scalar Quaternion::length() const
373 {
374  return sqrt(length2());
375 }
376 
377 
379 {
380  return *this /= length();
381 }
382 
383 
384 inline Scalar Quaternion::distance2(const Quaternion& q) const
385 {
386  Quaternion diff(*this);
387  diff -= q;
388  const Scalar d2 = diff.length2();
389  return d2;
390 }
391 
392 
393 inline Scalar Quaternion::distance(const Quaternion& q) const
394 {
395  Quaternion diff(*this);
396  diff -= q;
397  const Scalar d = diff.length();
398  return d;
399 }
400 
401 
402 inline Scalar Quaternion::angleCos(const Quaternion& q) const
403 {
404  const Scalar num(dot(q));
405  const Scalar den(sqrt(length2() * q.length2()));
406 
407  Scalar ang_cos(num / den);
408  return ang_cos;
409 }
410 
411 
412 inline Scalar Quaternion::angle(const Quaternion& q) const
413 {
414  const Scalar ang_cos(angleCos(q));
415  return acos(ang_cos);
416 }
417 
418 
420 {
421  return Quaternion(-m_co[0], -m_co[1], -m_co[2], m_co[3]);
422 }
423 
424 
426 {
427  return conjugate() / length2();
428 }
429 
430 
432  const Scalar& t) const
433 {
434  // Angle between quaternions
435  Scalar theta = angle(q);
436 
437  // Parallel test between quaternions
438  if (theta != 0.0)
439  {
440  // Quaternion located at position determined by "t"
441  Scalar d = Scalar(1.0) / sin(theta);
442  Scalar s0 = sin((Scalar(1.0) - t) * theta);
443  Scalar s1 = sin(t * theta);
444  return Quaternion((m_co[0] * s0 + q[0] * s1) * d,
445  (m_co[1] * s0 + q[1] * s1) * d,
446  (m_co[2] * s0 + q[2] * s1) * d,
447  (m_co[3] * s0 + q[3] * s1) * d);
448  }
449  else
450  {
451  return *this;
452  }
453 }
454 
455 
456 // ACCESS
457 
458 inline Scalar& Quaternion::at(size_t n)
459 {
460  util::Assert(n < 4, std::range_error("Index out of range"));
461  return m_co[n];
462 }
463 
464 
465 inline const Scalar& Quaternion::at(size_t n) const
466 {
467  util::Assert(n < 4, std::range_error("Index out of range"));
468  return m_co[n];
469 }
470 
471 
472 inline void Quaternion::setValue(const Scalar& x,
473  const Scalar& y,
474  const Scalar& z,
475  const Scalar& w)
476 {
477  m_co[0] = x;
478  m_co[1] = y;
479  m_co[2] = z;
480  m_co[3] = w;
481 }
482 
483 
484 inline void Quaternion::setValue(const Scalar* q)
485 {
486  m_co[0] = q[0];
487  m_co[1] = q[1];
488  m_co[2] = q[2];
489  m_co[3] = q[3];
490 }
491 
492 
494 
495 // OPERATORS
496 
497 inline Quaternion operator+(const Quaternion& q)
498 {
499  return q;
500 }
501 
502 
503 inline Quaternion operator-(const Quaternion& q)
504 {
505  return Quaternion(-q[0],
506  -q[1],
507  -q[2],
508  -q[3]);
509 }
510 
511 
512 inline Quaternion operator+(const Quaternion& q1,
513  const Quaternion& q2)
514 {
515  Quaternion q(q1);
516  return q += q2;
517 }
518 
519 
520 inline Quaternion operator-(const Quaternion& q1,
521  const Quaternion& q2)
522 {
523  Quaternion q(q1);
524  return q -= q2;
525 }
526 
527 
528 inline Quaternion operator*(const Quaternion& q1,
529  const Quaternion& q2)
530 {
531  Quaternion q(q1);
532  return q *= q2;
533 }
534 
535 inline Quaternion operator*(const Quaternion& q,
536  const Vector3& v)
537 {
538  return Quaternion(q[3] * v[0] + q[1] * v[2] - q[2] * v[1],
539  q[3] * v[1] + q[2] * v[0] - q[0] * v[2],
540  q[3] * v[2] + q[0] * v[1] - q[1] * v[0],
541  -q[0] * v[0] - q[1] * v[1] - q[2] * v[2]);
542 }
543 
544 
545 inline Quaternion operator*(const Vector3& v,
546  const Quaternion& q)
547 {
548  return Quaternion(v[0] * q[3] + v[1] * q[2] - v[2] * q[1],
549  v[1] * q[3] + v[2] * q[0] - v[0] * q[2],
550  v[2] * q[3] + v[0] * q[1] - v[1] * q[0],
551  -v[0] * q[0] - v[1] * q[1] - v[2] * q[2]);
552 }
553 
554 
555 inline Quaternion operator*(const Scalar& s,
556  const Quaternion& q)
557 {
558  Quaternion q1(q);
559  return q1 *= s;
560 }
561 
562 
563 inline Quaternion operator*(const Quaternion& q,
564  const Scalar& s)
565 {
566  Quaternion q1(q);
567  return q1 *= s;
568 }
569 
570 inline Quaternion operator/(const Quaternion& q,
571  const Scalar& s)
572 {
573 
574  Quaternion q1(q);
575  return q1 /= s;
576 }
577 
578 inline std::ostream& operator<<(std::ostream& os,
579  const Quaternion& q)
580 {
581  return os << '[' << q[0] << ' '
582  << q[1] << ' '
583  << q[2] << ' '
584  << q[3] << ']';
585 }
586 
587 
588 // FUNCTIONS
589 
590 inline Scalar length2(const Quaternion& q)
591 {
592  return q.length2();
593 }
594 
595 
596 inline Scalar length(const Quaternion& q)
597 {
598  return q.length();
599 }
600 
601 
602 inline Quaternion normalize(const Quaternion& q)
603 {
604  Quaternion q1(q);
605  return q1.normalize();
606 }
607 
608 
609 inline Scalar angleCos(const Quaternion& q1,
610  const Quaternion& q2)
611 {
612  return q1.angleCos(q2);
613 }
614 
615 
616 inline Scalar angle(const Quaternion& q1,
617  const Quaternion& q2)
618 {
619  return q1.angle(q2);
620 }
621 
622 
623 inline Scalar dot(const Quaternion& q1,
624  const Quaternion& q2)
625 {
626  return q1.dot(q2);
627 }
628 
629 
630 inline Quaternion conjugate(const Quaternion& q)
631 {
632  return q.conjugate();
633 }
634 
635 
636 inline Quaternion inverse(const Quaternion& q)
637 {
638  return q.inverse();
639 }
640 
641 
642 inline Quaternion slerp(const Quaternion& q1,
643  const Quaternion& q2,
644  const Scalar& t)
645 {
646  return q1.slerp(q2, t);
647 }
648 
649 } // mt
650 
651 #endif // MT_QUATERNION_H