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

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