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