Jpp
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
JQuaternion3D.hh
Go to the documentation of this file.
1 #ifndef __JQUATERNION3D__
2 #define __JQUATERNION3D__
3 
4 #include <istream>
5 #include <ostream>
6 #include <cmath>
7 
8 #include "JIO/JSerialisable.hh"
9 #include "JLang/JEquals.hh"
10 #include "JMath/JMath.hh"
11 #include "JGeometry3D/JVector3D.hh"
12 #include "JGeometry3D/JVersor3D.hh"
13 
14 
15 /**
16  * \author mdejong
17  */
18 
19 namespace JGEOMETRY3D {}
20 namespace JPP { using namespace JGEOMETRY3D; }
21 
22 namespace JGEOMETRY3D {
23 
24  using JIO::JReader;
25  using JIO::JWriter;
26  using JMATH::JMath;
27  using JLANG::JEquals;
28 
29 
30  /**
31  * This class represents a rotation.
32  */
33  struct JQuaternion2D {
34  protected:
35  /**
36  * Default constructor.
37  * This constructor corresponds to the identity operation.
38  */
40  __a(1.0),
41  __u(0.0)
42  {}
43 
44 
45  /**
46  * Constructor.
47  *
48  * \param theta rotation angle [rad]
49  */
50  JQuaternion2D(const double theta) :
51  __a(cos(0.5*theta)),
52  __u(sin(0.5*theta))
53  {}
54 
55 
56  public:
57  /**
58  * Get a value.
59  *
60  * \return a value
61  */
62  double getA() const
63  {
64  return __a;
65  }
66 
67 
68  /**
69  * Write quaternion from input.
70  *
71  * \param in input stream
72  * \param quaternion quaternion
73  * \return input stream
74  */
75  friend inline std::istream& operator>>(std::istream& in, JQuaternion2D& quaternion)
76  {
77  return in >> quaternion.__a >> quaternion.__u;
78  }
79 
80 
81  /**
82  * Write quaternion to output.
83  *
84  * \param out output stream
85  * \param quaternion quaternion
86  * \return output stream
87  */
88  friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion2D& quaternion)
89  {
90  return out << quaternion.__a << ' ' << quaternion.__u;
91  }
92 
93 
94  /**
95  * Read quaternion from input.
96  *
97  * \param in reader
98  * \param quaternion quaternion
99  * \return reader
100  */
101  friend inline JReader& operator>>(JReader& in, JQuaternion2D& quaternion)
102  {
103  return in >> quaternion.__a >> quaternion.__u;
104  }
105 
106 
107  /**
108  * Write quaternion to output.
109  *
110  * \param out writer
111  * \param quaternion quaternion
112  * \return writer
113  */
114  friend inline JWriter& operator<<(JWriter& out, const JQuaternion2D& quaternion)
115  {
116  return out << quaternion.__a << quaternion.__u;
117  }
118 
119 
120  protected:
121  double __a;
122  double __u;
123  };
124 
125 
126  /**
127  * This class represents a rotation around the x-axis.
128  */
129  struct JQuaternion3X :
130  public JQuaternion2D
131  {
132  /**
133  * Default constructor.
134  */
136  JQuaternion2D()
137  {}
138 
139 
140  /**
141  * Constructor.
142  *
143  * \param theta rotation angle [rad]
144  */
145  JQuaternion3X(const double theta) :
146  JQuaternion2D(theta)
147  {}
148 
149 
150  /**
151  * Get b value.
152  *
153  * \return b value
154  */
155  double getB() const
156  {
157  return __u;
158  }
159  };
160 
161 
162  /**
163  * This class represents a rotation around the y-axis.
164  */
165  struct JQuaternion3Y :
166  public JQuaternion2D
167  {
168  /**
169  * Default constructor.
170  */
172  JQuaternion2D()
173  {}
174 
175 
176  /**
177  * Constructor.
178  *
179  * \param theta rotation angle [rad]
180  */
181  JQuaternion3Y(const double theta) :
182  JQuaternion2D(theta)
183  {}
184 
185 
186  /**
187  * Get c value.
188  *
189  * \return c value
190  */
191  double getC() const
192  {
193  return __u;
194  }
195  };
196 
197 
198  /**
199  * This class represents a rotation around the z-axis.
200  */
201  struct JQuaternion3Z :
202  public JQuaternion2D
203  {
204  /**
205  * Default constructor.
206  */
208  JQuaternion2D()
209  {}
210 
211 
212  /**
213  * Constructor.
214  *
215  * \param theta rotation angle [rad]
216  */
217  JQuaternion3Z(const double theta) :
218  JQuaternion2D(theta)
219  {}
220 
221 
222  /**
223  * Get d value.
224  *
225  * \return d value
226  */
227  double getD() const
228  {
229  return __u;
230  }
231  };
232 
233 
234  /**
235  * Data structure for quaternion in three dimensions.
236  *
237  * This class implements the JMATH::JMath and JLANG::JEquals interfaces.
238  */
240  public JMath <JQuaternion3D>,
241  public JMath <JQuaternion3D, JQuaternion3X>,
242  public JMath <JQuaternion3D, JQuaternion3Y>,
243  public JMath <JQuaternion3D, JQuaternion3Z>,
244  public JEquals<JQuaternion3D>
245  {
246  public:
247 
252 
253 
254  /**
255  * Default constructor.
256  */
258  __a(0.0),
259  __b(0.0),
260  __c(0.0),
261  __d(0.0)
262  {}
263 
264 
265  /**
266  * Constructor.
267  *
268  * \param a a value
269  * \param b b value
270  * \param c c value
271  * \param d d value
272  */
273  JQuaternion3D(const double a,
274  const double b,
275  const double c,
276  const double d) :
277  __a(a),
278  __b(b),
279  __c(c),
280  __d(d)
281  {}
282 
283 
284  /**
285  * Constructor.
286  *
287  * \param pos position
288  */
289  JQuaternion3D(const JVector3D& pos) :
290  __a(0.0),
291  __b(pos.getX()),
292  __c(pos.getY()),
293  __d(pos.getZ())
294  {}
295 
296 
297  /**
298  * Constructor.
299  *
300  * \param dir direction
301  */
302  JQuaternion3D(const JVersor3D& dir) :
303  __a(0.0),
304  __b(dir.getDX()),
305  __c(dir.getDY()),
306  __d(dir.getDZ())
307  {}
308 
309 
310  /**
311  * Constructor.
312  * This constructor represents a rotation around the given axis by the given angle.
313  *
314  * \param theta rotation angle [rad]
315  * \param dir rotation axis
316  */
317  JQuaternion3D(const double theta,
318  const JVersor3D& dir)
319  {
320  const double ct = cos(0.5*theta);
321  const double st = sin(0.5*theta);
322 
323  __a = ct;
324  __b = st * dir.getDX();
325  __c = st * dir.getDY();
326  __d = st * dir.getDZ();
327  }
328 
329 
330  /**
331  * Constructor.
332  *
333  * \param qx rotation around x-axis
334  * \param qy rotation around y-axis
335  * \param qz rotation around x-axis
336  */
338  const JQuaternion3Y& qy,
339  const JQuaternion3Z& qz) :
340  __a(1.0),
341  __b(0.0),
342  __c(0.0),
343  __d(0.0)
344  {
345  mul(qz).mul(qy).mul(qx);
346  }
347 
348 
349  /**
350  * Type conversion operator.
351  *
352  * \return position
353  */
354  operator JVector3D() const
355  {
356  return JVector3D(this->getB(), this->getC(), this->getD());
357  }
358 
359 
360  /**
361  * Type conversion operator.
362  *
363  * \return direction
364  */
365  operator JVersor3D() const
366  {
367  return JVersor3D(this->getB(), this->getC(), this->getD());
368  }
369 
370 
371  /**
372  * Get a value.
373  *
374  * \return a value
375  */
376  double getA() const
377  {
378  return __a;
379  }
380 
381 
382  /**
383  * Get b value.
384  *
385  * \return b value
386  */
387  double getB() const
388  {
389  return __b;
390  }
391 
392 
393  /**
394  * Get c value.
395  *
396  * \return c value
397  */
398  double getC() const
399  {
400  return __c;
401  }
402 
403 
404  /**
405  * Get d value.
406  *
407  * \return d value
408  */
409  double getD() const
410  {
411  return __d;
412  }
413 
414 
415  /**
416  * Set to identity matrix
417  *
418  * \return this quaternion
419  */
421  {
422  __a = 1.0;
423  __b = 0.0;
424  __c = 0.0;
425  __d = 0.0;
426 
427  return *this;
428  }
429 
430 
431  /**
432  * Conjugate quaternion.
433  *
434  * \return this quaternion
435  */
437  {
438  __b = -__b;
439  __c = -__c;
440  __d = -__d;
441 
442  return *this;
443  }
444 
445 
446  /**
447  * Negate quaternion.
448  *
449  * \return this quaternion
450  */
452  {
453  __a = -__a;
454  __b = -__b;
455  __c = -__c;
456  __d = -__d;
457 
458  return *this;
459  }
460 
461 
462  /**
463  * Add quaternion.
464  *
465  * \param quaternion quaternion
466  * \return this quaternion
467  */
468  JQuaternion3D& add(const JQuaternion3D& quaternion)
469  {
470  __a += quaternion.getA();
471  __b += quaternion.getB();
472  __c += quaternion.getC();
473  __d += quaternion.getD();
474 
475  return *this;
476  }
477 
478 
479  /**
480  * Subtract quaternion.
481  *
482  * \param quaternion quaternion
483  * \return this quaternion
484  */
485  JQuaternion3D& sub(const JQuaternion3D& quaternion)
486  {
487  __a -= quaternion.getA();
488  __b -= quaternion.getB();
489  __c -= quaternion.getC();
490  __d -= quaternion.getD();
491 
492  return *this;
493  }
494 
495 
496  /**
497  * Scale quaternion.
498  *
499  * \param factor multiplication factor
500  * \return this quaternion
501  */
502  JQuaternion3D& mul(const double factor)
503  {
504  __a *= factor;
505  __b *= factor;
506  __c *= factor;
507  __d *= factor;
508 
509  return *this;
510  }
511 
512 
513  /**
514  * Scale quaternion.
515  *
516  * \param factor division factor
517  * \return this quaternion
518  */
519  JQuaternion3D& div(const double factor)
520  {
521  __a /= factor;
522  __b /= factor;
523  __c /= factor;
524  __d /= factor;
525 
526  return *this;
527  }
528 
529 
530  /**
531  * Quaternion multiplicaion.
532  *
533  * This method evaluates the Hamilton product (also called cross product).
534  *
535  * \param first first quaternion
536  * \param second second quaternion
537  * \return this quaternion
538  */
539  JQuaternion3D& mul(const JQuaternion3D& first, const JQuaternion3X& second)
540  {
541  __a = first.getA() * second.getA() - first.getB() * second.getB();
542  __b = first.getA() * second.getB() + first.getB() * second.getA();
543  __c = first.getC() * second.getA() + first.getD() * second.getB();
544  __d = -first.getC() * second.getB() + first.getD() * second.getA();
545 
546  return *this;
547  }
548 
549 
550  /**
551  * Quaternion multiplicaion.
552  *
553  * This method evaluates the Hamilton product (or cross product).
554  *
555  * \param first first quaternion
556  * \param second second quaternion
557  * \return this quaternion
558  */
560  const JQuaternion3Y& second)
561  {
562  __a = first.getA() * second.getA() - first.getC() * second.getC();
563  __b = first.getB() * second.getA() - first.getD() * second.getC();
564  __c = first.getA() * second.getC() + first.getC() * second.getA();
565  __d = first.getB() * second.getC() + first.getD() * second.getA();
566 
567  return *this;
568  }
569 
570 
571  /**
572  * Quaternion multiplicaion.
573  *
574  * This method evaluates the Hamilton product (or cross product).
575  *
576  * \param first first quaternion
577  * \param second second quaternion
578  * \return this quaternion
579  */
581  const JQuaternion3Z& second)
582  {
583  __a = first.getA() * second.getA() - first.getD() * second.getD();
584  __b = first.getB() * second.getA() + first.getC() * second.getD();
585  __c = -first.getB() * second.getD() + first.getC() * second.getA();
586  __d = first.getA() * second.getD() + first.getD() * second.getA();
587 
588  return *this;
589  }
590 
591 
592  /**
593  * Quaternion multiplicaion.
594  *
595  * This method evaluates the Hamilton product (or cross product).
596  *
597  * \param first first quaternion
598  * \param second second quaternion
599  * \return this quaternion
600  */
602  const JQuaternion3D& second)
603  {
604  __a = first.getA() * second.getA() - first.getB() * second.getB() - first.getC() * second.getC() - first.getD() * second.getD();
605  __b = first.getA() * second.getB() + first.getB() * second.getA() + first.getC() * second.getD() - first.getD() * second.getC();
606  __c = first.getA() * second.getC() - first.getB() * second.getD() + first.getC() * second.getA() + first.getD() * second.getB();
607  __d = first.getA() * second.getD() + first.getB() * second.getC() - first.getC() * second.getB() + first.getD() * second.getA();
608 
609  return *this;
610  }
611 
612 
613  /**
614  * Quaternion multiplicaion.
615  *
616  * \param qx rotation around x-axis
617  * \param qy rotation around y-axis
618  * \param qz rotation around x-axis
619  * \return this quaternion
620  */
622  const JQuaternion3Y& qy,
623  const JQuaternion3Z& qz)
624  {
625  return *this = JQuaternion3D().setIdentity().mul(qz).mul(qy).mul(qx);
626  }
627 
628 
629  /**
630  * Check equality.
631  *
632  * \param quaternion quaternion
633  * \return true if quaternions are equal; else false
634  */
635  bool equals(const JQuaternion3D& quaternion) const
636  {
637  return (getA() == quaternion.getA() &&
638  getB() == quaternion.getB() &&
639  getC() == quaternion.getC() &&
640  getD() == quaternion.getD());
641  }
642 
643 
644  /**
645  * Get length squared.
646  *
647  * \return square of length
648  */
649  double getLengthSquared() const
650  {
651  return getA()*getA() + getB()*getB() + getC()*getC() + getD()*getD();
652  }
653 
654 
655  /**
656  * Get length.
657  *
658  * \return length
659  */
660  double getLength() const
661  {
662  return sqrt(getLengthSquared());
663  }
664 
665 
666  /**
667  * Get squared of distance to quaternion.
668  *
669  * \param quaternion quaternion
670  * \return square of distance
671  */
672  double getDistanceSquared(const JQuaternion3D& quaternion) const
673  {
674  return JQuaternion3D(quaternion).sub(*this).getLengthSquared();
675  }
676 
677 
678  /**
679  * Get distance to quaternion.
680  *
681  * \param quaternion quaternion
682  * \return distance
683  */
684  double getDistance(const JQuaternion3D& quaternion) const
685  {
686  return sqrt(getDistanceSquared(quaternion));
687  }
688 
689 
690  /**
691  * Get dot product.
692  *
693  * \param quaternion quaternion
694  * \return dot product
695  */
696  double getDot(const JQuaternion3D& quaternion) const
697  {
698  return
699  getA() * quaternion.getA() -
700  getB() * quaternion.getB() -
701  getC() * quaternion.getC() -
702  getD() * quaternion.getD();
703  }
704 
705 
706  /**
707  * Write quaternion from input.
708  *
709  * \param in input stream
710  * \param quaternion quaternion
711  * \return input stream
712  */
713  friend inline std::istream& operator>>(std::istream& in, JQuaternion3D& quaternion)
714  {
715  in >> quaternion.__a;
716  in >> quaternion.__b;
717  in >> quaternion.__c;
718  in >> quaternion.__d;
719 
720  return in;
721  }
722 
723 
724  /**
725  * Write quaternion to output.
726  *
727  * \param out output stream
728  * \param quaternion quaternion
729  * \return output stream
730  */
731  friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion3D& quaternion)
732  {
733  out << quaternion.getA() << ' '
734  << quaternion.getB() << ' '
735  << quaternion.getC() << ' '
736  << quaternion.getD();
737 
738  return out;
739  }
740 
741 
742  /**
743  * Read quaternion from input.
744  *
745  * \param in reader
746  * \param quaternion quaternion
747  * \return reader
748  */
749  friend inline JReader& operator>>(JReader& in, JQuaternion3D& quaternion)
750  {
751  in >> quaternion.__a;
752  in >> quaternion.__b;
753  in >> quaternion.__c;
754  in >> quaternion.__d;
755 
756  return in;
757  }
758 
759 
760  /**
761  * Write quaternion to output.
762  *
763  * \param out writer
764  * \param quaternion quaternion
765  * \return writer
766  */
767  friend inline JWriter& operator<<(JWriter& out, const JQuaternion3D& quaternion)
768  {
769  out << quaternion.getA();
770  out << quaternion.getB();
771  out << quaternion.getC();
772  out << quaternion.getD();
773 
774  return out;
775  }
776 
777  protected:
778  double __a;
779  double __b;
780  double __c;
781  double __d;
782  };
783 }
784 
785 #endif
double getDot(const JQuaternion3D &quaternion) const
Get dot product.
Interface for binary output.
JQuaternion3X()
Default constructor.
double getB() const
Get b value.
Auxiliary base class for aritmetic operations of derived class types.
Definition: JMath.hh:26
JQuaternion3D(const double theta, const JVersor3D &dir)
Constructor.
JQuaternion2D()
Default constructor.
JQuaternion3X(const double theta)
Constructor.
This class represents a rotation around the x-axis.
friend std::istream & operator>>(std::istream &in, JQuaternion2D &quaternion)
Write quaternion from input.
double getB() const
Get b value.
double getA() const
Get a value.
double getLengthSquared() const
Get length squared.
JQuaternion3D & add(const JQuaternion3D &quaternion)
Add quaternion.
double getDistanceSquared(const JQuaternion3D &quaternion) const
Get squared of distance to quaternion.
JQuaternion3Z(const double theta)
Constructor.
double getC() const
Get c value.
friend std::ostream & operator<<(std::ostream &out, const JQuaternion2D &quaternion)
Write quaternion to output.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3Z &second)
Quaternion multiplicaion.
JQuaternion3Z()
Default constructor.
JQuaternion3D & mul(const double factor)
Scale quaternion.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3Y &second)
Quaternion multiplicaion.
JQuaternion3D & sub(const JQuaternion3D &quaternion)
Subtract quaternion.
JQuaternion3Y()
Default constructor.
Data structure for vector in three dimensions.
Definition: JVector3D.hh:32
This class represents a rotation.
double getDY() const
Get y direction.
Definition: JVersor3D.hh:101
double getDX() const
Get x direction.
Definition: JVersor3D.hh:90
This class represents a rotation around the z-axis.
Template definition of auxiliary base class for comparison of data structures.
Definition: JEquals.hh:24
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3X &second)
Quaternion multiplicaion.
Interface for binary input.
friend JReader & operator>>(JReader &in, JQuaternion2D &quaternion)
Read quaternion from input.
double getD() const
Get d value.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3D &second)
Quaternion multiplicaion.
Data structure for quaternion in three dimensions.
JQuaternion2D(const double theta)
Constructor.
bool equals(const JQuaternion3D &quaternion) const
Check equality.
JQuaternion3D & setIdentity()
Set to identity matrix.
This class represents a rotation around the y-axis.
friend std::ostream & operator<<(std::ostream &out, const JQuaternion3D &quaternion)
Write quaternion to output.
double getD() const
Get d value.
friend JReader & operator>>(JReader &in, JQuaternion3D &quaternion)
Read quaternion from input.
double getC() const
Get c value.
JQuaternion3D()
Default constructor.
Base class for data structures with artithmetic capabilities.
double getA() const
Get a value.
JQuaternion3D(const JVersor3D &dir)
Constructor.
JQuaternion3Y(const double theta)
Constructor.
JQuaternion3D(const JQuaternion3X &qx, const JQuaternion3Y &qy, const JQuaternion3Z &qz)
Constructor.
JQuaternion3D & mul(const JQuaternion3X &qx, const JQuaternion3Y &qy, const JQuaternion3Z &qz)
Quaternion multiplicaion.
Data structure for normalised vector in three dimensions.
Definition: JVersor3D.hh:23
JQuaternion3D(const double a, const double b, const double c, const double d)
Constructor.
friend std::istream & operator>>(std::istream &in, JQuaternion3D &quaternion)
Write quaternion from input.
JQuaternion3D & div(const double factor)
Scale quaternion.
JQuaternion3D & negate()
Negate quaternion.
friend JWriter & operator<<(JWriter &out, const JQuaternion3D &quaternion)
Write quaternion to output.
double getDZ() const
Get z direction.
Definition: JVersor3D.hh:112
JQuaternion3D(const JVector3D &pos)
Constructor.
double getDistance(const JQuaternion3D &quaternion) const
Get distance to quaternion.
friend JWriter & operator<<(JWriter &out, const JQuaternion2D &quaternion)
Write quaternion to output.
double getLength() const
Get length.
JQuaternion3D & conjugate()
Conjugate quaternion.