c2d52cd9052e8f6221c747b06673b1ec07e0ce6d
[occt.git] / src / NCollection / NCollection_Mat4.hxx
1 // Created on: 2013-05-30
2 // Created by: Anton POLETAEV
3 // Copyright (c) 2013-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 _NCollection_Mat4_HeaderFile
17 #define _NCollection_Mat4_HeaderFile
18
19 #include <NCollection_Vec4.hxx>
20
21 //! Generic matrix of 4 x 4 elements.
22 //! To be used in conjunction with NCollection_Vec4 entities.
23 //! Originally introduced for 3D space projection and orientation
24 //! operations.
25 template<typename Element_t>
26 class NCollection_Mat4
27 {
28
29 public:
30
31   //! Get number of rows.
32   //! @return number of rows.
33   static size_t Rows()
34   {
35     return 4;
36   }
37
38   //! Get number of columns.
39   //! @retur number of columns.
40   static size_t Cols()
41   {
42     return 4;
43   }
44
45   //! Empty constructor.
46   //! Construct the zero matrix.
47   NCollection_Mat4()
48   {
49     InitIdentity();
50   }
51
52   //! Copy constructor.
53   //! @param theOther [in] the matrix to copy values from.
54   NCollection_Mat4 (const NCollection_Mat4& theOther)
55   {
56     std::memcpy (this, &theOther, sizeof (NCollection_Mat4));
57   }
58
59   //! Assignment operator.
60   //! @param theOther [in] the matrix to copy values from.
61   const NCollection_Mat4& operator= (const NCollection_Mat4& theOther)
62   {
63     std::memcpy (this, &theOther, sizeof (NCollection_Mat4));
64     return *this;
65   }
66
67   //! Get element at the specified row and column.
68   //! @param theRow [in] the row.to address.
69   //! @param theCol [in] the column to address.
70   //! @return the value of the addressed element.
71   Element_t GetValue (const size_t theRow, const size_t theCol) const
72   {
73     return myMat[theCol * 4 + theRow];
74   }
75
76   //! Access element at the specified row and column.
77   //! @param theRow [in] the row.to access.
78   //! @param theCol [in] the column to access.
79   //! @return reference on the matrix element.
80   Element_t& ChangeValue (const size_t theRow, const size_t theCol)
81   {
82     return myMat[theCol * 4 + theRow];
83   }
84
85   //! Set value for the element specified by row and columns.
86   //! @param theRow   [in] the row to change.
87   //! @param theCol   [in] the column to change.
88   //! @param theValue [in] the value to set.s
89   void SetValue (const size_t    theRow,
90                  const size_t    theCol,
91                  const Element_t theValue)
92   {
93     myMat[theCol * 4 + theRow] = theValue;
94   }
95
96   //! Get vector of elements for the specified row.
97   //! @param theRow [in] the row to access.
98   //! @return vector of elements.
99   NCollection_Vec4<Element_t> GetRow (const size_t theRow) const
100   {
101     return NCollection_Vec4<Element_t> (GetValue (theRow, 0),
102                                         GetValue (theRow, 1),
103                                         GetValue (theRow, 2),
104                                         GetValue (theRow, 3));
105   }
106
107   //! Change first 3 row values by the passed vector.
108   //! @param theRow [in] the row to change.
109   //! @param theVec [in] the vector of values.
110   void SetRow (const size_t theRow, const NCollection_Vec3<Element_t>& theVec)
111   {
112     SetValue (theRow, 0, theVec.x());
113     SetValue (theRow, 1, theVec.y());
114     SetValue (theRow, 2, theVec.z());
115   }
116
117   //! Set row values by the passed 4 element vector.
118   //! @param theRow [in] the row to change.
119   //! @param theVec [in] the vector of values.
120   void SetRow (const size_t theRow, const NCollection_Vec4<Element_t>& theVec)
121   {
122     SetValue (theRow, 0, theVec.x());
123     SetValue (theRow, 1, theVec.y());
124     SetValue (theRow, 2, theVec.z());
125     SetValue (theRow, 3, theVec.w());
126   }
127
128   //! Get vector of elements for the specified column.
129   //! @param theCol [in] the column to access.
130   //! @return vector of elements.
131   NCollection_Vec4<Element_t> GetColumn (const size_t theCol) const
132   {
133     return NCollection_Vec4<Element_t> (GetValue (0, theCol),
134                                         GetValue (1, theCol),
135                                         GetValue (2, theCol),
136                                         GetValue (3, theCol));
137   }
138
139   //! Change first 3 column values by the passed vector.
140   //! @param theCol [in] the column to change.
141   //! @param theVec [in] the vector of values.
142   void SetColumn (const size_t theCol,
143                   const NCollection_Vec3<Element_t>& theVec)
144   {
145     SetValue (0, theCol, theVec.x());
146     SetValue (1, theCol, theVec.y());
147     SetValue (2, theCol, theVec.z());
148   }
149
150   //! Set column values by the passed 4 element vector.
151   //! @param theCol [in] the column to change.
152   //! @param theVec [in] the vector of values.
153   void SetColumn (const size_t theCol,
154                   const NCollection_Vec4<Element_t>& theVec)
155   {
156     SetValue (0, theCol, theVec.x());
157     SetValue (1, theCol, theVec.y());
158     SetValue (2, theCol, theVec.z());
159     SetValue (3, theCol, theVec.w());
160   }
161
162   //! Get vector of diagonal elements.
163   //! \return vector of diagonal elements.
164   NCollection_Vec4<Element_t> GetDiagonal() const
165   {
166     return NCollection_Vec4<Element_t> (GetValue (0, 0),
167                                         GetValue (1, 1),
168                                         GetValue (2, 2),
169                                         GetValue (3, 3));
170   }
171
172   //! Change first 3 elements of the diagonal matrix.
173   //! @param theVec the vector of values.
174   void SetDiagonal (const NCollection_Vec3<Element_t>& theVec)
175   {
176     SetValue (0, 0, theVec.x());
177     SetValue (1, 1, theVec.y());
178     SetValue (2, 2, theVec.z());
179   }
180
181   //! Set diagonal elements of the matrix by the passed vector.
182   //! @param theVec [in] the vector of values.
183   void SetDiagonal (const NCollection_Vec4<Element_t>& theVec)
184   {
185     SetValue (0, 0, theVec.x());
186     SetValue (1, 1, theVec.y());
187     SetValue (2, 2, theVec.z());
188     SetValue (3, 3, theVec.w());
189   }
190
191   //! Initialize the identity matrix.
192   void InitIdentity()
193   {
194     std::memcpy (this, myIdentityArray, sizeof (NCollection_Mat4));
195   }
196
197   //! Checks the matrix for identity.
198   bool IsIdentity() const
199   {
200     return std::memcmp (this, myIdentityArray, sizeof (NCollection_Mat4)) == 0;
201   }
202
203   //! Raw access to the data (for OpenGL exchange).
204   const Element_t* GetData()    const { return myMat; }
205   Element_t*       ChangeData()       { return myMat; }
206   operator const   Element_t*() const { return myMat; }
207   operator         Element_t*()       { return myMat; }
208
209   //! Multiply by the vector (M * V).
210   //! @param theVec [in] the vector to multiply.
211   NCollection_Vec4<Element_t> operator* (const NCollection_Vec4<Element_t>& theVec) const
212   {
213     return NCollection_Vec4<Element_t> (
214       GetValue (0, 0) * theVec.x() + GetValue (0, 1) * theVec.y() + GetValue (0, 2) * theVec.z() + GetValue (0, 3) * theVec.w(),
215       GetValue (1, 0) * theVec.x() + GetValue (1, 1) * theVec.y() + GetValue (1, 2) * theVec.z() + GetValue (1, 3) * theVec.w(),
216       GetValue (2, 0) * theVec.x() + GetValue (2, 1) * theVec.y() + GetValue (2, 2) * theVec.z() + GetValue (2, 3) * theVec.w(),
217       GetValue (3, 0) * theVec.x() + GetValue (3, 1) * theVec.y() + GetValue (3, 2) * theVec.z() + GetValue (3, 3) * theVec.w());
218   }
219
220   //! Compute matrix multiplication product: A * B.
221   //! @param theMatA [in] the matrix "A".
222   //! @param theMatB [in] the matrix "B".
223   NCollection_Mat4 Multiply (const NCollection_Mat4& theMatA,
224                              const NCollection_Mat4& theMatB)
225   {
226     NCollection_Mat4 aMatRes;
227
228     size_t aInputElem;
229     for (size_t aResElem = 0; aResElem < 16; ++aResElem)
230     {
231       aMatRes[aResElem] = (Element_t )0;
232       for (aInputElem = 0; aInputElem < 4; ++aInputElem)
233       {
234         aMatRes[aResElem] += theMatA.GetValue(aResElem % 4, aInputElem)
235                            * theMatB.GetValue(aInputElem, aResElem / 4);
236       }
237     }
238
239     return aMatRes;
240   }
241
242   //! Compute matrix multiplication.
243   //! @param theMat [in] the matrix to multiply.
244   void Multiply (const NCollection_Mat4& theMat)
245   {
246     *this = Multiply(*this, theMat);
247   }
248
249   //! Multiply by the another matrix.
250   //! @param theMat [in] the other matrix.
251   NCollection_Mat4& operator*= (const NCollection_Mat4& theMat)
252   {
253     Multiply (theMat);
254     return *this;
255   }
256
257   //! Compute matrix multiplication product.
258   //! @param theMat [in] the other matrix.
259   //! @return result of multiplication.
260   NCollection_Mat4 operator* (const NCollection_Mat4& theMat) const
261   {
262     return Multiplied (theMat);
263   }
264
265   //! Compute matrix multiplication product.
266   //! @param theMat [in] the other matrix.
267   //! @return result of multiplication.
268   NCollection_Mat4 Multiplied (const NCollection_Mat4& theMat) const
269   {
270     NCollection_Mat4 aTempMat (*this);
271     aTempMat *= theMat;
272     return aTempMat;
273   }
274
275   //! Compute per-component multiplication.
276   //! @param theFactor [in] the scale factor.
277   void Multiply (const Element_t theFactor)
278   {
279     for (size_t i = 0; i < 16; ++i)
280     {
281       myMat[i] *= theFactor;
282     }
283   }
284
285   //! Compute per-element multiplication.
286   //! @param theFactor [in] the scale factor.
287   NCollection_Mat4& operator*=(const Element_t theFactor)
288   {
289     Multiply (theFactor);
290     return *this;
291   }
292
293   //! Compute per-element multiplication.
294   //! @param theFactor [in] the scale factor.
295   //! @return the result of multiplicaton.
296   NCollection_Mat4 operator* (const Element_t theFactor) const
297   {
298     return Multiplied (theFactor);
299   }
300
301   //! Compute per-element multiplication.
302   //! @param theFactor [in] the scale factor.
303   //! @return the result of multiplicaton.
304   NCollection_Mat4 Multiplied (const Element_t theFactor) const
305   {
306     NCollection_Mat4 aTempMat (*this);
307     aTempMat *= theFactor;
308     return aTempMat;
309   }
310
311   //! Translate the matrix on the passed vector.
312   //! @param theVec [in] the translation vector.
313   void Translate (const NCollection_Vec3<Element_t>& theVec)
314   {
315     NCollection_Mat4 aTempMat;
316     aTempMat.SetColumn (3, theVec);
317     this->Multiply (aTempMat);
318   }
319
320   //! Transpose the matrix.
321   //! @return transposed copy of the matrix.
322   NCollection_Mat4 Transposed() const
323   {
324     NCollection_Mat4 aTempMat;
325     aTempMat.SetRow (0, GetColumn (0));
326     aTempMat.SetRow (1, GetColumn (1));
327     aTempMat.SetRow (2, GetColumn (2));
328     aTempMat.SetRow (3, GetColumn (3));
329     return aTempMat;
330   }
331
332   //! Transpose the matrix.
333   void Transpose()
334   {
335     *this = Transposed();
336   }
337
338   //! Compute inverted matrix.
339   //! @param theOutMx [out] the inverted matrix.
340   //! @return true if reversion success.
341   bool Inverted (NCollection_Mat4<Element_t>& theOutMx) const
342   {
343     Element_t* inv = theOutMx.myMat;
344
345     // use short-cut for better readability
346     const Element_t* m = myMat;
347
348     inv[ 0] = m[ 5] * (m[10] * m[15] - m[11] * m[14]) -
349               m[ 9] * (m[ 6] * m[15] - m[ 7] * m[14]) -
350               m[13] * (m[ 7] * m[10] - m[ 6] * m[11]);
351
352     inv[ 1] = m[ 1] * (m[11] * m[14] - m[10] * m[15]) -
353               m[ 9] * (m[ 3] * m[14] - m[ 2] * m[15]) -
354               m[13] * (m[ 2] * m[11] - m[ 3] * m[10]);
355
356     inv[ 2] = m[ 1] * (m[ 6] * m[15] - m[ 7] * m[14]) -
357               m[ 5] * (m[ 2] * m[15] - m[ 3] * m[14]) -
358               m[13] * (m[ 3] * m[ 6] - m[ 2] * m[ 7]);
359
360     inv[ 3] = m[ 1] * (m[ 7] * m[10] - m[ 6] * m[11]) -
361               m[ 5] * (m[ 3] * m[10] - m[ 2] * m[11]) -
362               m[ 9] * (m[ 2] * m[ 7] - m[ 3] * m[ 6]);
363
364     inv[ 4] = m[ 4] * (m[11] * m[14] - m[10] * m[15]) -
365               m[ 8] * (m[ 7] * m[14] - m[ 6] * m[15]) -
366               m[12] * (m[ 6] * m[11] - m[ 7] * m[10]);
367
368     inv[ 5] = m[ 0] * (m[10] * m[15] - m[11] * m[14]) -
369               m[ 8] * (m[ 2] * m[15] - m[ 3] * m[14]) -
370               m[12] * (m[ 3] * m[10] - m[ 2] * m[11]);
371
372     inv[ 6] = m[ 0] * (m[ 7] * m[14] - m[ 6] * m[15]) -
373               m[ 4] * (m[ 3] * m[14] - m[ 2] * m[15]) -
374               m[12] * (m[ 2] * m[ 7] - m[ 3] * m[ 6]);
375
376     inv[ 7] = m[ 0] * (m[ 6] * m[11] - m[ 7] * m[10]) -
377               m[ 4] * (m[ 2] * m[11] - m[ 3] * m[10]) -
378               m[ 8] * (m[ 3] * m[ 6] - m[ 2] * m[ 7]);
379
380     inv[ 8] = m[ 4] * (m[ 9] * m[15] - m[11] * m[13]) -
381               m[ 8] * (m[ 5] * m[15] - m[ 7] * m[13]) -
382               m[12] * (m[ 7] * m[ 9] - m[ 5] * m[11]);
383
384     inv[ 9] = m[ 0] * (m[11] * m[13] - m[ 9] * m[15]) -
385               m[ 8] * (m[ 3] * m[13] - m[ 1] * m[15]) -
386               m[12] * (m[ 1] * m[11] - m[ 3] * m[ 9]);
387
388     inv[10] = m[ 0] * (m[ 5] * m[15] - m[ 7] * m[13]) -
389               m[ 4] * (m[ 1] * m[15] - m[ 3] * m[13]) -
390               m[12] * (m[ 3] * m[ 5] - m[ 1] * m[ 7]);
391
392     inv[11] = m[ 0] * (m[ 7] * m[ 9] - m[ 5] * m[11]) -
393               m[ 4] * (m[ 3] * m[ 9] - m[ 1] * m[11]) -
394               m[ 8] * (m[ 1] * m[ 7] - m[ 3] * m[ 5]);
395
396     inv[12] = m[ 4] * (m[10] * m[13] - m[ 9] * m[14]) -
397               m[ 8] * (m[ 6] * m[13] - m[ 5] * m[14]) -
398               m[12] * (m[ 5] * m[10] - m[ 6] * m[ 9]);
399
400     inv[13] = m[ 0] * (m[ 9] * m[14] - m[10] * m[13]) -
401               m[ 8] * (m[ 1] * m[14] - m[ 2] * m[13]) -
402               m[12] * (m[ 2] * m[ 9] - m[ 1] * m[10]);
403
404     inv[14] = m[ 0] * (m[ 6] * m[13] - m[ 5] * m[14]) -
405               m[ 4] * (m[ 2] * m[13] - m[ 1] * m[14]) -
406               m[12] * (m[ 1] * m[ 6] - m[ 2] * m[ 5]);
407
408     inv[15] = m[ 0] * (m[ 5] * m[10] - m[ 6] * m[ 9]) -
409               m[ 4] * (m[ 1] * m[10] - m[ 2] * m[ 9]) -
410               m[ 8] * (m[ 2] * m[ 5] - m[ 1] * m[ 6]);
411
412     Element_t aDet = m[0] * inv[ 0] +
413                      m[1] * inv[ 4] +
414                      m[2] * inv[ 8] +
415                      m[3] * inv[12];
416
417     if (aDet == 0)
418       return false;
419
420     aDet = (Element_t) 1. / aDet;
421
422     for (int i = 0; i < 16; ++i)
423       inv[i] *= aDet;
424
425     return true;
426   }
427
428   // Converts NCollection_Mat4 with different element type.
429   template <typename Other_t>
430   void Convert (const NCollection_Mat4<Other_t>& theOther)
431   {
432     for (int anIdx = 0; anIdx < 16; ++anIdx)
433     {
434       myMat[anIdx] = static_cast<Element_t> (theOther.myMat[anIdx]);
435     }
436   }
437
438   //! Maps plain C array to matrix type.
439   static NCollection_Mat4<Element_t>& Map (Element_t* theData)
440   {
441     return *reinterpret_cast<NCollection_Mat4<Element_t>*> (theData);
442   }
443
444   //! Maps plain C array to matrix type.
445   static const NCollection_Mat4<Element_t>& Map (const Element_t* theData)
446   {
447     return *reinterpret_cast<const NCollection_Mat4<Element_t>*> (theData);
448   }
449
450 private:
451
452   Element_t myMat[16];
453
454 private:
455
456   static Element_t myIdentityArray[16];
457
458   // All instantiations are friend to each other
459   template<class OtherType> friend class NCollection_Mat4;
460
461 };
462
463 template<typename Element_t>
464 Element_t NCollection_Mat4<Element_t>::myIdentityArray[] =
465   {1, 0, 0, 0,
466    0, 1, 0, 0,
467    0, 0, 1, 0,
468    0, 0, 0, 1};
469
470 #endif // _NCollection_Mat4_HeaderFile