Newton Dynamics  4.00
dVectorScalar.h
1 /* Copyright (c) <2003-2019> <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 __D_VECTOR_SCALAR_H__
23 #define __D_VECTOR_SCALAR_H__
24 
25 // *****************************************************************************************
26 //
27 // 4 x 1 single precision vector class declaration
28 //
29 // *****************************************************************************************
30 #ifdef D_NEWTON_USE_DOUBLE
31  #define dVector dBigVector
32 #else
33 
34 class dBigVector;
35 class dVector
36 {
37  public:
38  D_INLINE dVector()
39  {
40  }
41 
42  D_INLINE dVector(dFloat32 val)
43  :m_x(val), m_y(val), m_z(val), m_w(val)
44  {
45  }
46 
47  D_INLINE dVector (const dVector& v)
48  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
49  {
50  //dAssert (dCheckVector ((*this)));
51  }
52 
53  D_INLINE dVector (const dFloat32* const ptr)
54  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3])
55  {
56  dAssert (dCheckVector ((*this)));
57  }
58 
59 #ifndef D_NEWTON_USE_DOUBLE
60  D_INLINE dVector(const dFloat64* const ptr)
61  :m_x(dFloat32(ptr[0]))
62  ,m_y(dFloat32(ptr[1]))
63  ,m_z(dFloat32(ptr[2]))
64  ,m_w(dFloat32(ptr[3]))
65  {
66  }
67 #endif
68 
69 
70  D_INLINE dVector (dFloat32 x, dFloat32 y, dFloat32 z, dFloat32 w)
71  :m_x(x), m_y(y), m_z(z), m_w(w)
72  {
73  dAssert (dCheckVector ((*this)));
74  }
75 
76  D_INLINE dVector (dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
77  :m_x(*((dFloat32*)&ix)), m_y(*((dFloat32*)&iy)), m_z(*((dFloat32*)&iz)), m_w(*((dFloat32*)&iw))
78  {
79  }
80 
81 #ifndef D_NEWTON_USE_DOUBLE
82  D_INLINE dVector (const dBigVector& copy)
83  :m_x(dFloat32 (((dFloat64*)&copy)[0]))
84  ,m_y(dFloat32 (((dFloat64*)&copy)[1]))
85  ,m_z(dFloat32 (((dFloat64*)&copy)[2]))
86  ,m_w(dFloat32 (((dFloat64*)&copy)[3]))
87  {
88  dAssert (dCheckVector ((*this)));
89  }
90 #endif
91 
92  D_INLINE dFloat32 GetScalar () const
93  {
94  return m_x;
95  }
96 
97  D_INLINE void Store (dFloat32* const dst) const
98  {
99  dst[0] = m_x;
100  dst[1] = m_y;
101  dst[2] = m_z;
102  dst[3] = m_w;
103  }
104 
105  D_INLINE dVector BroadcastX () const
106  {
107  return dVector (m_x);
108  }
109 
110  D_INLINE dVector BroadcastY () const
111  {
112  return dVector (m_y);
113  }
114 
115  D_INLINE dVector BroadcastZ () const
116  {
117  return dVector (m_z);
118  }
119 
120  D_INLINE dVector BroadcastW () const
121  {
122  return dVector (m_w);
123  }
124 
125 
126  D_INLINE dFloat32& operator[] (dInt32 i)
127  {
128  dAssert (i < 4);
129  dAssert (i >= 0);
130  return (&m_x)[i];
131  }
132 
133  D_INLINE const dFloat32& operator[] (dInt32 i) const
134  {
135  dAssert (i < 4);
136  dAssert (i >= 0);
137  return (&m_x)[i];
138  }
139 
140  D_INLINE dVector operator+ (const dVector& A) const
141  {
142  return dVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
143  }
144 
145  D_INLINE dVector operator- (const dVector& A) const
146  {
147  return dVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
148  }
149 
150  D_INLINE dVector operator* (const dVector& A) const
151  {
152  return dVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
153  }
154 
155  D_INLINE dVector& operator+= (const dVector& A)
156  {
157  return (*this = dVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
158  }
159 
160  D_INLINE dVector& operator-= (const dVector& A)
161  {
162  return (*this = dVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
163  }
164 
165  D_INLINE dVector& operator*= (const dVector& A)
166  {
167  return (*this = dVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
168  }
169 
170  D_INLINE dVector MulAdd(const dVector& A, const dVector& B) const
171  {
172  return *this + A * B;
173  }
174 
175  D_INLINE dVector MulSub(const dVector& A, const dVector& B) const
176  {
177  return *this - A * B;
178  }
179 
180  D_INLINE dVector AddHorizontal () const
181  {
182  return dVector (m_x + m_y + m_z + m_w);
183  }
184 
185  D_INLINE dVector Scale (dFloat32 scale) const
186  {
187  return dVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
188  }
189 
190  // return cross product
191  D_INLINE dVector CrossProduct (const dVector& B) const
192  {
193  return dVector (m_y * B.m_z - m_z * B.m_y,
194  m_z * B.m_x - m_x * B.m_z,
195  m_x * B.m_y - m_y * B.m_x, m_w);
196  }
197 
198  D_INLINE dVector CrossProduct (const dVector& A, const dVector& B) const
199  {
200  dFloat32 cofactor[3][3];
201  dFloat32 array[4][4];
202 
203  const dVector& me = *this;
204  for (dInt32 i = 0; i < 4; i ++)
205  {
206  array[0][i] = me[i];
207  array[1][i] = A[i];
208  array[2][i] = B[i];
209  array[3][i] = dFloat32 (1.0f);
210  }
211 
212  dVector normal;
213  dFloat32 sign = dFloat32 (-1.0f);
214  for (dInt32 i = 0; i < 4; i ++)
215  {
216  for (dInt32 j = 0; j < 3; j ++)
217  {
218  dInt32 k0 = 0;
219  for (dInt32 k = 0; k < 4; k ++)
220  {
221  if (k != i)
222  {
223  cofactor[j][k0] = array[j][k];
224  k0 ++;
225  }
226  }
227  }
228  dFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
229  dFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
230  dFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
231  dFloat32 det = x + y + z;
232 
233  normal[i] = sign * det;
234  sign *= dFloat32 (-1.0f);
235  }
236 
237  return normal;
238  }
239 
240  D_INLINE dVector GetInt () const
241  {
242  return dVector (dInt32 (dFloor (m_x)), dInt32(dFloor (m_y)), dInt32(dFloor (m_z)), dInt32 (dFloor (m_w)));
243  }
244 
245  D_INLINE dVector TestZero() const
246  {
247  const dInt32* const a = (dInt32*)&m_x;
248  return dVector ((a[0] == 0) ? dFloat32 (-1.0f) : dFloat32 (1.0f),
249  (a[1] == 0) ? dFloat32 (-1.0f) : dFloat32 (1.0f),
250  (a[2] == 0) ? dFloat32 (-1.0f) : dFloat32 (1.0f),
251  (a[3] == 0) ? dFloat32 (-1.0f) : dFloat32 (1.0f));
252  }
253 
254 
255  D_INLINE dVector Floor () const
256  {
257  return dVector (dFloor (m_x), dFloor (m_y), dFloor (m_z), dFloor (m_w));
258  }
259 
260  D_INLINE dVector DotProduct (const dVector &A) const
261  {
262  return dVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
263  }
264 
265  D_INLINE dVector Reciproc () const
266  {
267  return dVector (dFloat32 (1.0f) / m_x, dFloat32 (1.0f) / m_y, dFloat32 (1.0f) / m_z, dFloat32 (1.0f) / m_w);
268  }
269 
270  D_INLINE dVector Sqrt () const
271  {
272  return dVector (dSqrt (m_x), dSqrt (m_y), dSqrt (m_z), dSqrt (m_w));
273  }
274 
275  D_INLINE dVector InvSqrt () const
276  {
277  return dVector (dRsqrt (m_x), dRsqrt (m_y), dRsqrt (m_z), dRsqrt (m_w));
278  }
279 
280  D_INLINE dVector InvMagSqrt () const
281  {
282  return dVector (dRsqrt (DotProduct(*this).m_x));
283  }
284 
285  D_INLINE dVector Normalize () const
286  {
287  dAssert (m_w == dFloat32 (0.0f));
288  //return *this * dVector (dRsqrt (DotProduct(*this).m_x));
289  //return Scale (dRsqrt (DotProduct(*this).GetScalar()));
290  const dVector& me = *this;
291  return me * InvMagSqrt();
292  }
293 
294  dVector Abs () const
295  {
296  return dVector ((m_x > dFloat32 (0.0f)) ? m_x : -m_x,
297  (m_y > dFloat32 (0.0f)) ? m_y : -m_y,
298  (m_z > dFloat32 (0.0f)) ? m_z : -m_z,
299  (m_w > dFloat32 (0.0f)) ? m_w : -m_w);
300  }
301 
302  dFloat32 GetMax () const
303  {
304  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
305  }
306 
307  dVector GetMax (const dVector& data) const
308  {
309  return dVector ((m_x > data.m_x) ? m_x : data.m_x,
310  (m_y > data.m_y) ? m_y : data.m_y,
311  (m_z > data.m_z) ? m_z : data.m_z,
312  (m_w > data.m_w) ? m_w : data.m_w);
313  }
314 
315  dVector GetMin (const dVector& data) const
316  {
317  return dVector ((m_x < data.m_x) ? m_x : data.m_x,
318  (m_y < data.m_y) ? m_y : data.m_y,
319  (m_z < data.m_z) ? m_z : data.m_z,
320  (m_w < data.m_w) ? m_w : data.m_w);
321  }
322 
323 
324  // relational operators
325  D_INLINE dVector operator== (const dVector& data) const
326  {
327  return dVector ((m_x == data.m_x) ? dInt32 (0xffffffff) : 0,
328  (m_y == data.m_y) ? dInt32 (0xffffffff) : 0,
329  (m_z == data.m_z) ? dInt32 (0xffffffff) : 0,
330  (m_w == data.m_w) ? dInt32 (0xffffffff) : 0);
331  }
332 
333  D_INLINE dVector operator> (const dVector& data) const
334  {
335  return dVector ((m_x > data.m_x) ? dInt32 (0xffffffff) : 0,
336  (m_y > data.m_y) ? dInt32 (0xffffffff) : 0,
337  (m_z > data.m_z) ? dInt32 (0xffffffff) : 0,
338  (m_w > data.m_w) ? dInt32 (0xffffffff) : 0);
339  }
340 
341  D_INLINE dVector operator< (const dVector& data) const
342  {
343  return dVector ((m_x < data.m_x) ? dInt32 (0xffffffff) : 0,
344  (m_y < data.m_y) ? dInt32 (0xffffffff) : 0,
345  (m_z < data.m_z) ? dInt32 (0xffffffff) : 0,
346  (m_w < data.m_w) ? dInt32 (0xffffffff) : 0);
347  }
348 
349  D_INLINE dVector operator>= (const dVector& data) const
350  {
351  return dVector ((m_x >= data.m_x) ? dInt32 (0xffffffff) : 0,
352  (m_y >= data.m_y) ? dInt32 (0xffffffff) : 0,
353  (m_z >= data.m_z) ? dInt32 (0xffffffff) : 0,
354  (m_w >= data.m_w) ? dInt32 (0xffffffff) : 0);
355  }
356 
357  D_INLINE dVector operator<= (const dVector& data) const
358  {
359  return dVector ((m_x <= data.m_x) ? dInt32 (0xffffffff) : 0,
360  (m_y <= data.m_y) ? dInt32 (0xffffffff) : 0,
361  (m_z <= data.m_z) ? dInt32 (0xffffffff) : 0,
362  (m_w <= data.m_w) ? dInt32 (0xffffffff) : 0);
363  }
364 
365 
366  // logical operations
367  D_INLINE dVector operator& (const dVector& data) const
368  {
369  const dInt32* const a = (dInt32*)&m_x;
370  const dInt32* const b = (dInt32*)&data.m_x;
371  return dVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
372  }
373 
374  D_INLINE dVector operator| (const dVector& data) const
375  {
376  const dInt32* const a = (dInt32*)&m_x;
377  const dInt32* const b = (dInt32*)&data.m_x;
378  return dVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
379  }
380 
381  D_INLINE dVector operator^ (const dVector& data) const
382  {
383  const dInt32* const a = (dInt32*)&m_x;
384  const dInt32* const b = (dInt32*)&data.m_x;
385  return dVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
386  }
387 
388  D_INLINE dVector AndNot (const dVector& data) const
389  {
390  const dInt32* const a = (dInt32*)&m_x;
391  const dInt32* const b = (dInt32*)&data.m_x;
392  return dVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
393  }
394 
395  D_INLINE dVector Select (const dVector& data, const dVector& mask) const
396  {
397  // (((b ^ a) & mask)^a)
398  return (*this) ^ (mask & (data ^ (*this)));
399  }
400 
401  D_INLINE dInt32 GetSignMask() const
402  {
403  const dInt32* const a = (dInt32*)&m_x;
404  return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0));
405  }
406 
407  D_INLINE dVector ShiftRight() const
408  {
409  return dVector (m_w, m_x, m_y, m_z);
410  }
411 
412  D_INLINE dVector ShiftTripleRight () const
413  {
414  return dVector (m_z, m_x, m_y, m_w);
415  }
416 
417  D_INLINE dVector ShiftTripleLeft () const
418  {
419  return dVector (m_y, m_z, m_x, m_w);
420  }
421 
422  D_INLINE dVector ShiftRightLogical (dInt32 bits) const
423  {
424  return dVector (dInt32 (dUnsigned32 (m_ix) >> bits), dInt32 (dUnsigned32 (m_iy) >> bits), dInt32 (dUnsigned32 (m_iz) >> bits), dInt32 (dUnsigned32 (m_iw) >> bits));
425  }
426 
427  D_INLINE static void Transpose4x4 (dVector& dst0, dVector& dst1, dVector& dst2, dVector& dst3, const dVector& src0, const dVector& src1, const dVector& src2, const dVector& src3)
428  {
429  dVector tmp0 (src0);
430  dVector tmp1 (src1);
431  dVector tmp2 (src2);
432  dVector tmp3 (src3);
433 
434  dst0 = dVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
435  dst1 = dVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
436  dst2 = dVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
437  dst3 = dVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
438  }
439 
440  union
441  {
442  dInt32 m_i[4];
443  struct
444  {
445  dFloat32 m_x;
446  dFloat32 m_y;
447  dFloat32 m_z;
448  dFloat32 m_w;
449  };
450  struct
451  {
452  dInt32 m_ix;
453  dInt32 m_iy;
454  dInt32 m_iz;
455  dInt32 m_iw;
456  };
457  };
458 
459  static dVector m_zero;
460  static dVector m_one;
461  static dVector m_wOne;
462  static dVector m_half;
463  static dVector m_two;
464  static dVector m_three;
465  static dVector m_negOne;
466  static dVector m_xMask;
467  static dVector m_yMask;
468  static dVector m_zMask;
469  static dVector m_wMask;
470  static dVector m_epsilon;
471  static dVector m_signMask;
472  static dVector m_triplexMask;
473 } D_GCC_NEWTON_ALIGN_32 ;
474 
475 #endif
476 
477 class dBigVector
478 {
479  public:
480  D_INLINE dBigVector()
481  {
482  }
483 
484  D_INLINE dBigVector(dFloat64 val)
485  :m_x(val), m_y(val), m_z(val), m_w(val)
486  {
487  }
488 
489  D_INLINE dBigVector (const dBigVector& v)
490  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
491  {
492  }
493 
494 #ifndef D_NEWTON_USE_DOUBLE
495  D_INLINE dBigVector (const dVector& v)
496  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
497  {
498  }
499 
500  D_INLINE dBigVector (const dFloat32* const ptr)
501  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (dFloat32 (0.0f))
502  {
503  dAssert (dCheckVector ((*this)));
504  }
505 #endif
506 
507  D_INLINE dBigVector (const dFloat64* const ptr)
508  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3])
509  {
510  dAssert (dCheckVector ((*this)));
511  }
512 
513  D_INLINE dBigVector (dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w)
514  :m_x(x), m_y(y), m_z(z), m_w(w)
515  {
516  dAssert (dCheckVector ((*this)));
517  }
518 
519  D_INLINE dBigVector (dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
520  :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
521  {
522  }
523 
524  D_INLINE dBigVector (dInt64 ix, dInt64 iy, dInt64 iz, dInt64 iw)
525  :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
526  {
527  }
528 
529  D_INLINE dFloat64 GetScalar () const
530  {
531  return m_x;
532  }
533 
534  D_INLINE void Store (dFloat64* const dst) const
535  {
536  dst[0] = m_x;
537  dst[1] = m_y;
538  dst[2] = m_z;
539  dst[3] = m_w;
540  }
541 
542  D_INLINE dBigVector BroadcastX () const
543  {
544  return dBigVector (m_x);
545  }
546 
547  D_INLINE dBigVector BroadcastY () const
548  {
549  return dBigVector (m_y);
550  }
551 
552  D_INLINE dBigVector BroadcastZ () const
553  {
554  return dBigVector (m_z);
555  }
556 
557  D_INLINE dBigVector BroadcastW () const
558  {
559  return dBigVector (m_w);
560  }
561 
562 
563  D_INLINE dFloat64& operator[] (dInt32 i)
564  {
565  dAssert (i < 4);
566  dAssert (i >= 0);
567  return (&m_x)[i];
568  }
569 
570  D_INLINE const dFloat64& operator[] (dInt32 i) const
571  {
572  dAssert (i < 4);
573  dAssert (i >= 0);
574  return (&m_x)[i];
575  }
576 
577  D_INLINE dBigVector operator+ (const dBigVector& A) const
578  {
579  return dBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
580  }
581 
582  D_INLINE dBigVector operator- (const dBigVector& A) const
583  {
584  return dBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
585  }
586 
587  D_INLINE dBigVector operator* (const dBigVector& A) const
588  {
589  return dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
590  }
591 
592  D_INLINE dBigVector& operator+= (const dBigVector& A)
593  {
594  return (*this = dBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
595  }
596 
597  D_INLINE dBigVector& operator-= (const dBigVector& A)
598  {
599  return (*this = dBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
600  }
601 
602  D_INLINE dBigVector& operator*= (const dBigVector& A)
603  {
604  return (*this = dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
605  }
606 
607  D_INLINE dBigVector MulAdd(const dBigVector& A, const dBigVector& B) const
608  {
609  return *this + A * B;
610  }
611 
612  D_INLINE dBigVector MulSub(const dVector& A, const dBigVector& B) const
613  {
614  return *this - A * B;
615  }
616 
617 
618  D_INLINE dBigVector AddHorizontal () const
619  {
620  return dBigVector (m_x + m_y + m_z + m_w);
621  }
622 
623  D_INLINE dBigVector Scale (dFloat64 scale) const
624  {
625  return dBigVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
626  }
627 
628  // return cross product
629  D_INLINE dBigVector CrossProduct (const dBigVector& B) const
630  {
631  return dBigVector (m_y * B.m_z - m_z * B.m_y, m_z * B.m_x - m_x * B.m_z, m_x * B.m_y - m_y * B.m_x, m_w);
632  }
633 
634  D_INLINE dBigVector CrossProduct (const dBigVector& A, const dBigVector& B) const
635  {
636  dFloat64 cofactor[3][3];
637  dFloat64 array[4][4];
638 
639  const dBigVector& me = *this;
640  for (dInt32 i = 0; i < 4; i ++)
641  {
642  array[0][i] = me[i];
643  array[1][i] = A[i];
644  array[2][i] = B[i];
645  array[3][i] = dFloat32 (1.0f);
646  }
647 
648  dBigVector normal;
649  dFloat64 sign = dFloat64 (-1.0f);
650  for (dInt32 i = 0; i < 4; i ++)
651  {
652  for (dInt32 j = 0; j < 3; j ++)
653  {
654  dInt32 k0 = 0;
655  for (dInt32 k = 0; k < 4; k ++)
656  {
657  if (k != i)
658  {
659  cofactor[j][k0] = array[j][k];
660  k0 ++;
661  }
662  }
663  }
664  dFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
665  dFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
666  dFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
667  dFloat64 det = x + y + z;
668 
669  normal[i] = sign * det;
670  sign *= dFloat64 (-1.0f);
671  }
672 
673  return normal;
674  }
675 
676  D_INLINE dBigVector GetInt () const
677  {
678  return dBigVector (dInt64 (floor (m_x)), dInt64(floor (m_y)), dInt64(floor (m_z)), dInt64 (floor (m_w)));
679  }
680 
681  D_INLINE dBigVector TestZero() const
682  {
683  const dInt64* const a = (dInt64*)&m_x;
684  return dBigVector ((a[0] == 0) ? dFloat64 (-1.0f) : dFloat64 (1.0f),
685  (a[1] == 0) ? dFloat64 (-1.0f) : dFloat64 (1.0f),
686  (a[2] == 0) ? dFloat64 (-1.0f) : dFloat64 (1.0f),
687  (a[3] == 0) ? dFloat64 (-1.0f) : dFloat64 (1.0f));
688  }
689 
690 
691  D_INLINE dBigVector Floor () const
692  {
693  return dBigVector (floor (m_x), floor (m_y), floor (m_z), floor (m_w));
694  }
695 
696  D_INLINE dBigVector DotProduct (const dBigVector &A) const
697  {
698  return dBigVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
699  }
700 
701  D_INLINE dBigVector Reciproc () const
702  {
703  return dBigVector (dFloat64 (1.0f) / m_x, dFloat64 (1.0f) / m_y, dFloat64 (1.0f) / m_z, dFloat64 (1.0f) / m_w);
704  }
705 
706  D_INLINE dBigVector Sqrt () const
707  {
708  return dBigVector (sqrt (m_x), sqrt (m_y), sqrt (m_z), sqrt (m_w));
709  }
710 
711  D_INLINE dBigVector InvSqrt () const
712  {
713  return dBigVector (dFloat64 (1.0f) / sqrt (m_x), dFloat64 (1.0f) / sqrt (m_y), dFloat64 (1.0f) / sqrt (m_z), dFloat64 (1.0f) / sqrt (m_w));
714  }
715 
716  D_INLINE dBigVector InvMagSqrt () const
717  {
718  return dBigVector (dFloat64 (1.0f) / sqrt (DotProduct(*this).m_x));
719  }
720 
721  D_INLINE dBigVector Normalize() const
722  {
723  dAssert (m_w == dFloat64 (0.0f));
724  //const dBigVector& me = *this;
725  //return *this * dBigVector (dRsqrt(DotProduct(*this).m_x));
726  return *this * InvMagSqrt();
727  }
728 
729  dBigVector Abs () const
730  {
731  return dBigVector ((m_x > dFloat64 (0.0f)) ? m_x : -m_x,
732  (m_y > dFloat64 (0.0f)) ? m_y : -m_y,
733  (m_z > dFloat64 (0.0f)) ? m_z : -m_z,
734  (m_w > dFloat64 (0.0f)) ? m_w : -m_w);
735  }
736 
737  dFloat64 GetMax () const
738  {
739  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
740  }
741 
742  dBigVector GetMax (const dBigVector& data) const
743  {
744  return dBigVector ((m_x > data.m_x) ? m_x : data.m_x,
745  (m_y > data.m_y) ? m_y : data.m_y,
746  (m_z > data.m_z) ? m_z : data.m_z,
747  (m_w > data.m_w) ? m_w : data.m_w);
748  }
749 
750  dBigVector GetMin (const dBigVector& data) const
751  {
752  return dBigVector ((m_x < data.m_x) ? m_x : data.m_x,
753  (m_y < data.m_y) ? m_y : data.m_y,
754  (m_z < data.m_z) ? m_z : data.m_z,
755  (m_w < data.m_w) ? m_w : data.m_w);
756  }
757 
758  // relational operators
759  D_INLINE dBigVector operator== (const dBigVector& data) const
760  {
761  return dBigVector ((m_x == data.m_x) ? dInt64 (-1) : dInt64 (0),
762  (m_y == data.m_y) ? dInt64 (-1) : dInt64 (0),
763  (m_z == data.m_z) ? dInt64 (-1) : dInt64 (0),
764  (m_w == data.m_w) ? dInt64 (-1) : dInt64 (0));
765  }
766 
767  D_INLINE dBigVector operator> (const dBigVector& data) const
768  {
769  return dBigVector ((m_x > data.m_x) ? dInt64 (-1) : dInt64 (0),
770  (m_y > data.m_y) ? dInt64 (-1) : dInt64 (0),
771  (m_z > data.m_z) ? dInt64 (-1) : dInt64 (0),
772  (m_w > data.m_w) ? dInt64 (-1) : dInt64 (0));
773  }
774 
775  D_INLINE dBigVector operator< (const dBigVector& data) const
776  {
777  return dBigVector ((m_x < data.m_x) ? dInt64 (-1) : dInt64 (0),
778  (m_y < data.m_y) ? dInt64 (-1) : dInt64 (0),
779  (m_z < data.m_z) ? dInt64 (-1) : dInt64 (0),
780  (m_w < data.m_w) ? dInt64 (-1) : dInt64 (0));
781  }
782 
783  D_INLINE dBigVector operator>= (const dBigVector& data) const
784  {
785  return dBigVector ((m_x >= data.m_x) ? dInt64 (-1) : dInt64 (0),
786  (m_y >= data.m_y) ? dInt64 (-1) : dInt64 (0),
787  (m_z >= data.m_z) ? dInt64 (-1) : dInt64 (0),
788  (m_w >= data.m_w) ? dInt64 (-1) : dInt64 (0));
789  }
790 
791  D_INLINE dBigVector operator<= (const dBigVector& data) const
792  {
793  return dBigVector ((m_x <= data.m_x) ? dInt64 (-1) : dInt64 (0),
794  (m_y <= data.m_y) ? dInt64 (-1) : dInt64 (0),
795  (m_z <= data.m_z) ? dInt64 (-1) : dInt64 (0),
796  (m_w <= data.m_w) ? dInt64 (-1) : dInt64 (0));
797  }
798 
799  // logical operations
800  D_INLINE dBigVector operator& (const dBigVector& data) const
801  {
802  const dInt64* const a = (dInt64*)&m_x;
803  const dInt64* const b = (dInt64*)&data.m_x;
804  return dBigVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
805  }
806 
807  D_INLINE dBigVector operator| (const dBigVector& data) const
808  {
809  const dInt64* const a = (dInt64*)&m_x;
810  const dInt64* const b = (dInt64*)&data.m_x;
811  return dBigVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
812  }
813 
814  D_INLINE dBigVector operator^ (const dBigVector& data) const
815  {
816  const dInt64* const a = (dInt64*)&m_x;
817  const dInt64* const b = (dInt64*)&data.m_x;
818  return dBigVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
819  }
820 
821  D_INLINE dBigVector AndNot (const dBigVector& data) const
822  {
823  const dInt64* const a = (dInt64*)&m_x;
824  const dInt64* const b = (dInt64*)&data.m_x;
825  return dBigVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
826  }
827 
828  D_INLINE dBigVector Select(const dBigVector& data, const dBigVector& mask) const
829  {
830  // (((b ^ a) & mask)^a)
831  return (*this) ^ (mask & (data ^ (*this)));
832  }
833 
834  D_INLINE dInt32 GetSignMask() const
835  {
836  const dInt64* const a = (dInt64*)&m_x;
837  return (((a[0]>>63) ? 1 : 0) | ((a[1]>>63) ? 2 : 0) | ((a[2]>>63) ? 4 : 0) | ((a[3]>>63) ? 8 : 0));
838  }
839 
840  D_INLINE dVector ShiftRight() const
841  {
842  return dBigVector (m_w, m_x, m_y, m_z);
843  }
844 
845  D_INLINE dBigVector ShiftTripleRight () const
846  {
847  return dBigVector (m_z, m_x, m_y, m_w);
848  }
849 
850  D_INLINE dBigVector ShiftTripleLeft () const
851  {
852  return dBigVector (m_y, m_z, m_x, m_w);
853  }
854 
855  D_INLINE dBigVector ShiftRightLogical (dInt32 bits) const
856  {
857  return dBigVector (dInt64 (dUnsigned64 (m_ix) >> bits), dInt64 (dUnsigned64 (m_iy) >> bits), dInt64 (dUnsigned64 (m_iz) >> bits), dInt64 (dUnsigned64 (m_iw) >> bits));
858  }
859 
860  D_INLINE static void Transpose4x4 (dBigVector& dst0, dBigVector& dst1, dBigVector& dst2, dBigVector& dst3, const dBigVector& src0, const dBigVector& src1, const dBigVector& src2, const dBigVector& src3)
861  {
862  dBigVector tmp0 (src0);
863  dBigVector tmp1 (src1);
864  dBigVector tmp2 (src2);
865  dBigVector tmp3 (src3);
866 
867  dst0 = dBigVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
868  dst1 = dBigVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
869  dst2 = dBigVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
870  dst3 = dBigVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
871  }
872 
873  union
874  {
875  dInt64 m_i[4];
876  struct
877  {
878  dFloat64 m_x;
879  dFloat64 m_y;
880  dFloat64 m_z;
881  dFloat64 m_w;
882  };
883  struct
884  {
885  dInt64 m_ix;
886  dInt64 m_iy;
887  dInt64 m_iz;
888  dInt64 m_iw;
889  };
890  };
891 
892  static dBigVector m_zero;
893  static dBigVector m_one;
894  static dBigVector m_wOne;
895  static dBigVector m_half;
896  static dBigVector m_two;
897  static dBigVector m_three;
898  static dBigVector m_negOne;
899  static dBigVector m_xMask;
900  static dBigVector m_yMask;
901  static dBigVector m_zMask;
902  static dBigVector m_wMask;
903  static dBigVector m_epsilon;
904  static dBigVector m_signMask;
905  static dBigVector m_triplexMask;
906 } D_GCC_NEWTON_ALIGN_32;
907 
908 class dSpatialVector
909 {
910  public:
911  D_INLINE dSpatialVector()
912  {
913  }
914 
915  D_INLINE dSpatialVector(const dFloat32 a)
916  {
917  for (dInt32 i = 0; i < 6; i ++)
918  {
919  m_d[i] = a;
920  }
921  }
922 
923  D_INLINE dSpatialVector(const dVector& low, const dVector& high)
924  {
925  for (dInt32 i = 0; i < 3; i ++)
926  {
927  m_d[i] = low[i];
928  m_d[i + 3] = high[i];
929  }
930  }
931 
932  D_INLINE dSpatialVector(const dSpatialVector& src)
933  {
934  for (dInt32 i = 0; i < 6; i++)
935  {
936  m_d[i] = src[i];
937  }
938  }
939 
940  D_INLINE dFloat64& operator[] (dInt32 i)
941  {
942  dAssert(i < 6);
943  dAssert(i >= 0);
944  return m_d[i];
945  }
946 
947  D_INLINE const dFloat64& operator[] (dInt32 i) const
948  {
949  dAssert(i < 6);
950  dAssert(i >= 0);
951  return m_d[i];
952  }
953 
954  D_INLINE dSpatialVector operator+ (const dSpatialVector& A) const
955  {
956  dSpatialVector tmp;
957  for (dInt32 i = 0; i < 6; i++)
958  {
959  tmp[i] = m_d[i] + A.m_d[i];
960  }
961  return tmp;
962  }
963 
964  D_INLINE dSpatialVector operator* (const dSpatialVector& A) const
965  {
966  dSpatialVector tmp;
967  for (dInt32 i = 0; i < 6; i++)
968  {
969  tmp[i] = m_d[i] * A.m_d[i];
970  }
971  return tmp;
972  }
973 
974  D_INLINE dFloat64 DotProduct(const dSpatialVector& v) const
975  {
976  dFloat64 ret = dFloat64 (0.0f);
977  for (dInt32 i = 0; i < 6; i++)
978  {
979  ret += m_d[i] * v.m_d[i];
980  }
981  return ret;
982  }
983 
984  D_INLINE dSpatialVector Scale(dFloat64 s) const
985  {
986  dSpatialVector tmp;
987  for (dInt32 i = 0; i < 6; i++)
988  {
989  tmp[i] = m_d[i] * s;
990  }
991  return tmp;
992  }
993 
994  dFloat64 m_d[6];
995  static dSpatialVector m_zero;
996 } D_GCC_NEWTON_ALIGN_32 ;
997 
998 #endif
dBigVector
Definition: dVectorArmNeon.h:1521
dVector
Definition: dVectorArmNeon.h:1104
dSpatialVector
Definition: dVectorArmNeon.h:1954