# | Line 1 | Line 1 | |
---|---|---|
1 | < | /* |
1 | > | /* |
2 | * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved. | |
3 | * | |
4 | * The University of Notre Dame grants you ("Licensee") a | |
# | Line 6 | Line 6 | |
6 | * redistribute this software in source and binary code form, provided | |
7 | * that the following conditions are met: | |
8 | * | |
9 | < | * 1. Acknowledgement of the program authors must be made in any |
10 | < | * publication of scientific results based in part on use of the |
11 | < | * program. An acceptable form of acknowledgement is citation of |
12 | < | * the article in which the program was described (Matthew |
13 | < | * A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher |
14 | < | * J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented |
15 | < | * Parallel Simulation Engine for Molecular Dynamics," |
16 | < | * J. Comput. Chem. 26, pp. 252-271 (2005)) |
17 | < | * |
18 | < | * 2. Redistributions of source code must retain the above copyright |
9 | > | * 1. Redistributions of source code must retain the above copyright |
10 | * notice, this list of conditions and the following disclaimer. | |
11 | * | |
12 | < | * 3. Redistributions in binary form must reproduce the above copyright |
12 | > | * 2. Redistributions in binary form must reproduce the above copyright |
13 | * notice, this list of conditions and the following disclaimer in the | |
14 | * documentation and/or other materials provided with the | |
15 | * distribution. | |
# | Line 37 | Line 28 | |
28 | * arising out of the use of or inability to use software, even if the | |
29 | * University of Notre Dame has been advised of the possibility of | |
30 | * such damages. | |
31 | + | * |
32 | + | * SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your |
33 | + | * research, please cite the appropriate papers when you publish your |
34 | + | * work. Good starting points are: |
35 | + | * |
36 | + | * [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005). |
37 | + | * [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006). |
38 | + | * [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008). |
39 | + | * [4] Vardeman & Gezelter, in progress (2009). |
40 | */ | |
41 | ||
42 | /** | |
# | Line 49 | Line 49 | |
49 | #ifndef MATH_QUATERNION_HPP | |
50 | #define MATH_QUATERNION_HPP | |
51 | ||
52 | < | #include "math/Vector.hpp" |
52 | > | #include "math/Vector3.hpp" |
53 | #include "math/SquareMatrix.hpp" | |
54 | + | #define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) ) |
55 | + | const RealType tiny=1.0e-6; |
56 | ||
57 | < | namespace oopse{ |
57 | > | namespace OpenMD{ |
58 | ||
59 | < | /** |
60 | < | * @class Quaternion Quaternion.hpp "math/Quaternion.hpp" |
61 | < | * Quaternion is a sort of a higher-level complex number. |
62 | < | * It is defined as Q = w + x*i + y*j + z*k, |
63 | < | * where w, x, y, and z are numbers of type T (e.g. double), and |
64 | < | * i*i = -1; j*j = -1; k*k = -1; |
65 | < | * i*j = k; j*k = i; k*i = j; |
66 | < | */ |
67 | < | template<typename Real> |
68 | < | class Quaternion : public Vector<Real, 4> { |
67 | < | public: |
68 | < | Quaternion() : Vector<Real, 4>() {} |
59 | > | /** |
60 | > | * @class Quaternion Quaternion.hpp "math/Quaternion.hpp" |
61 | > | * Quaternion is a sort of a higher-level complex number. |
62 | > | * It is defined as Q = w + x*i + y*j + z*k, |
63 | > | * where w, x, y, and z are numbers of type T (e.g. RealType), and |
64 | > | * i*i = -1; j*j = -1; k*k = -1; |
65 | > | * i*j = k; j*k = i; k*i = j; |
66 | > | */ |
67 | > | template<typename Real> |
68 | > | class Quaternion : public Vector<Real, 4> { |
69 | ||
70 | < | /** Constructs and initializes a Quaternion from w, x, y, z values */ |
71 | < | Quaternion(Real w, Real x, Real y, Real z) { |
72 | < | this->data_[0] = w; |
73 | < | this->data_[1] = x; |
74 | < | this->data_[2] = y; |
75 | < | this->data_[3] = z; |
76 | < | } |
70 | > | public: |
71 | > | Quaternion() : Vector<Real, 4>() {} |
72 | > | |
73 | > | /** Constructs and initializes a Quaternion from w, x, y, z values */ |
74 | > | Quaternion(Real w, Real x, Real y, Real z) { |
75 | > | this->data_[0] = w; |
76 | > | this->data_[1] = x; |
77 | > | this->data_[2] = y; |
78 | > | this->data_[3] = z; |
79 | > | } |
80 | ||
81 | < | /** Constructs and initializes a Quaternion from a Vector<Real,4> */ |
82 | < | Quaternion(const Vector<Real,4>& v) |
83 | < | : Vector<Real, 4>(v){ |
84 | < | } |
81 | > | /** Constructs and initializes a Quaternion from a Vector<Real,4> */ |
82 | > | Quaternion(const Vector<Real,4>& v) |
83 | > | : Vector<Real, 4>(v){ |
84 | > | } |
85 | ||
86 | < | /** copy assignment */ |
87 | < | Quaternion& operator =(const Vector<Real, 4>& v){ |
88 | < | if (this == & v) |
89 | < | return *this; |
86 | > | /** copy assignment */ |
87 | > | Quaternion& operator =(const Vector<Real, 4>& v){ |
88 | > | if (this == & v) |
89 | > | return *this; |
90 | > | |
91 | > | Vector<Real, 4>::operator=(v); |
92 | > | |
93 | > | return *this; |
94 | > | } |
95 | > | |
96 | > | /** |
97 | > | * Returns the value of the first element of this quaternion. |
98 | > | * @return the value of the first element of this quaternion |
99 | > | */ |
100 | > | Real w() const { |
101 | > | return this->data_[0]; |
102 | > | } |
103 | ||
104 | < | Vector<Real, 4>::operator=(v); |
105 | < | |
106 | < | return *this; |
107 | < | } |
104 | > | /** |
105 | > | * Returns the reference of the first element of this quaternion. |
106 | > | * @return the reference of the first element of this quaternion |
107 | > | */ |
108 | > | Real& w() { |
109 | > | return this->data_[0]; |
110 | > | } |
111 | ||
112 | < | /** |
113 | < | * Returns the value of the first element of this quaternion. |
114 | < | * @return the value of the first element of this quaternion |
115 | < | */ |
116 | < | Real w() const { |
117 | < | return this->data_[0]; |
118 | < | } |
112 | > | /** |
113 | > | * Returns the value of the first element of this quaternion. |
114 | > | * @return the value of the first element of this quaternion |
115 | > | */ |
116 | > | Real x() const { |
117 | > | return this->data_[1]; |
118 | > | } |
119 | ||
120 | < | /** |
121 | < | * Returns the reference of the first element of this quaternion. |
122 | < | * @return the reference of the first element of this quaternion |
123 | < | */ |
124 | < | Real& w() { |
125 | < | return this->data_[0]; |
126 | < | } |
120 | > | /** |
121 | > | * Returns the reference of the second element of this quaternion. |
122 | > | * @return the reference of the second element of this quaternion |
123 | > | */ |
124 | > | Real& x() { |
125 | > | return this->data_[1]; |
126 | > | } |
127 | ||
128 | < | /** |
129 | < | * Returns the value of the first element of this quaternion. |
130 | < | * @return the value of the first element of this quaternion |
131 | < | */ |
132 | < | Real x() const { |
133 | < | return this->data_[1]; |
134 | < | } |
128 | > | /** |
129 | > | * Returns the value of the thirf element of this quaternion. |
130 | > | * @return the value of the third element of this quaternion |
131 | > | */ |
132 | > | Real y() const { |
133 | > | return this->data_[2]; |
134 | > | } |
135 | ||
136 | < | /** |
137 | < | * Returns the reference of the second element of this quaternion. |
138 | < | * @return the reference of the second element of this quaternion |
139 | < | */ |
140 | < | Real& x() { |
141 | < | return this->data_[1]; |
142 | < | } |
136 | > | /** |
137 | > | * Returns the reference of the third element of this quaternion. |
138 | > | * @return the reference of the third element of this quaternion |
139 | > | */ |
140 | > | Real& y() { |
141 | > | return this->data_[2]; |
142 | > | } |
143 | ||
144 | < | /** |
145 | < | * Returns the value of the thirf element of this quaternion. |
146 | < | * @return the value of the third element of this quaternion |
147 | < | */ |
148 | < | Real y() const { |
149 | < | return this->data_[2]; |
150 | < | } |
144 | > | /** |
145 | > | * Returns the value of the fourth element of this quaternion. |
146 | > | * @return the value of the fourth element of this quaternion |
147 | > | */ |
148 | > | Real z() const { |
149 | > | return this->data_[3]; |
150 | > | } |
151 | > | /** |
152 | > | * Returns the reference of the fourth element of this quaternion. |
153 | > | * @return the reference of the fourth element of this quaternion |
154 | > | */ |
155 | > | Real& z() { |
156 | > | return this->data_[3]; |
157 | > | } |
158 | ||
159 | < | /** |
160 | < | * Returns the reference of the third element of this quaternion. |
161 | < | * @return the reference of the third element of this quaternion |
162 | < | */ |
163 | < | Real& y() { |
164 | < | return this->data_[2]; |
139 | < | } |
159 | > | /** |
160 | > | * Tests if this quaternion is equal to other quaternion |
161 | > | * @return true if equal, otherwise return false |
162 | > | * @param q quaternion to be compared |
163 | > | */ |
164 | > | inline bool operator ==(const Quaternion<Real>& q) { |
165 | ||
166 | < | /** |
167 | < | * Returns the value of the fourth element of this quaternion. |
168 | < | * @return the value of the fourth element of this quaternion |
169 | < | */ |
170 | < | Real z() const { |
146 | < | return this->data_[3]; |
147 | < | } |
148 | < | /** |
149 | < | * Returns the reference of the fourth element of this quaternion. |
150 | < | * @return the reference of the fourth element of this quaternion |
151 | < | */ |
152 | < | Real& z() { |
153 | < | return this->data_[3]; |
154 | < | } |
155 | < | |
156 | < | /** |
157 | < | * Tests if this quaternion is equal to other quaternion |
158 | < | * @return true if equal, otherwise return false |
159 | < | * @param q quaternion to be compared |
160 | < | */ |
161 | < | inline bool operator ==(const Quaternion<Real>& q) { |
162 | < | |
163 | < | for (unsigned int i = 0; i < 4; i ++) { |
164 | < | if (!equal(this->data_[i], q[i])) { |
165 | < | return false; |
166 | < | } |
167 | < | } |
166 | > | for (unsigned int i = 0; i < 4; i ++) { |
167 | > | if (!equal(this->data_[i], q[i])) { |
168 | > | return false; |
169 | > | } |
170 | > | } |
171 | ||
172 | < | return true; |
173 | < | } |
172 | > | return true; |
173 | > | } |
174 | ||
175 | < | /** |
176 | < | * Returns the inverse of this quaternion |
177 | < | * @return inverse |
178 | < | * @note since quaternion is a complex number, the inverse of quaternion |
179 | < | * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2) |
180 | < | */ |
181 | < | Quaternion<Real> inverse() { |
182 | < | Quaternion<Real> q; |
183 | < | Real d = this->lengthSquare(); |
175 | > | /** |
176 | > | * Returns the inverse of this quaternion |
177 | > | * @return inverse |
178 | > | * @note since quaternion is a complex number, the inverse of quaternion |
179 | > | * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2) |
180 | > | */ |
181 | > | Quaternion<Real> inverse() { |
182 | > | Quaternion<Real> q; |
183 | > | Real d = this->lengthSquare(); |
184 | ||
185 | < | q.w() = w() / d; |
186 | < | q.x() = -x() / d; |
187 | < | q.y() = -y() / d; |
188 | < | q.z() = -z() / d; |
185 | > | q.w() = w() / d; |
186 | > | q.x() = -x() / d; |
187 | > | q.y() = -y() / d; |
188 | > | q.z() = -z() / d; |
189 | ||
190 | < | return q; |
191 | < | } |
190 | > | return q; |
191 | > | } |
192 | ||
193 | < | /** |
194 | < | * Sets the value to the multiplication of itself and another quaternion |
195 | < | * @param q the other quaternion |
196 | < | */ |
197 | < | void mul(const Quaternion<Real>& q) { |
198 | < | Quaternion<Real> tmp(*this); |
193 | > | /** |
194 | > | * Sets the value to the multiplication of itself and another quaternion |
195 | > | * @param q the other quaternion |
196 | > | */ |
197 | > | void mul(const Quaternion<Real>& q) { |
198 | > | Quaternion<Real> tmp(*this); |
199 | ||
200 | < | this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]); |
201 | < | this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]); |
202 | < | this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]); |
203 | < | this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]); |
204 | < | } |
200 | > | this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]); |
201 | > | this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]); |
202 | > | this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]); |
203 | > | this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]); |
204 | > | } |
205 | ||
206 | < | void mul(const Real& s) { |
207 | < | this->data_[0] *= s; |
208 | < | this->data_[1] *= s; |
209 | < | this->data_[2] *= s; |
210 | < | this->data_[3] *= s; |
211 | < | } |
206 | > | void mul(const Real& s) { |
207 | > | this->data_[0] *= s; |
208 | > | this->data_[1] *= s; |
209 | > | this->data_[2] *= s; |
210 | > | this->data_[3] *= s; |
211 | > | } |
212 | ||
213 | < | /** Set the value of this quaternion to the division of itself by another quaternion */ |
214 | < | void div(Quaternion<Real>& q) { |
215 | < | mul(q.inverse()); |
216 | < | } |
213 | > | /** Set the value of this quaternion to the division of itself by another quaternion */ |
214 | > | void div(Quaternion<Real>& q) { |
215 | > | mul(q.inverse()); |
216 | > | } |
217 | ||
218 | < | void div(const Real& s) { |
219 | < | this->data_[0] /= s; |
220 | < | this->data_[1] /= s; |
221 | < | this->data_[2] /= s; |
222 | < | this->data_[3] /= s; |
223 | < | } |
218 | > | void div(const Real& s) { |
219 | > | this->data_[0] /= s; |
220 | > | this->data_[1] /= s; |
221 | > | this->data_[2] /= s; |
222 | > | this->data_[3] /= s; |
223 | > | } |
224 | ||
225 | < | Quaternion<Real>& operator *=(const Quaternion<Real>& q) { |
226 | < | mul(q); |
227 | < | return *this; |
228 | < | } |
225 | > | Quaternion<Real>& operator *=(const Quaternion<Real>& q) { |
226 | > | mul(q); |
227 | > | return *this; |
228 | > | } |
229 | ||
230 | < | Quaternion<Real>& operator *=(const Real& s) { |
231 | < | mul(s); |
232 | < | return *this; |
233 | < | } |
230 | > | Quaternion<Real>& operator *=(const Real& s) { |
231 | > | mul(s); |
232 | > | return *this; |
233 | > | } |
234 | ||
235 | < | Quaternion<Real>& operator /=(Quaternion<Real>& q) { |
236 | < | *this *= q.inverse(); |
237 | < | return *this; |
238 | < | } |
235 | > | Quaternion<Real>& operator /=(Quaternion<Real>& q) { |
236 | > | *this *= q.inverse(); |
237 | > | return *this; |
238 | > | } |
239 | ||
240 | < | Quaternion<Real>& operator /=(const Real& s) { |
241 | < | div(s); |
242 | < | return *this; |
243 | < | } |
244 | < | /** |
245 | < | * Returns the conjugate quaternion of this quaternion |
246 | < | * @return the conjugate quaternion of this quaternion |
247 | < | */ |
248 | < | Quaternion<Real> conjugate() { |
249 | < | return Quaternion<Real>(w(), -x(), -y(), -z()); |
250 | < | } |
240 | > | Quaternion<Real>& operator /=(const Real& s) { |
241 | > | div(s); |
242 | > | return *this; |
243 | > | } |
244 | > | /** |
245 | > | * Returns the conjugate quaternion of this quaternion |
246 | > | * @return the conjugate quaternion of this quaternion |
247 | > | */ |
248 | > | Quaternion<Real> conjugate() const { |
249 | > | return Quaternion<Real>(w(), -x(), -y(), -z()); |
250 | > | } |
251 | ||
249 | – | /** |
250 | – | * Returns the corresponding rotation matrix (3x3) |
251 | – | * @return a 3x3 rotation matrix |
252 | – | */ |
253 | – | SquareMatrix<Real, 3> toRotationMatrix3() { |
254 | – | SquareMatrix<Real, 3> rotMat3; |
252 | ||
253 | < | Real w2; |
254 | < | Real x2; |
255 | < | Real y2; |
256 | < | Real z2; |
253 | > | /** |
254 | > | return rotation angle from -PI to PI |
255 | > | */ |
256 | > | inline Real get_rotation_angle() const{ |
257 | > | if( w < (Real)0.0 ) |
258 | > | return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() ); |
259 | > | else |
260 | > | return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ), w() ); |
261 | > | } |
262 | ||
263 | < | if (!this->isNormalized()) |
264 | < | this->normalize(); |
265 | < | |
266 | < | w2 = w() * w(); |
267 | < | x2 = x() * x(); |
268 | < | y2 = y() * y(); |
269 | < | z2 = z() * z(); |
270 | < | |
271 | < | rotMat3(0, 0) = w2 + x2 - y2 - z2; |
272 | < | rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() ); |
273 | < | rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() ); |
274 | < | |
275 | < | rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() ); |
276 | < | rotMat3(1, 1) = w2 - x2 + y2 - z2; |
277 | < | rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() ); |
278 | < | |
279 | < | rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() ); |
280 | < | rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() ); |
281 | < | rotMat3(2, 2) = w2 - x2 -y2 +z2; |
282 | < | |
283 | < | return rotMat3; |
284 | < | } |
285 | < | |
286 | < | };//end Quaternion |
263 | > | /** |
264 | > | create a unit quaternion from axis angle representation |
265 | > | */ |
266 | > | Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis, |
267 | > | const Real& angle){ |
268 | > | Vector3<Real> v(axis); |
269 | > | v.normalize(); |
270 | > | Real half_angle = angle*0.5; |
271 | > | Real sin_a = sin(half_angle); |
272 | > | *this = Quaternion<Real>(cos(half_angle), |
273 | > | v.x()*sin_a, |
274 | > | v.y()*sin_a, |
275 | > | v.z()*sin_a); |
276 | > | return *this; |
277 | > | } |
278 | > | |
279 | > | /** |
280 | > | convert a quaternion to axis angle representation, |
281 | > | preserve the axis direction and angle from -PI to +PI |
282 | > | */ |
283 | > | void toAxisAngle(Vector3<Real>& axis, Real& angle)const { |
284 | > | Real vl = sqrt( x()*x() + y()*y() + z()*z() ); |
285 | > | if( vl > tiny ) { |
286 | > | Real ivl = 1.0/vl; |
287 | > | axis.x() = x() * ivl; |
288 | > | axis.y() = y() * ivl; |
289 | > | axis.z() = z() * ivl; |
290 | ||
291 | + | if( w() < 0 ) |
292 | + | angle = 2.0*atan2(-vl, -w()); //-PI,0 |
293 | + | else |
294 | + | angle = 2.0*atan2( vl, w()); //0,PI |
295 | + | } else { |
296 | + | axis = Vector3<Real>(0.0,0.0,0.0); |
297 | + | angle = 0.0; |
298 | + | } |
299 | + | } |
300 | ||
301 | /** | |
302 | < | * Returns the vaule of scalar multiplication of this quaterion q (q * s). |
303 | < | * @return the vaule of scalar multiplication of this vector |
304 | < | * @param q the source quaternion |
305 | < | * @param s the scalar value |
306 | < | */ |
307 | < | template<typename Real, unsigned int Dim> |
308 | < | Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) { |
309 | < | Quaternion<Real> result(q); |
310 | < | result.mul(s); |
311 | < | return result; |
302 | > | shortest arc quaternion rotate one vector to another by shortest path. |
303 | > | create rotation from -> to, for any length vectors. |
304 | > | */ |
305 | > | Quaternion<Real> fromShortestArc(const Vector3d& from, |
306 | > | const Vector3d& to ) { |
307 | > | |
308 | > | Vector3d c( cross(from,to) ); |
309 | > | *this = Quaternion<Real>(dot(from,to), |
310 | > | c.x(), |
311 | > | c.y(), |
312 | > | c.z()); |
313 | > | |
314 | > | this->normalize(); // if "from" or "to" not unit, normalize quat |
315 | > | w += 1.0f; // reducing angle to halfangle |
316 | > | if( w <= 1e-6 ) { // angle close to PI |
317 | > | if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) { |
318 | > | this->data_[0] = w; |
319 | > | this->data_[1] = 0.0; //cross(from , Vector3d(1,0,0)) |
320 | > | this->data_[2] = from.z(); |
321 | > | this->data_[3] = -from.y(); |
322 | > | } else { |
323 | > | this->data_[0] = w; |
324 | > | this->data_[1] = from.y(); //cross(from, Vector3d(0,0,1)) |
325 | > | this->data_[2] = -from.x(); |
326 | > | this->data_[3] = 0.0; |
327 | > | } |
328 | > | } |
329 | > | this->normalize(); |
330 | } | |
299 | – | |
300 | – | /** |
301 | – | * Returns the vaule of scalar multiplication of this quaterion q (q * s). |
302 | – | * @return the vaule of scalar multiplication of this vector |
303 | – | * @param s the scalar value |
304 | – | * @param q the source quaternion |
305 | – | */ |
306 | – | template<typename Real, unsigned int Dim> |
307 | – | Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) { |
308 | – | Quaternion<Real> result(q); |
309 | – | result.mul(s); |
310 | – | return result; |
311 | – | } |
331 | ||
332 | < | /** |
333 | < | * Returns the multiplication of two quaternion |
315 | < | * @return the multiplication of two quaternion |
316 | < | * @param q1 the first quaternion |
317 | < | * @param q2 the second quaternion |
318 | < | */ |
319 | < | template<typename Real> |
320 | < | inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) { |
321 | < | Quaternion<Real> result(q1); |
322 | < | result *= q2; |
323 | < | return result; |
332 | > | Real ComputeTwist(const Quaternion& q) { |
333 | > | return (Real)2.0 * atan2(q.z(), q.w()); |
334 | } | |
335 | ||
336 | + | void RemoveTwist(Quaternion& q) { |
337 | + | Real t = ComputeTwist(q); |
338 | + | Quaternion rt = fromAxisAngle(V3Z, t); |
339 | + | |
340 | + | q *= rt.inverse(); |
341 | + | } |
342 | + | |
343 | + | void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle, |
344 | + | Vector3<Real>& swingAxis) { |
345 | + | |
346 | + | twistAngle = (Real)2.0 * atan2(z(), w()); |
347 | + | Quaternion rt, rs; |
348 | + | rt.fromAxisAngle(V3Z, twistAngle); |
349 | + | rs = *this * rt.inverse(); |
350 | + | |
351 | + | Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() ); |
352 | + | if( vl > tiny ) { |
353 | + | Real ivl = 1.0 / vl; |
354 | + | swingAxis.x() = rs.x() * ivl; |
355 | + | swingAxis.y() = rs.y() * ivl; |
356 | + | swingAxis.z() = rs.z() * ivl; |
357 | + | |
358 | + | if( rs.w() < 0.0 ) |
359 | + | swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0 |
360 | + | else |
361 | + | swingAngle = 2.0*atan2( vl, rs.w()); //0,PI |
362 | + | } else { |
363 | + | swingAxis = Vector3<Real>(1.0,0.0,0.0); |
364 | + | swingAngle = 0.0; |
365 | + | } |
366 | + | } |
367 | + | |
368 | + | |
369 | + | Vector3<Real> rotate(const Vector3<Real>& v) { |
370 | + | |
371 | + | Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(), |
372 | + | v.y() * w() + v.x() * z() - v.z() * x(), |
373 | + | v.z() * w() + v.y() * x() - v.x() * y(), |
374 | + | v.x() * x() + v.y() * y() + v.z() * z()); |
375 | + | |
376 | + | return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(), |
377 | + | w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(), |
378 | + | w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())* |
379 | + | ( 1.0/this->lengthSquare() ); |
380 | + | } |
381 | + | |
382 | + | Quaternion<Real>& align (const Vector3<Real>& V1, |
383 | + | const Vector3<Real>& V2) { |
384 | + | |
385 | + | // If V1 and V2 are not parallel, the axis of rotation is the unit-length |
386 | + | // vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation, |
387 | + | // A, is the angle between V1 and V2. The quaternion for the rotation is |
388 | + | // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz). |
389 | + | // |
390 | + | // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then |
391 | + | // compute sin(A/2) and cos(A/2), we reduce the computational costs |
392 | + | // by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) = |
393 | + | // Dot(V1,B). |
394 | + | // |
395 | + | // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but |
396 | + | // Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in |
397 | + | // which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where |
398 | + | // C = Cross(V1,B). |
399 | + | // |
400 | + | // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2, |
401 | + | // then B = 0. This can happen even if V1 is approximately -V2 using |
402 | + | // floating point arithmetic, since Vector3::Normalize checks for |
403 | + | // closeness to zero and returns the zero vector accordingly. The test |
404 | + | // for exactly zero is usually not recommend for floating point |
405 | + | // arithmetic, but the implementation of Vector3::Normalize guarantees |
406 | + | // the comparison is robust. In this case, the A = pi and any axis |
407 | + | // perpendicular to V1 may be used as the rotation axis. |
408 | + | |
409 | + | Vector3<Real> Bisector = V1 + V2; |
410 | + | Bisector.normalize(); |
411 | + | |
412 | + | Real CosHalfAngle = dot(V1,Bisector); |
413 | + | |
414 | + | this->data_[0] = CosHalfAngle; |
415 | + | |
416 | + | if (CosHalfAngle != (Real)0.0) { |
417 | + | Vector3<Real> Cross = cross(V1, Bisector); |
418 | + | this->data_[1] = Cross.x(); |
419 | + | this->data_[2] = Cross.y(); |
420 | + | this->data_[3] = Cross.z(); |
421 | + | } else { |
422 | + | Real InvLength; |
423 | + | if (fabs(V1[0]) >= fabs(V1[1])) { |
424 | + | // V1.x or V1.z is the largest magnitude component |
425 | + | InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]); |
426 | + | |
427 | + | this->data_[1] = -V1[2]*InvLength; |
428 | + | this->data_[2] = (Real)0.0; |
429 | + | this->data_[3] = +V1[0]*InvLength; |
430 | + | } else { |
431 | + | // V1.y or V1.z is the largest magnitude component |
432 | + | InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]); |
433 | + | |
434 | + | this->data_[1] = (Real)0.0; |
435 | + | this->data_[2] = +V1[2]*InvLength; |
436 | + | this->data_[3] = -V1[1]*InvLength; |
437 | + | } |
438 | + | } |
439 | + | return *this; |
440 | + | } |
441 | + | |
442 | + | void toTwistSwing ( Real& tw, Real& sx, Real& sy ) { |
443 | + | |
444 | + | // First test if the swing is in the singularity: |
445 | + | |
446 | + | if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; } |
447 | + | |
448 | + | // Decompose into twist-swing by solving the equation: |
449 | + | // |
450 | + | // Qtwist(t*2) * Qswing(s*2) = q |
451 | + | // |
452 | + | // note: (x,y) is the normalized swing axis (x*x+y*y=1) |
453 | + | // |
454 | + | // ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz ) |
455 | + | // ( CtCs xSsCt-yStSs xStSs+ySsCt StCs ) = ( qw qx qy qz ) (1) |
456 | + | // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2) |
457 | + | // |
458 | + | // The swing rotation/2 s comes from: |
459 | + | // |
460 | + | // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 => |
461 | + | // Cs = sqrt ( qw^2 + qz^2 ) (3) |
462 | + | // |
463 | + | // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 => |
464 | + | // Ss = sqrt ( qx^2 + qy^2 ) (4) |
465 | + | // From (1): |SsCt -StSs| |x| = |qx| |
466 | + | // |StSs +SsCt| |y| |qy| (5) |
467 | + | |
468 | + | Real qw, qx, qy, qz; |
469 | + | |
470 | + | if ( w()<0 ) { |
471 | + | qw=-w(); |
472 | + | qx=-x(); |
473 | + | qy=-y(); |
474 | + | qz=-z(); |
475 | + | } else { |
476 | + | qw=w(); |
477 | + | qx=x(); |
478 | + | qy=y(); |
479 | + | qz=z(); |
480 | + | } |
481 | + | |
482 | + | Real t = atan2 ( qz, qw ); // from (2) |
483 | + | Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3) |
484 | + | // and (4) |
485 | + | |
486 | + | Real x=0.0, y=0.0, sins=sin(s); |
487 | + | |
488 | + | if ( !ISZERO(sins,tiny) ) { |
489 | + | Real sint = sin(t); |
490 | + | Real cost = cos(t); |
491 | + | |
492 | + | // by solving the linear system in (5): |
493 | + | y = (-qx*sint + qy*cost)/sins; |
494 | + | x = ( qx*cost + qy*sint)/sins; |
495 | + | } |
496 | + | |
497 | + | tw = (Real)2.0*t; |
498 | + | sx = (Real)2.0*x*s; |
499 | + | sy = (Real)2.0*y*s; |
500 | + | } |
501 | + | |
502 | + | void toSwingTwist(Real& sx, Real& sy, Real& tw ) { |
503 | + | |
504 | + | // Decompose q into swing-twist using a similar development as |
505 | + | // in function toTwistSwing |
506 | + | |
507 | + | if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; } |
508 | + | |
509 | + | Real qw, qx, qy, qz; |
510 | + | if ( w() < 0 ){ |
511 | + | qw=-w(); |
512 | + | qx=-x(); |
513 | + | qy=-y(); |
514 | + | qz=-z(); |
515 | + | } else { |
516 | + | qw=w(); |
517 | + | qx=x(); |
518 | + | qy=y(); |
519 | + | qz=z(); |
520 | + | } |
521 | + | |
522 | + | // Get the twist t: |
523 | + | Real t = 2.0 * atan2(qz,qw); |
524 | + | |
525 | + | Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); |
526 | + | Real gam = t/2.0; |
527 | + | Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet; |
528 | + | Real singam = sin(gam); |
529 | + | Real cosgam = cos(gam); |
530 | + | |
531 | + | sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) ); |
532 | + | sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) ); |
533 | + | tw = Real( t ); |
534 | + | } |
535 | + | |
536 | + | |
537 | + | |
538 | /** | |
539 | < | * Returns the division of two quaternion |
540 | < | * @param q1 divisor |
329 | < | * @param q2 dividen |
539 | > | * Returns the corresponding rotation matrix (3x3) |
540 | > | * @return a 3x3 rotation matrix |
541 | */ | |
542 | + | SquareMatrix<Real, 3> toRotationMatrix3() { |
543 | + | SquareMatrix<Real, 3> rotMat3; |
544 | + | |
545 | + | Real w2; |
546 | + | Real x2; |
547 | + | Real y2; |
548 | + | Real z2; |
549 | ||
550 | < | template<typename Real> |
551 | < | inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) { |
552 | < | return q1 * q2.inverse(); |
550 | > | if (!this->isNormalized()) |
551 | > | this->normalize(); |
552 | > | |
553 | > | w2 = w() * w(); |
554 | > | x2 = x() * x(); |
555 | > | y2 = y() * y(); |
556 | > | z2 = z() * z(); |
557 | > | |
558 | > | rotMat3(0, 0) = w2 + x2 - y2 - z2; |
559 | > | rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() ); |
560 | > | rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() ); |
561 | > | |
562 | > | rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() ); |
563 | > | rotMat3(1, 1) = w2 - x2 + y2 - z2; |
564 | > | rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() ); |
565 | > | |
566 | > | rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() ); |
567 | > | rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() ); |
568 | > | rotMat3(2, 2) = w2 - x2 -y2 +z2; |
569 | > | |
570 | > | return rotMat3; |
571 | } | |
572 | ||
573 | + | };//end Quaternion |
574 | + | |
575 | + | |
576 | /** | |
577 | < | * Returns the value of the division of a scalar by a quaternion |
578 | < | * @return the value of the division of a scalar by a quaternion |
579 | < | * @param s scalar |
580 | < | * @param q quaternion |
342 | < | * @note for a quaternion q, 1/q = q.inverse() |
577 | > | * Returns the vaule of scalar multiplication of this quaterion q (q * s). |
578 | > | * @return the vaule of scalar multiplication of this vector |
579 | > | * @param q the source quaternion |
580 | > | * @param s the scalar value |
581 | */ | |
582 | < | template<typename Real> |
583 | < | Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) { |
582 | > | template<typename Real, unsigned int Dim> |
583 | > | Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) { |
584 | > | Quaternion<Real> result(q); |
585 | > | result.mul(s); |
586 | > | return result; |
587 | > | } |
588 | > | |
589 | > | /** |
590 | > | * Returns the vaule of scalar multiplication of this quaterion q (q * s). |
591 | > | * @return the vaule of scalar multiplication of this vector |
592 | > | * @param s the scalar value |
593 | > | * @param q the source quaternion |
594 | > | */ |
595 | > | template<typename Real, unsigned int Dim> |
596 | > | Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) { |
597 | > | Quaternion<Real> result(q); |
598 | > | result.mul(s); |
599 | > | return result; |
600 | > | } |
601 | ||
602 | < | Quaternion<Real> x; |
603 | < | x = q.inverse(); |
604 | < | x *= s; |
605 | < | return x; |
606 | < | } |
602 | > | /** |
603 | > | * Returns the multiplication of two quaternion |
604 | > | * @return the multiplication of two quaternion |
605 | > | * @param q1 the first quaternion |
606 | > | * @param q2 the second quaternion |
607 | > | */ |
608 | > | template<typename Real> |
609 | > | inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) { |
610 | > | Quaternion<Real> result(q1); |
611 | > | result *= q2; |
612 | > | return result; |
613 | > | } |
614 | > | |
615 | > | /** |
616 | > | * Returns the division of two quaternion |
617 | > | * @param q1 divisor |
618 | > | * @param q2 dividen |
619 | > | */ |
620 | > | |
621 | > | template<typename Real> |
622 | > | inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) { |
623 | > | return q1 * q2.inverse(); |
624 | > | } |
625 | > | |
626 | > | /** |
627 | > | * Returns the value of the division of a scalar by a quaternion |
628 | > | * @return the value of the division of a scalar by a quaternion |
629 | > | * @param s scalar |
630 | > | * @param q quaternion |
631 | > | * @note for a quaternion q, 1/q = q.inverse() |
632 | > | */ |
633 | > | template<typename Real> |
634 | > | Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) { |
635 | > | |
636 | > | Quaternion<Real> x; |
637 | > | x = q.inverse(); |
638 | > | x *= s; |
639 | > | return x; |
640 | > | } |
641 | ||
642 | < | template <class T> |
643 | < | inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) { |
644 | < | return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]); |
645 | < | } |
642 | > | template <class T> |
643 | > | inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) { |
644 | > | return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]); |
645 | > | } |
646 | ||
647 | < | typedef Quaternion<double> Quat4d; |
647 | > | typedef Quaternion<RealType> Quat4d; |
648 | } | |
649 | #endif //MATH_QUATERNION_HPP |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |