| Bsoft 2.0.5 Bernard's software package

Quaternion.h
Go to the documentation of this file.
1
9 #ifndef _Quaternion_
10 #define _Quaternion_
11
12 #include <cmath>
13
14 #include "Vector3.h"
15
16 template <typename Type> class Vector3;
17
18 /************************************************************************
19 @Object: class Quaternion
20 @Description:
21 Quaternion class for orientations and rotations.
22 @Features:
23 The internal variables are an array of 4 double precision floating point numbers.
24 *************************************************************************/
25 class Quaternion {
26 double s;
27 Vector3<double> v;
28 void check() {
29 if ( fabs(s) < TRIGPRECISION ) s = 0;
30 for ( long i=0; i<3; i++ )
31 if ( fabs(v[i]) < TRIGPRECISION ) v[i] = 0;
32 }
33 template <typename Type>
34 void from_axis_angle(Vector3<Type>& axis, double angle) {
35 double a = angle/2.0L;
36 Vector3<double> unitaxis = axis;
37 unitaxis.normalize();
38 s = cos(a);
39 v = unitaxis*sin(a);
40 check();
41 }
42 public:
43 Quaternion() : s(0), v(0,0,0) { }
44 Quaternion(const double scalar) : s(scalar), v(0,0,0) {
45 check();
46 }
47 Quaternion(const double scalar, const double x, const double y, const double z) :
48 s(scalar), v(x,y,z) {
49 check();
50 }
51 Quaternion(const Quaternion& q) : s(q.s), v(q.v) {
52 check();
53 }
54 template <typename Type>
55 Quaternion(const Vector3<Type>& vector) :
56 s(0), v(vector) {
57 check();
58 }
59 // Note the order - compare Quaternion(axis, angle)
60 template <typename Type>
61 Quaternion(const double scalar, const Vector3<Type>& vector) :
62 s(scalar), v(vector) {
63 check();
64 }
65 template <typename Type>
66 // Note the order - compare Quaternion(s,v)
67 Quaternion(Vector3<Type>& axis, double angle) {
68 from_axis_angle(axis, angle);
69 }
70 template <typename Type1, typename Type2>
71 Quaternion(Vector3<Type1>& from_vec, Vector3<Type2>& to_vec) {
72 Vector3<double> v1(from_vec);
73 Vector3<double> v2(to_vec);
74 v1.normalize();
75 v2.normalize();
76 // Calculate the vector product to get the axis of rotation
77 Vector3<double> axis = v1.cross(v2);
78 double angle = v1.angle(v2);
79 if ( axis.length2() < 1e-12 ) {
80 v2 = Vector3<double>(1,0,0);
81 axis = v1.cross(v2);
82 if ( axis.length2() < 1e-12 ) {
83 v2 = Vector3<double>(0,1,0);
84 axis = v1.cross(v2);
85 }
86 }
87 from_axis_angle(axis, angle);
88 }
89 Quaternion operator=(const Quaternion q) {
90 s = q.s; v = q.v;
91 return *this;
92 }
93 Quaternion operator-() {
94 s = -s; v = -v;
95 return *this;
96 }
97 Quaternion operator+=(const Quaternion& q) {
98 s += q.s; v += q.v;
99 return *this;
100 }
101 Quaternion operator+(const Quaternion& q) {
102 Quaternion qn(*this);
103 return qn += q;
104 }
105 Quaternion operator-=(const Quaternion& q) {
106 s -= q.s; v -= q.v;
107 return *this;
108 }
109 Quaternion operator-(const Quaternion& q) {
110 Quaternion qn(*this);
111 return qn -= q;
112 }
113 Quaternion operator*=(const Quaternion& q) {
114 *this = *this * q;
115 return *this;
116 }
117 Quaternion operator*(const Quaternion& q) {
118 Quaternion qn;
119 qn.s = s*q.s - v.scalar(q.v);
120 qn.v[0] = s*q.v[0] + v[0]*q.s + v[1]*q.v[2] - v[2]*q.v[1];
121 qn.v[1] = s*q.v[1] + v[1]*q.s + v[2]*q.v[0] - v[0]*q.v[2];
122 qn.v[2] = s*q.v[2] + v[2]*q.s + v[0]*q.v[1] - v[1]*q.v[0];
123 return qn;
124 }
125 Quaternion operator/=(const double d) {
126 double div = 1/d;
127 if ( !isfinite(div) ) cerr << "division by zero!" << endl;
128 s *= div;
129 v *= div;
130 return *this;
131 }
132 Quaternion operator/(const double d) {
133 Quaternion q(*this);
134 q /= d;
135 return q;
136 }
137 double& operator[](long i) {
138 if ( i<1 ) return s;
139 if ( i>3 ) i=3;
140 return v[i-1];
141 }
142 double scalar() { return s; }
143 double angle() { return 2*acos(s); }
144 Vector3<double> axis() {
145 Vector3<double> ax(v);
146 ax.normalize();
147 return ax;
148 }
149 double norm() { return sqrt(norm2()); }
150 double norm2() { return s*s + v.length2(); }
151 double normalize() {
152 double size = norm();
153 if ( !size ) {
154 cerr << "quaternion size = " << size << endl;
155 exit(-1);
156 }
157 *this /= size;
158 return size;
159 }
160 Quaternion conj() { return Quaternion(s, -v); }
161 double invert() {
162 double size2 = norm2();
163 double div = 1/size2;
164 s *= div;
165 v *= -div;
166 return sqrt(size2);
167 }
168 Quaternion inverse() {
169 Quaternion q(*this);
170 q.invert();
171 return q;
172 }
173 Quaternion rotate(const Quaternion& point) {
174 Quaternion q(*this);
175 q.invert();
176 return *this * point * q;
177 }
178 } ;
179
180 ostream& operator<<(ostream& output, Quaternion& q);
181
182 template <typename Type>
183 inline Vector3<Type> vector3_rotate(Vector3<Type>& vec, Vector3<Type>& axis, double angle)
184 {
185 Quaternion p(vec);
186 Quaternion q(axis, angle);
187
188 Quaternion nq = q.rotate(p);
189
190 Vector3<Type> newvec = nq.axis();
191
192 newvec *= vec.length();
193
194 return newvec;
195 }
196
197 #endif
198
199
200
Vector3::cross
Vector3 cross(const Vector3 &v)
Definition: Vector3.h:250
Quaternion::operator *
Quaternion operator *(const Quaternion &q)
Definition: Quaternion.h:117
Quaternion::operator=
Quaternion operator=(const Quaternion q)
Definition: Quaternion.h:89
Vector3::normalize
double normalize()
Definition: Vector3.h:236
Quaternion::angle
double angle()
Definition: Quaternion.h:143
Vector3::length
double length()
Definition: Vector3.h:222
Quaternion::operator+
Quaternion operator+(const Quaternion &q)
Definition: Quaternion.h:101
Quaternion::operator[]
double & operator[](long i)
Definition: Quaternion.h:137
Quaternion::rotate
Quaternion rotate(const Quaternion &point)
Definition: Quaternion.h:173
Quaternion::scalar
double scalar()
Definition: Quaternion.h:142
operator<<
ostream & operator<<(ostream &output, Quaternion &q)
Definition: Quaternion.cpp:15
Quaternion::invert
double invert()
Definition: Quaternion.h:161
Quaternion::operator-
Quaternion operator-()
Definition: Quaternion.h:93
Quaternion::Quaternion
Quaternion(Vector3< Type > &axis, double angle)
Definition: Quaternion.h:67
Quaternion
Definition: Quaternion.h:25
Quaternion::conj
Quaternion conj()
Definition: Quaternion.h:160
TRIGPRECISION
#define TRIGPRECISION
Definition: utilities.h:47
Quaternion::Quaternion
Quaternion(const double scalar)
Definition: Quaternion.h:44
Quaternion::operator *=
Quaternion operator *=(const Quaternion &q)
Definition: Quaternion.h:113
Quaternion::norm
double norm()
Definition: Quaternion.h:149
isfinite
bool isfinite(complex< T > &value)
Vector3::scalar
double scalar(const Vector3 &v) const
Definition: Vector3.h:247
Vector3::angle
double angle(Vector3< T2 > &v)
Definition: Vector3.h:257
Quaternion::operator/
Quaternion operator/(const double d)
Definition: Quaternion.h:132
Quaternion::normalize
double normalize()
Definition: Quaternion.h:151
Quaternion::Quaternion
Quaternion(const double scalar, const double x, const double y, const double z)
Definition: Quaternion.h:47
Quaternion::Quaternion
Quaternion(const Vector3< Type > &vector)
Definition: Quaternion.h:55
Quaternion::operator-=
Quaternion operator-=(const Quaternion &q)
Definition: Quaternion.h:105
Vector3::length2
double length2()
Definition: Vector3.h:221
Quaternion::inverse
Quaternion inverse()
Definition: Quaternion.h:168
Quaternion::operator-
Quaternion operator-(const Quaternion &q)
Definition: Quaternion.h:109
Quaternion::Quaternion
Quaternion(const Quaternion &q)
Definition: Quaternion.h:51
Quaternion::operator+=
Quaternion operator+=(const Quaternion &q)
Definition: Quaternion.h:97
Vector3
Vector class for 3-value vectors used in 3D space. The internal variables are an array of 3 numbers.
Definition: Quaternion.h:16
Quaternion::axis
Vector3< double > axis()
Definition: Quaternion.h:144
Vector3.h
Class for 3-value vectors.
Quaternion::Quaternion
Quaternion(const double scalar, const Vector3< Type > &vector)
Definition: Quaternion.h:61
Quaternion::norm2
double norm2()
Definition: Quaternion.h:150
Quaternion::operator/=
Quaternion operator/=(const double d)
Definition: Quaternion.h:125
Quaternion::Quaternion
Quaternion(Vector3< Type1 > &from_vec, Vector3< Type2 > &to_vec)
Definition: Quaternion.h:71
Quaternion::Quaternion
Quaternion()
Definition: Quaternion.h:43
vector3_rotate
Vector3< Type > vector3_rotate(Vector3< Type > &vec, Vector3< Type > &axis, double angle)
Definition: Quaternion.h:183