0026641: Visualization, TKOpenGl - handle correctly transformation persistence within...
[occt.git] / src / Graphic3d / Graphic3d_TransformUtils.hxx
1 // Created on: 2015-06-18
2 // Copyright (c) 2015 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and/or modify it under
7 // the terms of the GNU Lesser General Public License version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 #ifndef _Graphic3d_TransformUtils_HeaderFile
16 #define _Graphic3d_TransformUtils_HeaderFile
17
18 #include <Graphic3d_Vec.hxx>
19 #include <Standard_math.hxx> // M_PI
20
21 //! Helper class that implements transformation matrix functionality.
22 namespace Graphic3d_TransformUtils
23 {
24   template<class T> struct MatrixType {};
25
26   template<> struct MatrixType<Standard_Real> { typedef Graphic3d_Mat4d Mat4; };
27
28   template<> struct MatrixType<Standard_ShortReal> { typedef Graphic3d_Mat4 Mat4; };
29
30   template<class T> struct VectorType {};
31
32   template<> struct VectorType<Standard_Real> {
33     typedef Graphic3d_Vec2d Vec2;
34     typedef Graphic3d_Vec3d Vec3;
35     typedef Graphic3d_Vec4d Vec4;
36   };
37
38   template<> struct VectorType<Standard_ShortReal> {
39     typedef Graphic3d_Vec2 Vec2;
40     typedef Graphic3d_Vec3 Vec3;
41     typedef Graphic3d_Vec4 Vec4;
42   };
43
44   //! Converts gp_Trsf to Graphic3d_Mat4.
45   template<class T>
46   static void Convert (const gp_Trsf&                theTransformation,
47                        typename MatrixType<T>::Mat4& theOut);
48
49   //! Constructs a 3D orthographic projection matrix.
50   template<class T>
51   static void Ortho (typename MatrixType<T>::Mat4& theOut,
52                      const T                       theLeft,
53                      const T                       theRight,
54                      const T                       theBottom,
55                      const T                       theTop,
56                      const T                       theZNear,
57                      const T                       theZFar);
58
59   //! Constructs a 2D orthographic projection matrix.
60   template<class T>
61   static void Ortho2D (typename MatrixType<T>::Mat4& theOut,
62                        const T                       theLeft,
63                        const T                       theRight,
64                        const T                       theBottom,
65                        const T                       theTop);
66
67   //! Maps object coordinates to window coordinates.
68   template<class T>
69   static Standard_Boolean Project (const T                             theObjX,
70                                    const T                             theObjY,
71                                    const T                             theObjZ,
72                                    const typename MatrixType<T>::Mat4& theModViewMat,
73                                    const typename MatrixType<T>::Mat4& theProjectMat,
74                                    const Standard_Integer              theViewport[4],
75                                    T&                                  theWinX,
76                                    T&                                  theWinY,
77                                    T&                                  theWinZ);
78
79   //! Maps window coordinates to object coordinates.
80   template<class T>
81   static Standard_Boolean UnProject (const T                             theWinX,
82                                      const T                             theWinY,
83                                      const T                             theWinZ,
84                                      const typename MatrixType<T>::Mat4& theModViewMat,
85                                      const typename MatrixType<T>::Mat4& theProjectMat,
86                                      const Standard_Integer              theViewport[4],
87                                      T&                                  theObjX,
88                                      T&                                  theObjY,
89                                      T&                                  theObjZ);
90
91   //! Constructs a 4x4 rotation matrix.
92   template<class T>
93   static void ConstructRotate (typename MatrixType<T>::Mat4& theOut,
94                                T                             theA,
95                                T                             theX,
96                                T                             theY,
97                                T                             theZ);
98
99   //! Constructs a 4x4 rotation matrix.
100   template<class T>
101   static void Rotate (typename MatrixType<T>::Mat4& theOut,
102                       T                             theA,
103                       T                             theX,
104                       T                             theY,
105                       T                             theZ);
106
107   //! Constructs a 4x4 scaling matrix.
108   template<class T>
109   static void Scale (typename MatrixType<T>::Mat4& theOut,
110                      T                             theX,
111                      T                             theY,
112                      T                             theZ);
113
114   //! Constructs a 4x4 translation matrix.
115   template<class T>
116   static void Translate (typename MatrixType<T>::Mat4& theOut,
117                          T                             theX,
118                          T                             theY,
119                          T                             theZ);
120
121   //! Returns scaling factor from 3x3 affine matrix.
122   template<class T>
123   static Standard_Real ScaleFactor (const typename MatrixType<T>::Mat4& theMatrix);
124 }
125
126 // =======================================================================
127 // function : Convert
128 // purpose  :
129 // =======================================================================
130 template<class T>
131 void Graphic3d_TransformUtils::Convert (const gp_Trsf&                theTransformation,
132                                         typename MatrixType<T>::Mat4& theOut)
133 {
134   theOut.InitIdentity();
135
136   // Copy a 3x3 submatrix.
137   theOut.ChangeValue (0, 0) = theTransformation.Value (1, 1);
138   theOut.ChangeValue (0, 1) = theTransformation.Value (1, 2);
139   theOut.ChangeValue (0, 2) = theTransformation.Value (1, 3);
140   theOut.ChangeValue (1, 0) = theTransformation.Value (2, 1);
141   theOut.ChangeValue (1, 1) = theTransformation.Value (2, 2);
142   theOut.ChangeValue (1, 2) = theTransformation.Value (2, 3);
143   theOut.ChangeValue (2, 0) = theTransformation.Value (3, 1);
144   theOut.ChangeValue (2, 1) = theTransformation.Value (3, 2);
145   theOut.ChangeValue (2, 2) = theTransformation.Value (3, 3);
146
147   // Add a translate component.
148   theOut.ChangeValue (0, 3) = theTransformation.TranslationPart().X();
149   theOut.ChangeValue (1, 3) = theTransformation.TranslationPart().Y();
150   theOut.ChangeValue (2, 3) = theTransformation.TranslationPart().Z();
151 }
152
153 // =======================================================================
154 // function : Rotate
155 // purpose  : Constructs a 4x4 rotation matrix
156 // =======================================================================
157 template<class T>
158 void Graphic3d_TransformUtils::Rotate (typename MatrixType<T>::Mat4& theOut,
159                                        T                             theA,
160                                        T                             theX,
161                                        T                             theY,
162                                        T                             theZ)
163 {
164   typename MatrixType<T>::Mat4 aMat;
165   ConstructRotate (aMat, theA, theX, theY, theZ);
166   theOut = theOut * aMat;
167 }
168
169 // =======================================================================
170 // function : Translate
171 // purpose  : Constructs a 4x4 translation matrix
172 // =======================================================================
173 template<class T>
174 void Graphic3d_TransformUtils::Translate (typename MatrixType<T>::Mat4& theOut,
175                                           T                             theX,
176                                           T                             theY,
177                                           T                             theZ)
178 {
179   theOut.ChangeValue (0, 3) = theOut.GetValue (0, 0) * theX +
180                               theOut.GetValue (0, 1) * theY +
181                               theOut.GetValue (0, 2) * theZ +
182                               theOut.GetValue (0, 3);
183
184   theOut.ChangeValue (1, 3) = theOut.GetValue (1, 0) * theX +
185                               theOut.GetValue (1, 1) * theY +
186                               theOut.GetValue (1, 2) * theZ +
187                               theOut.GetValue (1, 3);
188
189   theOut.ChangeValue (2, 3) = theOut.GetValue (2, 0) * theX +
190                               theOut.GetValue (2, 1) * theY +
191                               theOut.GetValue (2, 2) * theZ +
192                               theOut.GetValue (2, 3);
193
194   theOut.ChangeValue (3, 3) = theOut.GetValue (3, 0) * theX +
195                               theOut.GetValue (3, 1) * theY +
196                               theOut.GetValue (3, 2) * theZ +
197                               theOut.GetValue (3, 3);
198 }
199
200 // =======================================================================
201 // function : Scale
202 // purpose  : Constructs a 4x4 scaling matrix
203 // =======================================================================
204 template<class T>
205 void Graphic3d_TransformUtils::Scale (typename MatrixType<T>::Mat4& theOut,
206                                       T                             theX,
207                                       T                             theY,
208                                       T                             theZ)
209 {
210   theOut.ChangeValue (0, 0) *= theX;
211   theOut.ChangeValue (1, 0) *= theX;
212   theOut.ChangeValue (2, 0) *= theX;
213   theOut.ChangeValue (3, 0) *= theX;
214
215   theOut.ChangeValue (0, 1) *= theY;
216   theOut.ChangeValue (1, 1) *= theY;
217   theOut.ChangeValue (2, 1) *= theY;
218   theOut.ChangeValue (3, 1) *= theY;
219
220   theOut.ChangeValue (0, 2) *= theZ;
221   theOut.ChangeValue (1, 2) *= theZ;
222   theOut.ChangeValue (2, 2) *= theZ;
223   theOut.ChangeValue (3, 2) *= theZ;
224 }
225
226 // =======================================================================
227 // function : ConstructRotate
228 // purpose  : Constructs a 4x4 rotation matrix
229 // =======================================================================
230 template<class T>
231 void Graphic3d_TransformUtils::ConstructRotate (typename MatrixType<T>::Mat4& theOut,
232                                                 T                             theA,
233                                                 T                             theX,
234                                                 T                             theY,
235                                                 T                             theZ)
236 {
237   const T aSin = std::sin (theA * static_cast<T> (M_PI / 180.0));
238   const T aCos = std::cos (theA * static_cast<T> (M_PI / 180.0));
239
240   const Standard_Boolean isOnlyX = (theX != static_cast<T> (0.0))
241                                 && (theY == static_cast<T> (0.0))
242                                 && (theZ == static_cast<T> (0.0));
243
244   const Standard_Boolean isOnlyY = (theX == static_cast<T> (0.0))
245                                 && (theY != static_cast<T> (0.0))
246                                 && (theZ == static_cast<T> (0.0));
247
248   const Standard_Boolean isOnlyZ = (theX == static_cast<T> (0.0))
249                                 && (theY == static_cast<T> (0.0))
250                                 && (theZ != static_cast<T> (0.0));
251
252   if (isOnlyX) // Rotation only around X.
253   {
254     theOut.SetValue (1, 1, aCos);
255     theOut.SetValue (2, 2, aCos);
256
257     if (theX < static_cast<T> (0.0))
258     {
259       theOut.SetValue (1, 2,  aSin);
260       theOut.SetValue (2, 1, -aSin);
261     }
262     else
263     {
264       theOut.SetValue (1, 2, -aSin);
265       theOut.SetValue (2, 1,  aSin);
266     }
267
268     return;
269   }
270   else if (isOnlyY) // Rotation only around Y.
271   {
272     theOut.SetValue (0, 0, aCos);
273     theOut.SetValue (2, 2, aCos);
274
275     if (theY < static_cast<T> (0.0))
276     {
277       theOut.SetValue (0, 2, -aSin);
278       theOut.SetValue (2, 0,  aSin);
279     }
280     else
281     {
282       theOut.SetValue (0, 2,  aSin);
283       theOut.SetValue (2, 0, -aSin);
284     }
285
286     return;
287   }
288   else if (isOnlyZ) // Rotation only around Z.
289   {
290     theOut.SetValue (0, 0, aCos);
291     theOut.SetValue (1, 1, aCos);
292
293     if (theZ < static_cast<T> (0.0))
294     {
295       theOut.SetValue (0, 1,  aSin);
296       theOut.SetValue (1, 0, -aSin);
297     }
298     else
299     {
300       theOut.SetValue (0, 1, -aSin);
301       theOut.SetValue (1, 0,  aSin);
302     }
303
304     return;
305   }
306
307   T aNorm = std::sqrt (theX * theX + theY * theY + theZ * theZ);
308
309   if (aNorm <= static_cast<T> (1.0e-4))
310   {
311     return; // Negligible rotation.
312   }
313
314   aNorm = static_cast<T> (1.0) / aNorm;
315
316   theX *= aNorm;
317   theY *= aNorm;
318   theZ *= aNorm;
319
320   const T aXX = theX * theX;
321   const T aYY = theY * theY;
322   const T aZZ = theZ * theZ;
323   const T aXY = theX * theY;
324   const T aYZ = theY * theZ;
325   const T aZX = theZ * theX;
326   const T aSinX = theX * aSin;
327   const T aSinY = theY * aSin;
328   const T aSinZ = theZ * aSin;
329
330   const T aOneMinusCos = static_cast<T> (1.0) - aCos;
331
332   theOut.SetValue (0, 0, aOneMinusCos * aXX + aCos);
333   theOut.SetValue (0, 1, aOneMinusCos * aXY - aSinZ);
334   theOut.SetValue (0, 2, aOneMinusCos * aZX + aSinY);
335
336   theOut.SetValue (1, 0, aOneMinusCos * aXY + aSinZ);
337   theOut.SetValue (1, 1, aOneMinusCos * aYY + aCos);
338   theOut.SetValue (1, 2, aOneMinusCos * aYZ - aSinX);
339
340   theOut.SetValue (2, 0, aOneMinusCos * aZX - aSinY);
341   theOut.SetValue (2, 1, aOneMinusCos * aYZ + aSinX);
342   theOut.SetValue (2, 2, aOneMinusCos * aZZ + aCos);
343 }
344
345 // =======================================================================
346 // function : Ortho
347 // purpose  : Constructs a 3D orthographic projection matrix
348 // =======================================================================
349 template<class T>
350 void Graphic3d_TransformUtils::Ortho (typename MatrixType<T>::Mat4& theOut,
351                                       const T                       theLeft,
352                                       const T                       theRight,
353                                       const T                       theBottom,
354                                       const T                       theTop,
355                                       const T                       theZNear,
356                                       const T                       theZFar)
357 {
358   theOut.InitIdentity();
359
360   T* aData = theOut.ChangeData();
361
362   const T anInvDx = static_cast<T> (1.0) / (theRight - theLeft);
363   const T anInvDy = static_cast<T> (1.0) / (theTop - theBottom);
364   const T anInvDz = static_cast<T> (1.0) / (theZFar - theZNear);
365
366   aData[0]  = static_cast<T> ( 2.0) * anInvDx;
367   aData[5]  = static_cast<T> ( 2.0) * anInvDy;
368   aData[10] = static_cast<T> (-2.0) * anInvDz;
369
370   aData[12] = -(theRight + theLeft) * anInvDx;
371   aData[13] = -(theTop + theBottom) * anInvDy;
372   aData[14] = -(theZFar + theZNear) * anInvDz;
373 }
374
375 // =======================================================================
376 // function : Ortho2D
377 // purpose  : Constructs a 2D orthographic projection matrix
378 // =======================================================================
379 template<class T>
380 void Graphic3d_TransformUtils::Ortho2D (typename MatrixType<T>::Mat4& theOut,
381                                         const T                       theLeft,
382                                         const T                       theRight,
383                                         const T                       theBottom,
384                                         const T                       theTop)
385 {
386   Ortho (theOut, theLeft, theRight, theBottom, theTop, static_cast<T> (-1.0), static_cast<T> (1.0));
387 }
388
389 // =======================================================================
390 // function : Project
391 // purpose  : Maps object coordinates to window coordinates
392 // =======================================================================
393 template<class T>
394 static Standard_Boolean Graphic3d_TransformUtils::Project (const T                             theObjX,
395                                                            const T                             theObjY,
396                                                            const T                             theObjZ,
397                                                            const typename MatrixType<T>::Mat4& theModViewMat,
398                                                            const typename MatrixType<T>::Mat4& theProjectMat,
399                                                            const Standard_Integer              theViewport[4],
400                                                            T&                                  theWinX,
401                                                            T&                                  theWinY,
402                                                            T&                                  theWinZ)
403 {
404   typename VectorType<T>::Vec4 anIn (theObjX, theObjY, theObjZ, static_cast<T> (1.0));
405
406   typename VectorType<T>::Vec4 anOut = theProjectMat * (theModViewMat * anIn);
407
408   if (anOut.w() == static_cast<T> (0.0))
409   {
410     return Standard_False;
411   }
412
413   anOut.w() = static_cast<T> (1.0) / anOut.w();
414
415   anOut.x() *= anOut.w();
416   anOut.y() *= anOut.w();
417   anOut.z() *= anOut.w();
418
419   // Map x, y and z to range 0-1.
420   anOut.x() = anOut.x() * static_cast<T> (0.5) + static_cast<T> (0.5);
421   anOut.y() = anOut.y() * static_cast<T> (0.5) + static_cast<T> (0.5);
422   anOut.z() = anOut.z() * static_cast<T> (0.5) + static_cast<T> (0.5);
423
424   // Map x,y to viewport.
425   anOut.x() = anOut.x() * theViewport[2] + theViewport[0];
426   anOut.y() = anOut.y() * theViewport[3] + theViewport[1];
427
428   theWinX = anOut.x();
429   theWinY = anOut.y();
430   theWinZ = anOut.z();
431
432   return Standard_True;
433 }
434
435 // =======================================================================
436 // function : UnProject
437 // purpose  : Maps window coordinates to object coordinates
438 // =======================================================================
439 template<class T>
440 static Standard_Boolean Graphic3d_TransformUtils::UnProject (const T                             theWinX,
441                                                              const T                             theWinY,
442                                                              const T                             theWinZ,
443                                                              const typename MatrixType<T>::Mat4& theModViewMat,
444                                                              const typename MatrixType<T>::Mat4& theProjectMat,
445                                                              const Standard_Integer              theViewport[4],
446                                                              T&                                  theObjX,
447                                                              T&                                  theObjY,
448                                                              T&                                  theObjZ)
449 {
450   typename MatrixType<T>::Mat4 anUnviewMat;
451
452   if (!(theProjectMat * theModViewMat).Inverted (anUnviewMat))
453   {
454     return Standard_False;
455   }
456
457   typename VectorType<T>::Vec4 anIn (theWinX, theWinY, theWinZ, static_cast<T> (1.0));
458
459   // Map x and y from window coordinates.
460   anIn.x() = (anIn.x() - theViewport[0]) / theViewport[2];
461   anIn.y() = (anIn.y() - theViewport[1]) / theViewport[3];
462
463   // Map to range -1 to 1.
464   anIn.x() = anIn.x() * static_cast<T> (2.0) - static_cast<T> (1.0);
465   anIn.y() = anIn.y() * static_cast<T> (2.0) - static_cast<T> (1.0);
466   anIn.z() = anIn.z() * static_cast<T> (2.0) - static_cast<T> (1.0);
467
468   typename VectorType<T>::Vec4 anOut = anUnviewMat * anIn;
469
470   if (anOut.w() == static_cast<T> (0.0))
471   {
472     return Standard_False;
473   }
474
475   anOut.w() = static_cast<T> (1.0) / anOut.w();
476
477   anOut.x() *= anOut.w();
478   anOut.y() *= anOut.w();
479   anOut.z() *= anOut.w();
480
481   theObjX = anOut.x();
482   theObjY = anOut.y();
483   theObjZ = anOut.z();
484
485   return Standard_True;
486 }
487
488 // =======================================================================
489 // function : ScaleFactor
490 // purpose  :
491 // =======================================================================
492 template<class T>
493 static Standard_Real Graphic3d_TransformUtils::ScaleFactor (const typename MatrixType<T>::Mat4& theMatrix)
494 {
495   // The determinant of the matrix should give the scale factor (cubed).
496   const T aDeterminant = (theMatrix.GetValue (0, 0) * theMatrix.GetValue (1, 1) * theMatrix.GetValue (2, 2) +
497                           theMatrix.GetValue (0, 1) * theMatrix.GetValue (1, 2) * theMatrix.GetValue (2, 0) +
498                           theMatrix.GetValue (0, 2) * theMatrix.GetValue (1, 0) * theMatrix.GetValue (2, 1))
499                        - (theMatrix.GetValue (0, 2) * theMatrix.GetValue (1, 1) * theMatrix.GetValue (2, 0) +
500                           theMatrix.GetValue (0, 0) * theMatrix.GetValue (1, 2) * theMatrix.GetValue (2, 1) +
501                           theMatrix.GetValue (0, 1) * theMatrix.GetValue (1, 0) * theMatrix.GetValue (2, 2));
502
503   return Pow (static_cast<Standard_Real> (aDeterminant), 1.0 / 3.0);
504 }
505
506 #endif // _Graphic3d_TransformUtils_HeaderFile