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