b311480e |
1 | // Created on: 2010-05-11 |
2 | // Created by: Kirill GAVRILOV |
973c2be1 |
3 | // Copyright (c) 2010-2014 OPEN CASCADE SAS |
7fd59977 |
4 | // |
973c2be1 |
5 | // This file is part of Open CASCADE Technology software library. |
b311480e |
6 | // |
d5f74e42 |
7 | // This library is free software; you can redistribute it and/or modify it under |
8 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
9 | // by the Free Software Foundation, with special exception defined in the file |
10 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
11 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
12 | // |
973c2be1 |
13 | // Alternatively, this file may be used under the terms of Open CASCADE |
14 | // commercial license or contractual agreement. |
b311480e |
15 | |
7fd59977 |
16 | // Note: implementation is based on free samples from |
17 | // http://www.gamedev.ru/code/articles/?id=4215&page=3 |
18 | // and maths found in Wikipedia and elsewhere |
19 | |
20 | #include <gp_Quaternion.hxx> |
21 | #include <gp_Vec.hxx> |
22 | #include <gp_Mat.hxx> |
23 | |
24 | //======================================================================= |
25 | //function : IsEqual |
26 | //purpose : |
27 | //======================================================================= |
28 | |
29 | Standard_Boolean gp_Quaternion::IsEqual (const gp_Quaternion& theOther) const |
30 | { |
31 | if (this == &theOther) |
32 | return Standard_True; |
33 | return Abs (x - theOther.x) <= gp::Resolution() && |
34 | Abs (y - theOther.y) <= gp::Resolution() && |
35 | Abs (z - theOther.z) <= gp::Resolution() && |
36 | Abs (w - theOther.w) <= gp::Resolution(); |
37 | } |
38 | |
39 | //======================================================================= |
40 | //function : SetRotation |
41 | //purpose : |
42 | //======================================================================= |
43 | |
44 | void gp_Quaternion::SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo) |
45 | { |
46 | gp_Vec aVecCross (theVecFrom.Crossed (theVecTo)); |
47 | Set (aVecCross.X(), aVecCross.Y(), aVecCross.Z(), theVecFrom.Dot (theVecTo)); |
48 | Normalize(); // if "from" or "to" not unit, normalize quat |
49 | w += 1.0; // reducing angle to halfangle |
50 | if (w <= gp::Resolution()) // angle close to PI |
51 | { |
52 | if ((theVecFrom.Z() * theVecFrom.Z()) > (theVecFrom.X() * theVecFrom.X())) |
53 | Set ( 0.0, theVecFrom.Z(), -theVecFrom.Y(), w); // theVecFrom * gp_Vec(1,0,0) |
54 | else |
55 | Set (theVecFrom.Y(), -theVecFrom.X(), 0.0, w); // theVecFrom * gp_Vec(0,0,1) |
56 | } |
57 | Normalize(); |
58 | } |
59 | |
60 | //======================================================================= |
61 | //function : SetRotation |
62 | //purpose : |
63 | //======================================================================= |
64 | |
65 | void gp_Quaternion::SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec) |
66 | { |
67 | gp_Vec aVecCross (theVecFrom.Crossed (theVecTo)); |
68 | Set (aVecCross.X(), aVecCross.Y(), aVecCross.Z(), theVecFrom.Dot (theVecTo)); |
69 | Normalize(); // if "from" or "to" not unit, normalize quat |
70 | w += 1.0; // reducing angle to halfangle |
71 | if (w <= gp::Resolution()) // angle close to PI |
72 | { |
73 | gp_Vec theAxis = theVecFrom.Crossed (theHelpCrossVec); |
74 | Set (theAxis.X(), theAxis.Y(), theAxis.Z(), w); |
75 | } |
76 | Normalize(); |
77 | } |
78 | |
79 | //======================================================================= |
80 | //function : SetVectorAndAngle |
81 | //purpose : |
82 | //======================================================================= |
83 | |
84 | void gp_Quaternion::SetVectorAndAngle (const gp_Vec& theAxis, const Standard_Real theAngle) |
85 | { |
86 | gp_Vec anAxis = theAxis.Normalized(); |
87 | Standard_Real anAngleHalf = 0.5 * theAngle; |
88 | Standard_Real sin_a = Sin (anAngleHalf); |
89 | Set (anAxis.X() * sin_a, anAxis.Y() * sin_a, anAxis.Z() * sin_a, Cos (anAngleHalf)); |
90 | } |
91 | |
92 | //======================================================================= |
93 | //function : GetVectorAndAngle |
94 | //purpose : |
95 | //======================================================================= |
96 | |
97 | void gp_Quaternion::GetVectorAndAngle (gp_Vec& theAxis, Standard_Real& theAngle) const |
98 | { |
99 | Standard_Real vl = Sqrt (x * x + y * y + z * z); |
100 | if (vl > gp::Resolution()) |
101 | { |
102 | Standard_Real ivl = 1.0 / vl; |
103 | theAxis.SetCoord (x * ivl, y * ivl, z * ivl); |
104 | if (w < 0.0) |
105 | { |
106 | theAngle = 2.0 * ATan2 (-vl, -w); // [-PI, 0] |
107 | } |
108 | else |
109 | { |
110 | theAngle = 2.0 * ATan2 ( vl, w); // [ 0, PI] |
111 | } |
112 | } |
113 | else |
114 | { |
115 | theAxis.SetCoord (0.0, 0.0, 1.0); |
116 | theAngle = 0.0; |
117 | } |
118 | } |
119 | |
120 | //======================================================================= |
121 | //function : SetMatrix |
122 | //purpose : |
123 | //======================================================================= |
124 | |
125 | void gp_Quaternion::SetMatrix (const gp_Mat& theMat) |
126 | { |
127 | Standard_Real tr = theMat (1, 1) + theMat (2, 2) + theMat(3, 3); // trace of martix |
128 | if (tr > 0.0) |
129 | { // if trace positive than "w" is biggest component |
130 | Set (theMat (3, 2) - theMat (2, 3), |
131 | theMat (1, 3) - theMat (3, 1), |
132 | theMat (2, 1) - theMat (1, 2), |
133 | tr + 1.0); |
134 | Scale (0.5 / Sqrt (w)); // "w" contain the "norm * 4" |
135 | } |
136 | else if ((theMat (1, 1) > theMat (2, 2)) && (theMat (1, 1) > theMat (3, 3))) |
137 | { // Some of vector components is bigger |
138 | Set (1.0 + theMat (1, 1) - theMat (2, 2) - theMat (3, 3), |
139 | theMat (1, 2) + theMat (2, 1), |
140 | theMat (1, 3) + theMat (3, 1), |
141 | theMat (3, 2) - theMat (2, 3)); |
142 | Scale (0.5 / Sqrt (x)); |
143 | } |
144 | else if (theMat (2, 2) > theMat (3, 3)) |
145 | { |
146 | Set (theMat (1, 2) + theMat (2, 1), |
147 | 1.0 + theMat (2, 2) - theMat (1, 1) - theMat (3, 3), |
148 | theMat (2, 3) + theMat (3, 2), |
149 | theMat (1, 3) - theMat (3, 1)); |
150 | Scale (0.5 / Sqrt (y)); |
151 | } |
152 | else |
153 | { |
154 | Set (theMat (1, 3) + theMat (3, 1), |
155 | theMat (2, 3) + theMat (3, 2), |
156 | 1.0 + theMat (3, 3) - theMat (1, 1) - theMat (2, 2), |
157 | theMat (2, 1) - theMat (1, 2)); |
158 | Scale (0.5 / Sqrt (z)); |
159 | } |
160 | } |
161 | |
162 | //======================================================================= |
163 | //function : GetMatrix |
164 | //purpose : |
165 | //======================================================================= |
166 | |
167 | gp_Mat gp_Quaternion::GetMatrix () const |
168 | { |
169 | Standard_Real wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; |
170 | Standard_Real s = 2.0 / SquareNorm(); |
171 | x2 = x * s; y2 = y * s; z2 = z * s; |
172 | xx = x * x2; xy = x * y2; xz = x * z2; |
173 | yy = y * y2; yz = y * z2; zz = z * z2; |
174 | wx = w * x2; wy = w * y2; wz = w * z2; |
175 | |
176 | gp_Mat aMat; |
177 | |
178 | aMat (1, 1) = 1.0 - (yy + zz); |
179 | aMat (1, 2) = xy - wz; |
180 | aMat (1, 3) = xz + wy; |
181 | |
182 | aMat (2, 1) = xy + wz; |
183 | aMat (2, 2) = 1.0 - (xx + zz); |
184 | aMat (2, 3) = yz - wx; |
185 | |
186 | aMat (3, 1) = xz - wy; |
187 | aMat (3, 2) = yz + wx; |
188 | aMat (3, 3) = 1.0 - (xx + yy); |
189 | // 1 division 16 multiplications 15 addidtions 12 variables |
190 | |
191 | return aMat; |
192 | } |
193 | |
4f5ad416 |
194 | namespace { // anonymous namespace |
195 | |
7fd59977 |
196 | //======================================================================= |
197 | //function : translateEulerSequence |
198 | //purpose : |
199 | // Code supporting conversion between quaternion and generalized |
200 | // Euler angles (sequence of three rotations) is based on |
201 | // algorithm by Ken Shoemake, published in Graphics Gems IV, p. 222-22 |
202 | // http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c |
203 | //======================================================================= |
204 | |
205 | struct gp_EulerSequence_Parameters |
206 | { |
207 | Standard_Integer i; // first rotation axis |
208 | Standard_Integer j; // next axis of rotation |
209 | Standard_Integer k; // third axis |
210 | Standard_Boolean isOdd; // true if order of two first rotation axes is odd permutation, e.g. XZ |
211 | Standard_Boolean isTwoAxes; // true if third rotation is about the same axis as first |
212 | Standard_Boolean isExtrinsic; // true if rotations are made around fixed axes |
213 | |
214 | gp_EulerSequence_Parameters (Standard_Integer theAx1, |
215 | Standard_Boolean theisOdd, |
216 | Standard_Boolean theisTwoAxes, |
217 | Standard_Boolean theisExtrinsic) |
218 | : i(theAx1), |
219 | j(1 + (theAx1 + (theisOdd ? 1 : 0)) % 3), |
220 | k(1 + (theAx1 + (theisOdd ? 0 : 1)) % 3), |
221 | isOdd(theisOdd), |
222 | isTwoAxes(theisTwoAxes), |
223 | isExtrinsic(theisExtrinsic) |
224 | {} |
225 | }; |
226 | |
227 | gp_EulerSequence_Parameters translateEulerSequence (const gp_EulerSequence theSeq) |
228 | { |
229 | typedef gp_EulerSequence_Parameters Params; |
230 | const Standard_Boolean F = Standard_False; |
231 | const Standard_Boolean T = Standard_True; |
232 | |
233 | switch (theSeq) |
234 | { |
235 | case gp_Extrinsic_XYZ: return Params (1, F, F, T); |
236 | case gp_Extrinsic_XZY: return Params (1, T, F, T); |
237 | case gp_Extrinsic_YZX: return Params (2, F, F, T); |
238 | case gp_Extrinsic_YXZ: return Params (2, T, F, T); |
239 | case gp_Extrinsic_ZXY: return Params (3, F, F, T); |
240 | case gp_Extrinsic_ZYX: return Params (3, T, F, T); |
241 | |
4f5ad416 |
242 | // Conversion of intrinsic angles is made by the same code as for extrinsic, |
243 | // using equivalence rule: intrinsic rotation is equivalent to extrinsic |
244 | // rotation by the same angles but with inverted order of elemental rotations. |
245 | // Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure; |
246 | // sequence of axes is inverted by setting appropriate parameters here. |
247 | // Note that proper Euler angles (last block below) are symmetric for sequence of axes. |
248 | case gp_Intrinsic_XYZ: return Params (3, T, F, F); |
249 | case gp_Intrinsic_XZY: return Params (2, F, F, F); |
250 | case gp_Intrinsic_YZX: return Params (1, T, F, F); |
251 | case gp_Intrinsic_YXZ: return Params (3, F, F, F); |
252 | case gp_Intrinsic_ZXY: return Params (2, T, F, F); |
253 | case gp_Intrinsic_ZYX: return Params (1, F, F, F); |
7fd59977 |
254 | |
255 | case gp_Extrinsic_XYX: return Params (1, F, T, T); |
256 | case gp_Extrinsic_XZX: return Params (1, T, T, T); |
257 | case gp_Extrinsic_YZY: return Params (2, F, T, T); |
258 | case gp_Extrinsic_YXY: return Params (2, T, T, T); |
259 | case gp_Extrinsic_ZXZ: return Params (3, F, T, T); |
260 | case gp_Extrinsic_ZYZ: return Params (3, T, T, T); |
261 | |
262 | case gp_Intrinsic_XYX: return Params (1, F, T, F); |
263 | case gp_Intrinsic_XZX: return Params (1, T, T, F); |
264 | case gp_Intrinsic_YZY: return Params (2, F, T, F); |
265 | case gp_Intrinsic_YXY: return Params (2, T, T, F); |
266 | case gp_Intrinsic_ZXZ: return Params (3, F, T, F); |
267 | case gp_Intrinsic_ZYZ: return Params (3, T, T, F); |
268 | |
269 | default: |
270 | case gp_EulerAngles : return Params (3, F, T, F); // = Intrinsic_ZXZ |
4f5ad416 |
271 | case gp_YawPitchRoll: return Params (1, F, F, F); // = Intrinsic_ZYX |
7fd59977 |
272 | }; |
273 | } |
274 | |
4f5ad416 |
275 | } // anonymous namespace |
276 | |
7fd59977 |
277 | //======================================================================= |
278 | //function : SetEulerAngles |
279 | //purpose : |
280 | //======================================================================= |
281 | |
282 | void gp_Quaternion::SetEulerAngles (const gp_EulerSequence theOrder, |
283 | const Standard_Real theAlpha, |
284 | const Standard_Real theBeta, |
285 | const Standard_Real theGamma) |
286 | { |
287 | gp_EulerSequence_Parameters o = translateEulerSequence (theOrder); |
288 | |
289 | Standard_Real a = theAlpha, b = theBeta, c = theGamma; |
290 | if ( ! o.isExtrinsic ) |
291 | { |
292 | a = theGamma; |
293 | c = theAlpha; |
294 | } |
295 | if ( o.isOdd ) |
296 | b = -b; |
297 | |
298 | Standard_Real ti = 0.5 * a; |
299 | Standard_Real tj = 0.5 * b; |
300 | Standard_Real th = 0.5 * c; |
301 | Standard_Real ci = Cos (ti); |
302 | Standard_Real cj = Cos (tj); |
303 | Standard_Real ch = Cos (th); |
304 | Standard_Real si = Sin (ti); |
305 | Standard_Real sj = Sin (tj); |
306 | Standard_Real sh = Sin (th); |
307 | Standard_Real cc = ci * ch; |
308 | Standard_Real cs = ci * sh; |
309 | Standard_Real sc = si * ch; |
310 | Standard_Real ss = si * sh; |
311 | |
312 | Standard_Real values[4]; // w, x, y, z |
313 | if ( o.isTwoAxes ) |
314 | { |
315 | values[o.i] = cj * (cs + sc); |
316 | values[o.j] = sj * (cc + ss); |
317 | values[o.k] = sj * (cs - sc); |
318 | values[0] = cj * (cc - ss); |
319 | } |
320 | else |
321 | { |
322 | values[o.i] = cj * sc - sj * cs; |
323 | values[o.j] = cj * ss + sj * cc; |
324 | values[o.k] = cj * cs - sj * sc; |
325 | values[0] = cj * cc + sj * ss; |
326 | } |
327 | if ( o.isOdd ) |
328 | values[o.j] = -values[o.j]; |
329 | |
330 | x = values[1]; |
331 | y = values[2]; |
332 | z = values[3]; |
333 | w = values[0]; |
334 | } |
335 | |
336 | //======================================================================= |
337 | //function : GetEulerAngles |
338 | //purpose : |
339 | //======================================================================= |
340 | |
341 | void gp_Quaternion::GetEulerAngles (const gp_EulerSequence theOrder, |
342 | Standard_Real& theAlpha, |
343 | Standard_Real& theBeta, |
344 | Standard_Real& theGamma) const |
345 | { |
346 | gp_Mat M = GetMatrix(); |
347 | |
348 | gp_EulerSequence_Parameters o = translateEulerSequence (theOrder); |
349 | if ( o.isTwoAxes ) |
350 | { |
351 | double sy = sqrt (M(o.i, o.j) * M(o.i, o.j) + M(o.i, o.k) * M(o.i, o.k)); |
352 | if (sy > 16 * DBL_EPSILON) |
353 | { |
354 | theAlpha = ATan2 (M(o.i, o.j), M(o.i, o.k)); |
355 | theGamma = ATan2 (M(o.j, o.i), -M(o.k, o.i)); |
356 | } |
357 | else |
358 | { |
359 | theAlpha = ATan2 (-M(o.j, o.k), M(o.j, o.j)); |
360 | theGamma = 0.; |
361 | } |
362 | theBeta = ATan2 (sy, M(o.i, o.i)); |
363 | } |
364 | else |
365 | { |
366 | double cy = sqrt (M(o.i, o.i) * M(o.i, o.i) + M(o.j, o.i) * M(o.j, o.i)); |
367 | if (cy > 16 * DBL_EPSILON) |
368 | { |
369 | theAlpha = ATan2 (M(o.k, o.j), M(o.k, o.k)); |
370 | theGamma = ATan2 (M(o.j, o.i), M(o.i, o.i)); |
371 | } |
372 | else |
373 | { |
374 | theAlpha = ATan2 (-M(o.j, o.k), M(o.j, o.j)); |
375 | theGamma = 0.; |
376 | } |
377 | theBeta = ATan2 (-M(o.k, o.i), cy); |
378 | } |
379 | if ( o.isOdd ) |
380 | { |
381 | theAlpha = -theAlpha; |
382 | theBeta = -theBeta; |
383 | theGamma = -theGamma; |
384 | } |
385 | if ( ! o.isExtrinsic ) |
386 | { |
387 | Standard_Real aFirst = theAlpha; |
388 | theAlpha = theGamma; |
389 | theGamma = aFirst; |
390 | } |
391 | } |
392 | |
393 | //======================================================================= |
394 | //function : StabilizeLength |
395 | //purpose : |
396 | //======================================================================= |
397 | |
398 | void gp_Quaternion::StabilizeLength() |
399 | { |
400 | Standard_Real cs = Abs (x) + Abs (y) + Abs (z) + Abs (w); |
401 | if (cs > 0.0) |
402 | { |
403 | x /= cs; y /= cs; z /= cs; w /= cs; |
404 | } |
405 | else |
406 | { |
407 | SetIdent(); |
408 | } |
409 | } |
410 | |
411 | //======================================================================= |
412 | //function : Normalize |
413 | //purpose : |
414 | //======================================================================= |
415 | |
416 | void gp_Quaternion::Normalize() |
417 | { |
418 | Standard_Real aMagn = Norm(); |
419 | if (aMagn < gp::Resolution()) |
420 | { |
421 | StabilizeLength(); |
422 | aMagn = Norm(); |
423 | } |
424 | Scale (1.0 / aMagn); |
425 | } |
426 | |
427 | //======================================================================= |
428 | //function : Normalize |
429 | //purpose : |
430 | //======================================================================= |
431 | |
432 | Standard_Real gp_Quaternion::GetRotationAngle() const |
433 | { |
434 | if (w < 0.0) |
435 | { |
436 | return 2.0 * ATan2 (-Sqrt (x * x + y * y + z * z), -w); |
437 | } |
438 | else |
439 | { |
440 | return 2.0 * ATan2 ( Sqrt (x * x + y * y + z * z), w); |
441 | } |
442 | } |
443 | |
444 | //======================================================================= |
445 | //function : Multiply |
446 | //purpose : |
447 | //======================================================================= |
448 | |
449 | gp_Vec gp_Quaternion::Multiply (const gp_Vec& theVec) const |
450 | { |
451 | gp_Quaternion theQ (theVec.X() * w + theVec.Z() * y - theVec.Y() * z, |
452 | theVec.Y() * w + theVec.X() * z - theVec.Z() * x, |
453 | theVec.Z() * w + theVec.Y() * x - theVec.X() * y, |
454 | theVec.X() * x + theVec.Y() * y + theVec.Z() * z); |
455 | return gp_Vec (w * theQ.x + x * theQ.w + y * theQ.z - z * theQ.y, |
456 | w * theQ.y + y * theQ.w + z * theQ.x - x * theQ.z, |
457 | w * theQ.z + z * theQ.w + x * theQ.y - y * theQ.x) * (1.0 / SquareNorm()); |
458 | } |