0032137: Coding Rules - merge redundant .lxx files into header files within Package gp
[occt.git] / src / gp / gp_Quaternion.hxx
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 #ifndef _gp_Quaternion_HeaderFile
17 #define _gp_Quaternion_HeaderFile
18
19 #include <gp_EulerSequence.hxx>
20 #include <gp_Mat.hxx>
21 #include <gp_Vec.hxx>
22
23 //! Represents operation of rotation in 3d space as quaternion
24 //! and implements operations with rotations basing on
25 //! quaternion mathematics.
26 //!
27 //! In addition, provides methods for conversion to and from other
28 //! representations of rotation (3*3 matrix, vector and
29 //! angle, Euler angles)
30 class gp_Quaternion 
31 {
32 public:
33
34   DEFINE_STANDARD_ALLOC
35
36   //! Creates an identity quaternion
37   gp_Quaternion()
38   : x (0.0),
39     y (0.0),
40     z (0.0),
41     w (1.0)
42   {}
43
44   //! Creates quaternion directly from component values
45   gp_Quaternion (const Standard_Real theX, const Standard_Real theY, const Standard_Real theZ, const Standard_Real theW)
46   : x (theX),
47     y (theY),
48     z (theZ),
49     w (theW)
50   {}
51
52   //! Creates quaternion representing shortest-arc rotation
53   //! operator producing vector theVecTo from vector theVecFrom.
54   gp_Quaternion (const gp_Vec& theVecFrom, const gp_Vec& theVecTo)
55   {
56     SetRotation (theVecFrom, theVecTo);
57   }
58
59   //! Creates quaternion representing shortest-arc rotation
60   //! operator producing vector theVecTo from vector theVecFrom.
61   //! Additional vector theHelpCrossVec defines preferred direction for
62   //! rotation and is used when theVecTo and theVecFrom are directed
63   //! oppositely.
64   gp_Quaternion(const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec)
65   {
66     SetRotation (theVecFrom, theVecTo, theHelpCrossVec);
67   }
68
69   //! Creates quaternion representing rotation on angle
70   //! theAngle around vector theAxis
71   gp_Quaternion(const gp_Vec& theAxis, const Standard_Real theAngle)
72   {
73     SetVectorAndAngle (theAxis, theAngle);
74   }
75
76   //! Creates quaternion from rotation matrix 3*3
77   //! (which should be orthonormal skew-symmetric matrix)
78   gp_Quaternion(const gp_Mat& theMat)
79   {
80     SetMatrix (theMat);
81   }
82
83   //! Simple equal test without precision
84   Standard_EXPORT Standard_Boolean IsEqual (const gp_Quaternion& theOther) const;
85
86   //! Sets quaternion to shortest-arc rotation producing
87   //! vector theVecTo from vector theVecFrom.
88   //! If vectors theVecFrom and theVecTo are opposite then rotation
89   //! axis is computed as theVecFrom ^ (1,0,0) or theVecFrom ^ (0,0,1).
90   Standard_EXPORT void SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo);
91
92   //! Sets quaternion to shortest-arc rotation producing
93   //! vector theVecTo from vector theVecFrom.
94   //! If vectors theVecFrom and theVecTo are opposite then rotation
95   //! axis is computed as theVecFrom ^ theHelpCrossVec.
96   Standard_EXPORT void SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec);
97
98   //! Create a unit quaternion from Axis+Angle representation
99   Standard_EXPORT void SetVectorAndAngle (const gp_Vec& theAxis, const Standard_Real theAngle);
100
101   //! Convert a quaternion to Axis+Angle representation,
102   //! preserve the axis direction and angle from -PI to +PI
103   Standard_EXPORT void GetVectorAndAngle (gp_Vec& theAxis, Standard_Real& theAngle) const;
104
105   //! Create a unit quaternion by rotation matrix
106   //! matrix must contain only rotation (not scale or shear)
107   //!
108   //! For numerical stability we find first the greatest component of quaternion
109   //! and than search others from this one
110   Standard_EXPORT void SetMatrix (const gp_Mat& theMat);
111
112   //! Returns rotation operation as 3*3 matrix
113   Standard_EXPORT gp_Mat GetMatrix() const;
114
115   //! Create a unit quaternion representing rotation defined
116   //! by generalized Euler angles
117   Standard_EXPORT void SetEulerAngles (const gp_EulerSequence theOrder, const Standard_Real theAlpha, const Standard_Real theBeta, const Standard_Real theGamma);
118
119   //! Returns Euler angles describing current rotation
120   Standard_EXPORT void GetEulerAngles (const gp_EulerSequence theOrder, Standard_Real& theAlpha, Standard_Real& theBeta, Standard_Real& theGamma) const;
121
122   void Set (const Standard_Real theX, const Standard_Real theY, const Standard_Real theZ, const Standard_Real theW);
123
124   void Set (const gp_Quaternion& theQuaternion);
125
126   Standard_Real X() const { return x; }
127
128   Standard_Real Y() const { return y; }
129
130   Standard_Real Z() const { return z; }
131
132   Standard_Real W() const { return w; }
133
134   //! Make identity quaternion (zero-rotation)
135   void SetIdent()
136   {
137     x = y = z = 0.0;
138     w = 1.0;
139   }
140
141   //! Reverse direction of rotation (conjugate quaternion)
142   void Reverse()
143   {
144     x = -x;
145     y = -y;
146     z = -z;
147   }
148
149   //! Return rotation with reversed direction (conjugated quaternion)
150   Standard_NODISCARD gp_Quaternion Reversed() const { return gp_Quaternion (-x, -y, -z, w); }
151
152   //! Inverts quaternion (both rotation direction and norm)
153   void Invert()
154   {
155     Standard_Real anIn = 1.0 / SquareNorm();
156     Set (-x * anIn, -y * anIn, -z * anIn, w * anIn);
157   }
158
159   //! Return inversed quaternion q^-1
160   Standard_NODISCARD gp_Quaternion Inverted() const
161   {
162     Standard_Real anIn = 1.0 / SquareNorm();
163     return gp_Quaternion (-x * anIn, -y * anIn, -z * anIn, w * anIn);
164   }
165
166   //! Returns square norm of quaternion
167   Standard_Real SquareNorm() const
168   {
169     return x * x + y * y + z * z + w * w;
170   }
171
172   //! Returns norm of quaternion
173   Standard_Real Norm() const { return Sqrt (SquareNorm()); }
174
175   //! Scale all components by quaternion by theScale; note that
176   //! rotation is not changed by this operation (except 0-scaling)
177   void Scale (const Standard_Real theScale);
178
179   void operator *= (const Standard_Real theScale) { Scale (theScale); }
180
181   //! Returns scaled quaternion
182   Standard_NODISCARD gp_Quaternion Scaled (const Standard_Real theScale) const
183   {
184     return gp_Quaternion (x * theScale, y * theScale, z * theScale, w * theScale);
185   }
186
187   Standard_NODISCARD gp_Quaternion operator * (const Standard_Real theScale) const { return Scaled (theScale); }
188
189   //! Stabilize quaternion length within 1 - 1/4.
190   //! This operation is a lot faster than normalization
191   //! and preserve length goes to 0 or infinity
192   Standard_EXPORT void StabilizeLength();
193
194   //! Scale quaternion that its norm goes to 1.
195   //! The appearing of 0 magnitude or near is a error,
196   //! so we can be sure that can divide by magnitude
197   Standard_EXPORT void Normalize();
198
199   //! Returns quaternion scaled so that its norm goes to 1.
200   Standard_NODISCARD gp_Quaternion Normalized() const
201   {
202     gp_Quaternion aNormilizedQ (*this);
203     aNormilizedQ.Normalize();
204     return aNormilizedQ;
205   }
206
207   //! Returns quaternion with all components negated.
208   //! Note that this operation does not affect neither
209   //! rotation operator defined by quaternion nor its norm.
210   Standard_NODISCARD gp_Quaternion Negated() const { return gp_Quaternion (-x, -y, -z, -w); }
211
212   Standard_NODISCARD gp_Quaternion operator -() const { return Negated(); }
213
214   //! Makes sum of quaternion components; result is "rotations mix"
215   Standard_NODISCARD gp_Quaternion Added (const gp_Quaternion& theOther) const
216   {
217     return gp_Quaternion (x + theOther.x, y + theOther.y, z + theOther.z, w + theOther.w);
218   }
219
220   Standard_NODISCARD gp_Quaternion operator + (const gp_Quaternion& theOther) const { return Added (theOther); }
221
222   //! Makes difference of quaternion components; result is "rotations mix"
223   Standard_NODISCARD gp_Quaternion Subtracted (const gp_Quaternion& theOther) const
224   {
225     return gp_Quaternion (x - theOther.x, y - theOther.y, z - theOther.z, w - theOther.w);
226   }
227
228   Standard_NODISCARD gp_Quaternion operator - (const gp_Quaternion& theOther) const { return Subtracted (theOther); }
229
230   //! Multiply function - work the same as Matrices multiplying.
231   //! @code
232   //! qq' = (cross(v,v') + wv' + w'v, ww' - dot(v,v'))
233   //! @endcode
234   //! Result is rotation combination: q' than q (here q=this, q'=theQ).
235   //! Notices that:
236   //! @code
237   //! qq' != q'q;
238   //! qq^-1 = q;
239   //! @endcode
240   Standard_NODISCARD gp_Quaternion Multiplied (const gp_Quaternion& theOther) const;
241
242   Standard_NODISCARD gp_Quaternion operator * (const gp_Quaternion& theOther) const { return Multiplied (theOther); }
243
244   //! Adds components of other quaternion; result is "rotations mix"
245   void Add (const gp_Quaternion& theOther);
246
247   void operator += (const gp_Quaternion& theOther) { Add (theOther); }
248
249   //! Subtracts components of other quaternion; result is "rotations mix"
250   void Subtract (const gp_Quaternion& theOther);
251
252   void operator -= (const gp_Quaternion& theOther) { Subtract (theOther); }
253
254   //! Adds rotation by multiplication
255   void Multiply (const gp_Quaternion& theOther)
256   {
257     (*this) = Multiplied (theOther);  // have no optimization here
258   }
259
260   void operator *= (const gp_Quaternion& theOther) { Multiply (theOther); }
261
262   //! Computes inner product / scalar product / Dot
263   Standard_Real Dot (const gp_Quaternion& theOther) const
264   {
265     return x * theOther.x + y * theOther.y + z * theOther.z + w * theOther.w;
266   }
267
268   //! Return rotation angle from -PI to PI
269   Standard_EXPORT Standard_Real GetRotationAngle() const;
270
271   //! Rotates vector by quaternion as rotation operator
272   Standard_EXPORT gp_Vec Multiply (const gp_Vec& theVec) const;
273
274   gp_Vec operator * (const gp_Vec& theVec) const { return Multiply (theVec); }
275
276 private:
277
278   Standard_Real x;
279   Standard_Real y;
280   Standard_Real z;
281   Standard_Real w;
282
283 };
284
285 //=======================================================================
286 //function : Set
287 //purpose  :
288 //=======================================================================
289 inline void gp_Quaternion::Set (Standard_Real theX, Standard_Real theY,
290                                 Standard_Real theZ, Standard_Real theW)
291 {
292   this->x = theX;
293   this->y = theY;
294   this->z = theZ;
295   this->w = theW;
296 }
297
298 //=======================================================================
299 //function : Set
300 //purpose  :
301 //=======================================================================
302 inline void gp_Quaternion::Set (const gp_Quaternion& theQuaternion)
303 {
304   x = theQuaternion.x; 
305   y = theQuaternion.y; 
306   z = theQuaternion.z; 
307   w = theQuaternion.w;
308 }
309
310 //=======================================================================
311 //function : Scale
312 //purpose  :
313 //=======================================================================
314 inline void gp_Quaternion::Scale (const Standard_Real theScale)
315 {
316   x *= theScale; 
317   y *= theScale; 
318   z *= theScale; 
319   w *= theScale;
320 }
321
322 //=======================================================================
323 //function : Multiplied
324 //purpose  :
325 //=======================================================================
326 inline gp_Quaternion gp_Quaternion::Multiplied (const gp_Quaternion& theQ) const
327 {
328   return gp_Quaternion (w * theQ.x + x * theQ.w + y * theQ.z - z * theQ.y,
329                         w * theQ.y + y * theQ.w + z * theQ.x - x * theQ.z,
330                         w * theQ.z + z * theQ.w + x * theQ.y - y * theQ.x,
331                         w * theQ.w - x * theQ.x - y * theQ.y - z * theQ.z);
332   // 16 multiplications    12 addidtions    0 variables
333 }
334
335 //=======================================================================
336 //function : Add
337 //purpose  :
338 //=======================================================================
339 inline void gp_Quaternion::Add (const gp_Quaternion& theQ)
340 {
341   x += theQ.x; 
342   y += theQ.y; 
343   z += theQ.z; 
344   w += theQ.w;
345 }
346
347 //=======================================================================
348 //function : Subtract
349 //purpose  :
350 //=======================================================================
351 inline void gp_Quaternion::Subtract (const gp_Quaternion& theQ)
352 {
353   x -= theQ.x; 
354   y -= theQ.y; 
355   z -= theQ.z; 
356   w -= theQ.w;
357 }
358
359 #endif // _gp_Quaternion_HeaderFile