VR-Vantage 2.8 API Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DtMatrix4.h
Go to the documentation of this file.
1 /******************************************************************************
2 ** Copyright (c) 2020 MAK Technologies, Inc.
3 ** All rights reserved.
4 ******************************************************************************/
5 
9 #pragma once
10 
11 #include <vrvGraphicsUtils/DtAssert.h>
12 #include <vrvGraphicsUtils/DtMatrix3.h>
13 
14 #include <cstring>
15 
16 namespace makVrv
17 {
18 
21  template<class T>
22  class DtMatrix4
23  {
24  public:
26  DtMatrix4();
28  DtMatrix4(const T rhs[4][4]);
30  DtMatrix4(const T rhs[16]);
32  DtMatrix4(T m00, T m01, T m02, T m03
33  , T m10, T m11, T m12, T m13
34  , T m20, T m21, T m22, T m23
35  , T m30, T m31, T m32, T m33);
39  DtMatrix4(const DtMatrix3<T>& rhs);
40 
42  ~DtMatrix4();
43  public:
44 
46  void operator=(const DtMatrix3<T>& rhs);
47 
49  DtMatrix4 operator*(const DtMatrix4& rhs) const;
50 
52  DtVector4<T> operator*(const DtVector4<T>& rhs) const;
53 
55  void setZero(void);
56 
58  void setIdentity(void);
59 
61  DtMatrix4 getTranspose(void) const;
62 
64  bool isEqual(const DtMatrix4& rhs, T tolerance) const;
65 
66  bool isIdentity(T tolerance = std::numeric_limits<T>::epsilon()) const;
67 
68  DtMatrix4 convert_depth_gl_to_dx() const;
69 
70  const T* operator [](size_t iRow) const;
71  T* operator [](size_t iRow);
72 
73  const void* ptr(void) const;
74 
75  static DtMatrix4 theIdentityMatrix;
76 
78  bool isAffine(void) const;
79 
80  DtVector3<T> transformAffine(const DtVector3<T>& v) const;
81  DtVector4<T> transformAffine(const DtVector4<T>& v) const;
82 
84  DtMatrix4<T> getInverse(void) const;
85 
87  // begin with
88  DtMatrix4<T> getInverseAffine(void) const;
89 
91  void zeroOutTranslation(void);
92 
94  void translateBack(const DtVector3<T>& v);
95 
97  bool operator==(const DtMatrix4<T>& rhs) const;
98 
100  bool operator!=(const DtMatrix4<T>& rhs) const;
101  private:
102  union
103  {
104  T _m[16];
105  T m[4][4];
106  };
107  };
108 
109  template <class T>
110  DtMatrix4<T> DtMatrix4<T>::theIdentityMatrix = DtMatrix4<T>(1,0,0,0
111  , 0,1,0,0
112  , 0,0,1,0
113  , 0,0,0,1);
114 
115  typedef DtMatrix4<float> Matrix4_32;
116  typedef DtMatrix4<double> Matrix4_64;
117 
118  template<class T>
119  DtMatrix4<T>::DtMatrix4()
120  {
121 
122  }
123 
124  template<class T>
125  DtMatrix4<T>::DtMatrix4(const T rhs[4][4])
126  {
127  memcpy(_m, rhs, 16*sizeof(T));
128  }
129 
130  template<class T>
131  DtMatrix4<T>::DtMatrix4(T m00, T m01, T m02, T m03
132  , T m10, T m11, T m12, T m13
133  , T m20, T m21, T m22, T m23
134  , T m30, T m31, T m32, T m33)
135  {
136  m[0][0]=m00;m[0][1]=m01;m[0][2]=m02;m[0][3]=m03;
137  m[1][0]=m10;m[1][1]=m11;m[1][2]=m12;m[1][3]=m13;
138  m[2][0]=m20;m[2][1]=m21;m[2][2]=m22;m[2][3]=m23;
139  m[3][0]=m30;m[3][1]=m31;m[3][2]=m32;m[3][3]=m33;
140  }
141 
142  template<class T>
143  DtMatrix4<T>::DtMatrix4(const T rhs[16])
144  {
145  memcpy(m, rhs, 16*sizeof(T));
146  }
147 
148  template<class T>
149  DtMatrix4<T>::~DtMatrix4()
150  {
151  //intentional nop
152  }
153 
154  template<class T>
155  const void* DtMatrix4<T>::ptr(void) const
156  {
157  return _m;
158  }
159 
160  template<class T>
161  const T* DtMatrix4<T>::operator [](size_t iRow) const
162  {
163  ASSERT_PREDICATE_RETURN_ZERO(iRow<4);
164  return m[iRow];
165  }
166  template<class T>
167  T* DtMatrix4<T>::operator [](size_t iRow)
168  {
169  ASSERT_PREDICATE_RETURN_ZERO(iRow<4);
170  return m[iRow];
171  }
172 
173  template<class T>
174  DtMatrix4<T> DtMatrix4<T>::operator*(const DtMatrix4<T>& rhs) const
175  {
176  /*
177  | lhs.00, lhs.01, lhs.02, lhs.03 | | rhs.00, rhs.01, rhs.02, rhs.03 |
178  | lhs.10, lhs.11, lhs.12, lhs.13 | | rhs.10, rhs.11, rhs.12, rhs.13 |
179  | lhs.20, lhs.21, lhs.22, lhs.23 | | rhs.20, rhs.21, rhs.22, rhs.23 |
180  | lhs.30, lhs.31, lhs.32, lhs.33 | | rhs.30, rhs.31, rhs.32, rhs.33 |
181  */
182  const DtMatrix4& lhs = *this;
183 
184  DtMatrix4 result;
185 
186  result.m[0][0] = lhs.m[0][0]*rhs.m[0][0] + lhs.m[0][1]*rhs.m[1][0] + lhs.m[0][2]*rhs.m[2][0] + lhs.m[0][3]*rhs.m[3][0];
187  result.m[0][1] = lhs.m[0][0]*rhs.m[0][1] + lhs.m[0][1]*rhs.m[1][1] + lhs.m[0][2]*rhs.m[2][1] + lhs.m[0][3]*rhs.m[3][1];
188  result.m[0][2] = lhs.m[0][0]*rhs.m[0][2] + lhs.m[0][1]*rhs.m[1][2] + lhs.m[0][2]*rhs.m[2][2] + lhs.m[0][3]*rhs.m[3][2];
189  result.m[0][3] = lhs.m[0][0]*rhs.m[0][3] + lhs.m[0][1]*rhs.m[1][3] + lhs.m[0][2]*rhs.m[2][3] + lhs.m[0][3]*rhs.m[3][3];
190 
191  result.m[1][0] = lhs.m[1][0]*rhs.m[0][0] + lhs.m[1][1]*rhs.m[1][0] + lhs.m[1][2]*rhs.m[2][0] + lhs.m[1][3]*rhs.m[3][0];
192  result.m[1][1] = lhs.m[1][0]*rhs.m[0][1] + lhs.m[1][1]*rhs.m[1][1] + lhs.m[1][2]*rhs.m[2][1] + lhs.m[1][3]*rhs.m[3][1];
193  result.m[1][2] = lhs.m[1][0]*rhs.m[0][2] + lhs.m[1][1]*rhs.m[1][2] + lhs.m[1][2]*rhs.m[2][2] + lhs.m[1][3]*rhs.m[3][2];
194  result.m[1][3] = lhs.m[1][0]*rhs.m[0][3] + lhs.m[1][1]*rhs.m[1][3] + lhs.m[1][2]*rhs.m[2][3] + lhs.m[1][3]*rhs.m[3][3];
195 
196  result.m[2][0] = lhs.m[2][0]*rhs.m[0][0] + lhs.m[2][1]*rhs.m[1][0] + lhs.m[2][2]*rhs.m[2][0] + lhs.m[2][3]*rhs.m[3][0];
197  result.m[2][1] = lhs.m[2][0]*rhs.m[0][1] + lhs.m[2][1]*rhs.m[1][1] + lhs.m[2][2]*rhs.m[2][1] + lhs.m[2][3]*rhs.m[3][1];
198  result.m[2][2] = lhs.m[2][0]*rhs.m[0][2] + lhs.m[2][1]*rhs.m[1][2] + lhs.m[2][2]*rhs.m[2][2] + lhs.m[2][3]*rhs.m[3][2];
199  result.m[2][3] = lhs.m[2][0]*rhs.m[0][3] + lhs.m[2][1]*rhs.m[1][3] + lhs.m[2][2]*rhs.m[2][3] + lhs.m[2][3]*rhs.m[3][3];
200 
201  result.m[3][0] = lhs.m[3][0]*rhs.m[0][0] + lhs.m[3][1]*rhs.m[1][0] + lhs.m[3][2]*rhs.m[2][0] + lhs.m[3][3]*rhs.m[3][0];
202  result.m[3][1] = lhs.m[3][0]*rhs.m[0][1] + lhs.m[3][1]*rhs.m[1][1] + lhs.m[3][2]*rhs.m[2][1] + lhs.m[3][3]*rhs.m[3][1];
203  result.m[3][2] = lhs.m[3][0]*rhs.m[0][2] + lhs.m[3][1]*rhs.m[1][2] + lhs.m[3][2]*rhs.m[2][2] + lhs.m[3][3]*rhs.m[3][2];
204  result.m[3][3] = lhs.m[3][0]*rhs.m[0][3] + lhs.m[3][1]*rhs.m[1][3] + lhs.m[3][2]*rhs.m[2][3] + lhs.m[3][3]*rhs.m[3][3];
205 
206  return result;
207  }
208 
209 
210  template<class T>
211  DtVector4<T> DtMatrix4<T>::operator*(const DtVector4<T>& rhs) const
212  {
213  /*
214  | lhs.00, lhs.01, lhs.02, lhs.03 | | rhs.0 |
215  | lhs.10, lhs.11, lhs.12, lhs.13 | | rhs.1 |
216  | lhs.20, lhs.21, lhs.22, lhs.23 | | rhs.2 |
217  | lhs.30, lhs.31, lhs.32, lhs.33 | | rhs.3 |
218  */
219  DtVector4<T> vResult;
220 
221  const DtMatrix4& lhs = *this;
222 
223  vResult.x = lhs.m[0][0]*rhs.x + lhs.m[0][1]*rhs.y + lhs.m[0][2]*rhs.z + lhs.m[0][3]*rhs.w;
224  vResult.y = lhs.m[1][0]*rhs.x + lhs.m[1][1]*rhs.y + lhs.m[1][2]*rhs.z + lhs.m[1][3]*rhs.w;
225  vResult.z = lhs.m[2][0]*rhs.x + lhs.m[2][1]*rhs.y + lhs.m[2][2]*rhs.z + lhs.m[2][3]*rhs.w;
226  vResult.w = lhs.m[3][0]*rhs.x + lhs.m[3][1]*rhs.y + lhs.m[3][2]*rhs.z + lhs.m[3][3]*rhs.w;
227 
228  return vResult;
229  }
230 
231  template<class T>
232  void DtMatrix4<T>::setZero(void)
233  {
234  memset(_m, 0, 16*sizeof(T));
235  }
236 
237  template<class T>
238  void DtMatrix4<T>::setIdentity(void)
239  {
240  DtMatrix4<T>::setZero();
241  m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1;
242  }
243 
244  template<class T>
245  DtMatrix4<T> DtMatrix4<T>::getTranspose(void) const
246  {
247  return DtMatrix4<T>(
248  m[0][0], m[1][0], m[2][0], m[3][0]
249  , m[0][1], m[1][1], m[2][1], m[3][1]
250  , m[0][2], m[1][2], m[2][2], m[3][2]
251  , m[0][3], m[1][3], m[2][3], m[3][3]
252  );
253  }
254 
255  template<class T>
256  DtMatrix4<T> DtMatrix4<T>::convert_depth_gl_to_dx() const
257  {
258  DtMatrix4<T> result = *this;
259  result[2][0] = (result[2][0] + result[3][0])*0.5f;
260  result[2][1] = (result[2][1] + result[3][1])*0.5f;
261  result[2][2] = (result[2][2] + result[3][2])*0.5f;
262  result[2][3] = (result[2][3] + result[3][3])*0.5f;
263  return result;
264  }
265 
266  template<class T>
267  bool DtMatrix4<T>::isEqual(const DtMatrix4<T>& rhs, T tolerance) const
268  {
269  const DtMatrix4& lhs = *this;
270  for(size_t i = 0; i < 4; ++i)
271  {
272  for(size_t j = 0; j < 4; ++j)
273  {
274  if (!DtMath::isEqual(lhs[i][j], rhs[i][j], tolerance))
275  return false;
276  }
277  }
278  return true;
279  }
280 
281  template<class T>
282  bool DtMatrix4<T>::isIdentity(T tolerance) const
283  {
284  DtMatrix4 rhs; rhs.setIdentity();
285  return isEqual(rhs, tolerance);
286  }
287 
288  template<class T>
289  DtMatrix4<T>::DtMatrix4(const DtMatrix3<T>& rhs)
290  {
291  operator=(rhs);
292  }
293  template<class T>
294  void DtMatrix4<T>::operator=(const DtMatrix3<T>& rhs)
295  {
296  setIdentity();
297 
298  m[0][0] = rhs[0][0]; m[0][1] = rhs[0][1]; m[0][2] = rhs[0][2];
299  m[1][0] = rhs[1][0]; m[1][1] = rhs[1][1]; m[1][2] = rhs[1][2];
300  m[2][0] = rhs[2][0]; m[2][1] = rhs[2][1]; m[2][2] = rhs[2][2];
301  }
302 
303  template<class T>
304  bool DtMatrix4<T>::isAffine(void) const
305  {
306  return m[3][0] == 0 && m[3][1] == 0 && m[3][2] == 0 && m[3][3] == 1;
307  }
308 
309  template<class T>
310  DtVector3<T> DtMatrix4<T>::transformAffine(const DtVector3<T>& v) const
311  {
312  ASSERT_PREDICATE(IsAffine());
313  if (DtMatrix4<T>::IsAffine()==false)
314  return DtVector3<T>::theZeroVector;
315 
316  return DtVector3<T>(
317  m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + + m[0][3]
318  , m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + + m[1][3]
319  , m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + + m[2][3]
320  );
321  }
322 
323  template<class T>
324  DtVector4<T> DtMatrix4<T>::transformAffine(const DtVector4<T>& v) const
325  {
326  ASSERT_PREDICATE(IsAffine());
327  if (DtMatrix4<T>::IsAffine()==false)
328  return DtVector4<T>::ZERO;
329 
330  return DtVector4<T>(
331  m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + + m[0][3]
332  , m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + + m[1][3]
333  , m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + + m[2][3]
334  , v.w
335  );
336  }
337 
338  template<class T>
339  DtMatrix4<T> DtMatrix4<T>::getInverse(void) const
340  {
341  T m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
342  T m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
343  T m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
344  T m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
345 
346  T v0 = m20 * m31 - m21 * m30;
347  T v1 = m20 * m32 - m22 * m30;
348  T v2 = m20 * m33 - m23 * m30;
349  T v3 = m21 * m32 - m22 * m31;
350  T v4 = m21 * m33 - m23 * m31;
351  T v5 = m22 * m33 - m23 * m32;
352 
353  T t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
354  T t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
355  T t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
356  T t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
357 
358  T invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
359 
360  T d00 = t00 * invDet;
361  T d10 = t10 * invDet;
362  T d20 = t20 * invDet;
363  T d30 = t30 * invDet;
364 
365  T d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
366  T d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
367  T d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
368  T d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
369 
370  v0 = m10 * m31 - m11 * m30;
371  v1 = m10 * m32 - m12 * m30;
372  v2 = m10 * m33 - m13 * m30;
373  v3 = m11 * m32 - m12 * m31;
374  v4 = m11 * m33 - m13 * m31;
375  v5 = m12 * m33 - m13 * m32;
376 
377  T d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
378  T d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
379  T d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
380  T d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
381 
382  v0 = m21 * m10 - m20 * m11;
383  v1 = m22 * m10 - m20 * m12;
384  v2 = m23 * m10 - m20 * m13;
385  v3 = m22 * m11 - m21 * m12;
386  v4 = m23 * m11 - m21 * m13;
387  v5 = m23 * m12 - m22 * m13;
388 
389  T d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
390  T d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
391  T d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
392  T d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
393 
394  return DtMatrix4(
395  d00, d01, d02, d03,
396  d10, d11, d12, d13,
397  d20, d21, d22, d23,
398  d30, d31, d32, d33);
399  }
400 
401  template<class T>
402  DtMatrix4<T> DtMatrix4<T>::getInverseAffine(void) const
403  {
404  ASSERT_PREDICATE(IsAffine());
405 
406  T m10 = m[1][0], m11 = m[1][1], m12 = m[1][2];
407  T m20 = m[2][0], m21 = m[2][1], m22 = m[2][2];
408 
409  T t00 = m22 * m11 - m21 * m12;
410  T t10 = m20 * m12 - m22 * m10;
411  T t20 = m21 * m10 - m20 * m11;
412 
413  T m00 = m[0][0], m01 = m[0][1], m02 = m[0][2];
414 
415  T invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20);
416 
417  t00 *= invDet; t10 *= invDet; t20 *= invDet;
418 
419  m00 *= invDet; m01 *= invDet; m02 *= invDet;
420 
421  T r00 = t00;
422  T r01 = m02 * m21 - m01 * m22;
423  T r02 = m01 * m12 - m02 * m11;
424 
425  T r10 = t10;
426  T r11 = m00 * m22 - m02 * m20;
427  T r12 = m02 * m10 - m00 * m12;
428 
429  T r20 = t20;
430  T r21 = m01 * m20 - m00 * m21;
431  T r22 = m00 * m11 - m01 * m10;
432 
433  T m03 = m[0][3], m13 = m[1][3], m23 = m[2][3];
434 
435  T r03 = - (r00 * m03 + r01 * m13 + r02 * m23);
436  T r13 = - (r10 * m03 + r11 * m13 + r12 * m23);
437  T r23 = - (r20 * m03 + r21 * m13 + r22 * m23);
438 
439  return DtMatrix4<T>(
440  r00, r01, r02, r03,
441  r10, r11, r12, r13,
442  r20, r21, r22, r23,
443  0, 0, 0, 1);
444  }
445 
446  template<class T>
447  void DtMatrix4<T>::zeroOutTranslation(void)
448  {
449  m[0][3] = 0;
450  m[1][3] = 0;
451  m[2][3] = 0;
452  }
453 
454  template<class T>
455  void DtMatrix4<T>::translateBack(const DtVector3<T>& v)
456  {
457  m[0][3] = m[0][3] - v.x;
458  m[1][3] = m[1][3] - v.y;
459  m[2][3] = m[2][3] - v.z;
460  }
461 
462  template<class T>
463  bool DtMatrix4<T>::operator==(const DtMatrix4<T>& rhs) const
464  {
465  return (memcmp(ptr(), rhs.ptr(), sizeof(T) * 16) == 0);
466  }
467 
468  template<class T>
469  bool DtMatrix4<T>::operator!=(const DtMatrix4<T>& rhs) const
470  {
471  return (memcmp(ptr(), rhs.ptr(), sizeof(T) * 16) != 0);
472  }
473 
474  template<class T>
475  inline std::ostream& operator <<
476  ( std::ostream& o, const DtMatrix4<T>& mat )
477  {
478  for(size_t i = 0; i < 4; ++i)
479  {
480  o<<mat[i][0]<<" "<<mat[i][1]<<" "<<mat[i][2]<<" "<<mat[i][3]<<std::endl;
481  }
482  return o;
483  }
484 
485 } //namespace makVrv

Copyright © 2005-2021 MAK Technologies. All Rights Reserved (www.mak.com)