ScummVM API documentation
dgMatrix.h
1 /* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 #ifndef __dgMatrix__
23 #define __dgMatrix__
24 
25 
26 #include "dgStdafx.h"
27 #include "dgDebug.h"
28 #include "dgVector.h"
29 #include "dgPlane.h"
30 #include "dgSimd_Instrutions.h"
31 #include <math.h>
32 
33 class dgMatrix;
34 class dgQuaternion;
35 
36 const dgMatrix &dgGetZeroMatrix();
37 const dgMatrix &dgGetIdentityMatrix();
38 
39 
40 DG_MSC_VECTOR_ALIGMENT
41 class dgMatrix {
42 public:
43  DG_CLASS_ALLOCATOR(allocator)
44 
45  dgMatrix();
46  constexpr dgMatrix(const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit);
47  dgMatrix(const dgQuaternion &rotation, const dgVector &position);
48 
49  // create a orthonormal normal vector basis
50  dgMatrix(const dgVector &front);
51 
52 
53 
54 
55  dgVector &operator[](dgInt32 i);
56  const dgVector &operator[](dgInt32 i) const;
57 
58  dgMatrix Inverse() const;
59  dgMatrix Inverse4x4() const;
60  dgMatrix Transpose() const;
61  dgMatrix Transpose4X4() const;
62  dgMatrix Symetric3by3Inverse() const;
63  dgVector RotateVector(const dgVector &v) const;
64  dgVector UnrotateVector(const dgVector &v) const;
65  dgVector TransformVector(const dgVector &v) const;
66  dgVector UntransformVector(const dgVector &v) const;
67  dgPlane TransformPlane(const dgPlane &localPlane) const;
68  dgPlane UntransformPlane(const dgPlane &globalPlane) const;
69  void TransformBBox(const dgVector &p0local, const dgVector &p1local, dgVector &p0, dgVector &p1) const;
70 
71  dgVector CalcPitchYawRoll() const;
72 
73 
74  void TransformTriplex(dgFloat32 *const dst, dgInt32 dstStrideInBytes,
75  const dgFloat32 *const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
76 
77 #ifndef __USE_DOUBLE_PRECISION__
78  void TransformTriplex(dgFloat64 *const dst, dgInt32 dstStrideInBytes,
79  const dgFloat64 *const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
80  void TransformTriplex(dgFloat64 *const dst, dgInt32 dstStrideInBytes,
81  const dgFloat32 *const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
82 #endif
83 
84  dgMatrix operator* (const dgMatrix &B) const;
85 
86 
87  // this function can not be a member of dgMatrix, because
88  // dgMatrix a define to handle only orthogonal matrices
89  // and this function take a parameter to a symmetric matrix
90  void EigenVectors(dgVector &eigenValues, const dgMatrix &initialGuess = dgGetIdentityMatrix());
91  void EigenVectors(const dgMatrix &initialGuess = dgGetIdentityMatrix());
92 
93 
94  // simd operations
95  dgMatrix InverseSimd() const;
96  dgMatrix MultiplySimd(const dgMatrix &B) const;
97  dgVector RotateVectorSimd(const dgVector &v) const;
98  dgVector UnrotateVectorSimd(const dgVector &v) const;
99  dgVector TransformVectorSimd(const dgVector &v) const;
100  void TransformVectorsSimd(dgVector *const dst, const dgVector *const src, dgInt32 count) const;
101 
102  dgVector m_front;
103  dgVector m_up;
104  dgVector m_right;
105  dgVector m_posit;
106 } DG_GCC_VECTOR_ALIGMENT;
107 
108 
109 
110 
111 
112 DG_INLINE dgMatrix::dgMatrix() {
113 }
114 
115 constexpr DG_INLINE dgMatrix::dgMatrix(const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit)
116  : m_front(front), m_up(up), m_right(right), m_posit(posit) {
117 }
118 
119 DG_INLINE dgMatrix::dgMatrix(const dgVector &front) {
120  m_front = front;
121  if (dgAbsf(front.m_z) > dgFloat32(0.577f)) {
122  m_right = front * dgVector(-front.m_y, front.m_z, dgFloat32(0.0f), dgFloat32(0.0f));
123  } else {
124  m_right = front * dgVector(-front.m_y, front.m_x, dgFloat32(0.0f), dgFloat32(0.0f));
125  }
126  m_right = m_right.Scale(dgRsqrt(m_right % m_right));
127  m_up = m_right * m_front;
128 
129  m_front.m_w = dgFloat32(0.0f);
130  m_up.m_w = dgFloat32(0.0f);
131  m_right.m_w = dgFloat32(0.0f);
132  m_posit = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
133 
134  NEWTON_ASSERT((dgAbsf(m_front % m_front) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
135  NEWTON_ASSERT((dgAbsf(m_up % m_up) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
136  NEWTON_ASSERT((dgAbsf(m_right % m_right) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
137  NEWTON_ASSERT((dgAbsf(m_right % (m_front * m_up)) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
138 }
139 
140 
141 DG_INLINE dgVector &dgMatrix::operator[](dgInt32 i) {
142  NEWTON_ASSERT(i < 4);
143  NEWTON_ASSERT(i >= 0);
144  return (&m_front)[i];
145 }
146 
147 DG_INLINE const dgVector &dgMatrix::operator[](dgInt32 i) const {
148  NEWTON_ASSERT(i < 4);
149  NEWTON_ASSERT(i >= 0);
150  return (&m_front)[i];
151 }
152 
153 
154 DG_INLINE dgMatrix dgMatrix::Transpose() const {
155  return dgMatrix(dgVector(m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
156  dgVector(m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
157  dgVector(m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
158  dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
159 }
160 
161 DG_INLINE dgMatrix dgMatrix::Transpose4X4() const {
162  return dgMatrix(dgVector(m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x),
163  dgVector(m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y),
164  dgVector(m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z),
165  dgVector(m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w));
166 
167 }
168 
169 DG_INLINE dgVector dgMatrix::RotateVector(const dgVector &v) const {
170  return dgVector(v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x,
171  v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y,
172  v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z, v.m_w);
173 }
174 
175 
176 DG_INLINE dgVector dgMatrix::UnrotateVector(const dgVector &v) const {
177  return dgVector(v % m_front, v % m_up, v % m_right, v.m_w);
178 }
179 
180 
181 DG_INLINE dgVector dgMatrix::TransformVector(const dgVector &v) const {
182 // return m_posit + RotateVector(v);
183  return dgVector(v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x + m_posit.m_x,
184  v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y + m_posit.m_y,
185  v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z + m_posit.m_z, v.m_w);
186 
187 }
188 
189 DG_INLINE dgVector dgMatrix::UntransformVector(const dgVector &v) const {
190  return UnrotateVector(v - m_posit);
191 }
192 
193 DG_INLINE dgPlane dgMatrix::TransformPlane(const dgPlane &localPlane) const {
194  return dgPlane(RotateVector(localPlane), localPlane.m_w - (localPlane % UnrotateVector(m_posit)));
195 }
196 
197 DG_INLINE dgPlane dgMatrix::UntransformPlane(const dgPlane &globalPlane) const {
198  return dgPlane(UnrotateVector(globalPlane), globalPlane.Evalue(m_posit));
199 }
200 
201 DG_INLINE void dgMatrix::EigenVectors(const dgMatrix &initialGuess) {
202  dgVector eigenValues;
203  EigenVectors(eigenValues, initialGuess);
204 }
205 
206 
207 DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang) {
208  dgFloat32 cosAng;
209  dgFloat32 sinAng;
210  sinAng = dgSin(ang);
211  cosAng = dgCos(ang);
212  return dgMatrix(dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
213  dgVector(dgFloat32(0.0f), cosAng, sinAng, dgFloat32(0.0f)),
214  dgVector(dgFloat32(0.0f), -sinAng, cosAng, dgFloat32(0.0f)),
215  dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
216 
217 }
218 
219 DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang) {
220  dgFloat32 cosAng;
221  dgFloat32 sinAng;
222  sinAng = dgSin(ang);
223  cosAng = dgCos(ang);
224  return dgMatrix(dgVector(cosAng, dgFloat32(0.0f), -sinAng, dgFloat32(0.0f)),
225  dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
226  dgVector(sinAng, dgFloat32(0.0f), cosAng, dgFloat32(0.0f)),
227  dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
228 }
229 
230 DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang) {
231  dgFloat32 cosAng;
232  dgFloat32 sinAng;
233  sinAng = dgSin(ang);
234  cosAng = dgCos(ang);
235  return dgMatrix(dgVector(cosAng, sinAng, dgFloat32(0.0f), dgFloat32(0.0f)),
236  dgVector(-sinAng, cosAng, dgFloat32(0.0f), dgFloat32(0.0f)),
237  dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
238  dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
239 }
240 
241 
242 DG_INLINE dgMatrix dgMatrix::Inverse() const {
243  return dgMatrix(dgVector(m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
244  dgVector(m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
245  dgVector(m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
246  dgVector(- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), dgFloat32(1.0f)));
247 }
248 
249 DG_INLINE dgVector dgMatrix::TransformVectorSimd(const dgVector &v) const {
250 #ifdef DG_BUILD_SIMD_CODE
251  const dgMatrix &source = *this;
252  return dgVector(simd_mul_add_v(
253  simd_mul_add_v(
254  simd_mul_add_v((simd_type &) source[3], (simd_type &) source[0], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(0, 0, 0, 0))),
255  (simd_type &) source[1], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(1, 1, 1, 1))),
256  (simd_type &) source[2], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(2, 2, 2, 2))));
257 #else
258  return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
259 #endif
260 }
261 
262 DG_INLINE void dgMatrix::TransformVectorsSimd(dgVector *const dst, const dgVector *const src, dgInt32 count) const {
263 #ifdef DG_BUILD_SIMD_CODE
264  const dgMatrix &source = *this;
265  for (dgInt32 i = 0; i < count; i ++) {
266  (simd_type &)dst[i] = simd_mul_add_v(
267  simd_mul_add_v(
268  simd_mul_add_v((simd_type &) source[3],
269  (simd_type &) source[0], simd_permut_v((simd_type &) src[i], (simd_type &) src[i], PURMUT_MASK(0, 0, 0, 0))),
270  (simd_type &) source[1], simd_permut_v((simd_type &) src[i], (simd_type &) src[i], PURMUT_MASK(1, 1, 1, 1))),
271  (simd_type &) source[2], simd_permut_v((simd_type &) src[i], (simd_type &) src[i], PURMUT_MASK(2, 2, 2, 2)));
272  }
273 #endif
274 }
275 
276 
277 DG_INLINE dgVector dgMatrix::RotateVectorSimd(const dgVector &v) const {
278 #ifdef DG_BUILD_SIMD_CODE
279  const dgMatrix &source = *this;
280  return dgVector(simd_mul_add_v(
281  simd_mul_add_v(
282  simd_mul_v((simd_type &) source[0], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(0, 0, 0, 0))),
283  (simd_type &) source[1], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(1, 1, 1, 1))),
284  (simd_type &) source[2], simd_permut_v((simd_type &) v, (simd_type &) v, PURMUT_MASK(2, 2, 2, 2))));
285 
286 #else
287  return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
288 #endif
289 }
290 
291 DG_INLINE dgVector dgMatrix::UnrotateVectorSimd(const dgVector &v) const {
292 #ifdef DG_BUILD_SIMD_CODE
293  return dgVector(v.DotProductSimd(m_front), v.DotProductSimd(m_up), v.DotProductSimd(m_right), v.m_w);
294 #else
295  return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
296 #endif
297 }
298 
299 DG_INLINE dgMatrix dgMatrix::InverseSimd() const {
300 #ifdef DG_BUILD_SIMD_CODE
301  simd_type r0;
302  simd_type r1;
303  simd_type r2;
304  dgMatrix matrix;
305  const dgMatrix &source = *this;
306 
307  NEWTON_ASSERT((dgUnsigned64(this) & 0x0f) == 0);
308 
309  r2 = simd_set1(dgFloat32(0.0f));
310  r0 = simd_pack_lo_v((simd_type &) source[0], (simd_type &) source[1]);
311  r1 = simd_pack_lo_v((simd_type &) source[2], r2);
312  (simd_type &) matrix[0] = simd_move_lh_v(r0, r1);
313  (simd_type &) matrix[1] = simd_move_hl_v(r1, r0);
314  r0 = simd_pack_hi_v((simd_type &) source[0], (simd_type &) source[1]);
315  r1 = simd_pack_hi_v((simd_type &) source[2], r2);
316  (simd_type &) matrix[2] = simd_move_lh_v(r0, r1);
317 
318  (simd_type &) matrix[3] = simd_sub_v(r2,
319  simd_mul_add_v(
320  simd_mul_add_v(simd_mul_v((simd_type &) matrix[0], simd_permut_v((simd_type &) source[3], (simd_type &) source[3], PURMUT_MASK(3, 0, 0, 0))),
321  (simd_type &) matrix[1], simd_permut_v((simd_type &) source[3], (simd_type &) source[3], PURMUT_MASK(3, 1, 1, 1))),
322  (simd_type &) matrix[2], simd_permut_v((simd_type &) source[3], (simd_type &) source[3], PURMUT_MASK(3, 2, 2, 2))));
323  matrix[3][3] = dgFloat32(1.0f);
324  return matrix;
325 
326 
327 #else
328  return dgGetIdentityMatrix();
329 #endif
330 }
331 
332 DG_INLINE dgMatrix dgMatrix::MultiplySimd(const dgMatrix &B) const {
333 #ifdef DG_BUILD_SIMD_CODE
334  const dgMatrix &A = *this;
335  return dgMatrix(dgVector(simd_mul_add_v(
336  simd_mul_add_v(
337  simd_mul_add_v(simd_mul_v((simd_type &) B[0], simd_permut_v((simd_type &) A[0], (simd_type &) A[0], PURMUT_MASK(0, 0, 0, 0))),
338  (simd_type &) B[1], simd_permut_v((simd_type &) A[0], (simd_type &) A[0], PURMUT_MASK(1, 1, 1, 1))),
339  (simd_type &) B[2], simd_permut_v((simd_type &) A[0], (simd_type &) A[0], PURMUT_MASK(2, 2, 2, 2))),
340  (simd_type &) B[3], simd_permut_v((simd_type &) A[0], (simd_type &) A[0], PURMUT_MASK(3, 3, 3, 3)))),
341 
342  dgVector(simd_mul_add_v(
343  simd_mul_add_v(
344  simd_mul_add_v(simd_mul_v((simd_type &) B[0], simd_permut_v((simd_type &) A[1], (simd_type &) A[1], PURMUT_MASK(0, 0, 0, 0))),
345  (simd_type &) B[1], simd_permut_v((simd_type &) A[1], (simd_type &) A[1], PURMUT_MASK(1, 1, 1, 1))),
346  (simd_type &) B[2], simd_permut_v((simd_type &) A[1], (simd_type &) A[1], PURMUT_MASK(2, 2, 2, 2))),
347  (simd_type &) B[3], simd_permut_v((simd_type &) A[1], (simd_type &) A[1], PURMUT_MASK(3, 3, 3, 3)))),
348 
349  dgVector(simd_mul_add_v(
350  simd_mul_add_v(
351  simd_mul_add_v(simd_mul_v((simd_type &) B[0], simd_permut_v((simd_type &) A[2], (simd_type &) A[2], PURMUT_MASK(0, 0, 0, 0))),
352  (simd_type &) B[1], simd_permut_v((simd_type &) A[2], (simd_type &) A[2], PURMUT_MASK(1, 1, 1, 1))),
353  (simd_type &) B[2], simd_permut_v((simd_type &) A[2], (simd_type &) A[2], PURMUT_MASK(2, 2, 2, 2))),
354  (simd_type &) B[3], simd_permut_v((simd_type &) A[2], (simd_type &) A[2], PURMUT_MASK(3, 3, 3, 3)))),
355 
356 
357  dgVector(simd_mul_add_v(
358  simd_mul_add_v(
359  simd_mul_add_v(simd_mul_v((simd_type &) B[0], simd_permut_v((simd_type &) A[3], (simd_type &) A[3], PURMUT_MASK(0, 0, 0, 0))),
360  (simd_type &) B[1], simd_permut_v((simd_type &) A[3], (simd_type &) A[3], PURMUT_MASK(1, 1, 1, 1))),
361  (simd_type &) B[2], simd_permut_v((simd_type &) A[3], (simd_type &) A[3], PURMUT_MASK(2, 2, 2, 2))),
362  (simd_type &) B[3], simd_permut_v((simd_type &) A[3], (simd_type &) A[3], PURMUT_MASK(3, 3, 3, 3)))));
363 #else
364  return dgGetIdentityMatrix();
365 
366 #endif
367 }
368 
369 
370 #endif
371 
dgPlane
Definition: dgPlane.h:29
dgVector
Definition: dgVector.h:86
dgQuaternion
Definition: dgQuaternion.h:31
dgMatrix
Definition: dgMatrix.h:41