Newton Dynamics  4.00
dVectorArmNeon.h
1 /* Copyright (c) <2003-2016> <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_ARM_NEON_H__
23 #define __D_VECTOR_ARM_NEON_H__
24 
25 #if 0
26 // *****************************************************************************************
27 //
28 // 4 x 1 single precision vector class declaration
29 //
30 // *****************************************************************************************
31 #ifdef D_NEWTON_USE_DOUBLE
32 #define dVector dBigVector
33 #else
34 
35 
36 #if DG_ARCH >= DG_ARCH_NEON_64
37 
38 A R M 6 4
39 
40 #define vec_minv vminvq_u32
41 #define vec_maxv maxvq_u32
42 #define vec_hadd4 vaddvq_f32
43 #define vec_floor vrndq_f32
44 
45 #else
46 
47 
48 #define D_INLINE_FORCEINLINE(type) static inline type
49 
50 D_INLINE_FORCEINLINE(dInt32) vec_minv(uint32x4_t v)
51 {
52  uint32x2_t tmp = vpmin_u32(vget_low_u32(v), vget_high_u32(v));
53  tmp = vpmin_u32(tmp, tmp);
54  return tmp[0] != 0;
55 }
56 
57 D_INLINE_FORCEINLINE(dInt32) vec_maxv(uint32x4_t v)
58 {
59  uint32x2_t tmp = vpmax_u32(vget_low_u32(v), vget_high_u32(v));
60  tmp = vpmax_u32(tmp, tmp);
61  return tmp[0] != 0;
62 }
63 
64 
65 D_INLINE_FORCEINLINE(float)vec_hadd4(float32x4_t v)
66 {
67  float32x4_t tmp = vaddq_f32(v, vrev64q_f32(v));
68  tmp = vaddq_f32(tmp, vcombine_f32(vget_high_f32(tmp), vget_low_f32(tmp)));
69  return tmp[0];
70 }
71 
72 
73 
74 
75 #endif
76 
77 D_INLINE_FORCEINLINE(float) vec_hadd3(float32x4_t v)
78 {
79  float32x2_t temp = vpadd_f32(vget_low_f32(v), vget_low_f32(v));
80  temp = vadd_f32(temp, vget_high_f32(v));
81  return vget_lane_f32(temp, 0);
82 }
83 
84 
85 #define vec_mul vmulq_f32
86 #define vec_add vaddq_f32
87 #define vec_sub vsubq_f32
88 #define vec_max vmaxq_f32
89 #define vec_min vminq_f32
90 #define vec_splat vdupq_n_f32
91 #define vec_div vdivq_f32
92 #define vec_rcp vrecpeq_f32
93 #define vec_store vst1q_f32
94 #define vec_load vld1q_f32
95 #define vec_abs vabsq_f32
96 #define vec_cvt vcvtq_s32_f32
97 #define vec_sqrt vrsqrtsq_f32
98 #define vec_recp vrecpsq_f32
99 #define vec_rsqrt rsqrteq_f32
100 #define vec_cmpne vceqq_f32
101 #define vec_cmpgt vcgtq_f32
102 #define vec_cmpge vcgeq_f32
103 #define vec_cmpeq vceqq_f32
104 #define vec_cmplt vcltq_f32
105 #define vec_cmple vcleq_f32
106 #define vec_xor veorq_u32
107 #define vec_or vorrq_u32
108 #define vec_and vandq_u32
109 #define vec_not vmvnq_u32
110 #define vec_andnot vbicq_u32
111 
112 #if defined __ARM_FEATURE_FMA
113 // a * b + c (no rounding, better results)
114 #define vec_madd vfmaq_f32
115 #define vec_msub vmlsq_f32
116 #else
117 #define vec_madd vmlaq_f32
118 #define vec_msub vmlsq_f32
119 #endif
120 
121 static inline float32x4_t vec_set(const float w, const float z, const float y, const float x)
122 {
123  float ptr[] = { x, y, z, w };
124  return vec_load(ptr);
125 }
126 
127 
128 class dBigVector;
129 DG_MSC_VECTOR_ALIGMENT
130 class dVector {
131 public:
132  D_INLINE dVector()
133  {
134  }
135 
136  D_INLINE dVector(const float32x4_t type)
137  : m_type(type) {
138  }
139 
140  D_INLINE dVector(const uint32x4_t type)
141  : m_typeInt(type) {
142  }
143 
144 
145  D_INLINE dVector(dFloat32 val)
146  : m_type(vmovq_n_f32(val))
147  {
148  }
149 
150  D_INLINE dVector (const dVector& v)
151  : m_type( v.m_type )
152  {
153  //dAssert (dgCheckVector ((*this)));
154  }
155 
156  D_INLINE dVector (const dFloat32* const ptr)
157  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (dFloat32 (0.0f))
158  {
159  dAssert (dgCheckVector ((*this)));
160  }
161 
162 #ifndef D_NEWTON_USE_DOUBLE
163  D_INLINE dVector(const dFloat64* const ptr)
164  :m_x(dFloat32(ptr[0]))
165  ,m_y(dFloat32(ptr[1]))
166  ,m_z(dFloat32(ptr[2]))
167  ,m_w(dFloat32(0.0f))
168  {
169  }
170 #endif
171 
172 
173  D_INLINE dVector (dFloat32 x, dFloat32 y, dFloat32 z, dFloat32 w)
174  :m_x(x), m_y(y), m_z(z), m_w(w)
175  {
176  dAssert (dgCheckVector ((*this)));
177  }
178 
179  D_INLINE dVector (dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
180  :m_x(*((dFloat32*)&ix)), m_y(*((dFloat32*)&iy)), m_z(*((dFloat32*)&iz)), m_w(*((dFloat32*)&iw))
181  {
182  }
183 
184 #ifndef D_NEWTON_USE_DOUBLE
185  D_INLINE dVector (const dBigVector& copy)
186  :m_x(dFloat32 (((dFloat64*)&copy)[0]))
187  ,m_y(dFloat32 (((dFloat64*)&copy)[1]))
188  ,m_z(dFloat32 (((dFloat64*)&copy)[2]))
189  ,m_w(dFloat32 (((dFloat64*)&copy)[3]))
190  {
191  dAssert (dgCheckVector ((*this)));
192  }
193 #endif
194 
195  D_INLINE dFloat32 GetScalar () const
196  {
197  return m_x;
198  }
199 
200  D_INLINE void Store (dFloat32* const dst) const
201  {
202  vec_store(dst, m_type);
203  }
204 
205  D_INLINE dVector BroadcastX () const
206  {
207  return dVector (m_x);
208  }
209 
210  D_INLINE dVector BroadcastY () const
211  {
212  return dVector (m_y);
213  }
214 
215  D_INLINE dVector BroadcastZ () const
216  {
217  return dVector (m_z);
218  }
219 
220  D_INLINE dVector BroadcastW () const
221  {
222  return dVector (m_w);
223  }
224 
225 
226  D_INLINE dFloat32& operator[] (dInt32 i)
227  {
228  dAssert (i < 4);
229  dAssert (i >= 0);
230  return (&m_x)[i];
231  }
232 
233  D_INLINE const dFloat32& operator[] (dInt32 i) const
234  {
235  dAssert (i < 4);
236  dAssert (i >= 0);
237  return (&m_x)[i];
238  }
239 
240  D_INLINE dVector operator+ (const dVector& A) const
241  {
242  return vec_add(m_type, A.m_type);
243  }
244 
245  D_INLINE dVector operator- (const dVector& A) const
246  {
247  return vec_sub(m_type, A.m_type);
248  }
249 
250  D_INLINE dVector operator* (const dVector& A) const
251  {
252  return vec_mul(m_type, A.m_type);
253  }
254 
255  D_INLINE dVector& operator+= (const dVector& A)
256  {
257  m_type = vec_add(m_type, A.m_type);
258  return *this;
259  }
260 
261  D_INLINE dVector& operator-= (const dVector& A)
262  {
263  m_type = vec_sub(m_type, A.m_type);
264  return *this;
265  }
266 
267  D_INLINE dVector& operator*= (const dVector& A)
268  {
269  m_type = vec_mul(m_type, A.m_type);
270  return *this;
271  }
272 
273 
274 
275  D_INLINE dVector AddHorizontal () const
276  {
277  return vec_hadd3(m_type); // dVector (m_x + m_y + m_z + m_w);
278  }
279 
280  D_INLINE dVector Scale3 (dFloat32 scale) const
281  {
282  return dVector (m_x * scale, m_y * scale, m_z * scale, m_w);
283  }
284 
285  D_INLINE dVector Scale (dFloat32 scale) const
286  {
287  return vec_mul(vmovq_n_f32(scale), m_type);
288  }
289 
290  // component wise multiplication
291  D_INLINE dVector CompProduct3 (const dVector& A) const
292  {
293  return dVector (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
294  }
295 
296  // return cross product
297  D_INLINE dVector CrossProduct (const dVector& B) const
298  {
299  /*
300  float32x4_t v1 = m_type;
301  float32x4_t v2 = B.m_type;
302  float32x4x2_t v_1203 = vzipq_f32(vcombine_f32(vrev64_f32(vget_low_f32(v1)), vrev64_f32(vget_low_f32(v2))), vcombine_f32(vget_high_f32(v1), vget_high_f32(v2)));
303  float32x4x2_t v_2013 = vzipq_f32(vcombine_f32(vrev64_f32(vget_low_f32(v_1203.val[0])), vrev64_f32(vget_low_f32(v_1203.val[1]))), vcombine_f32(vget_high_f32(v_1203.val[0]), vget_high_f32(v_1203.val[1])));
304  inVec->vec = vmlsq_f32(vmulq_f32(v_1203.val[0], v_2013.val[1]), v_1203.val[1], v_2013.val[0]);
305  */
306  return dVector (m_y * B.m_z - m_z * B.m_y,
307  m_z * B.m_x - m_x * B.m_z,
308  m_x * B.m_y - m_y * B.m_x, m_w);
309  }
310 
311  D_INLINE dVector CrossProduct (const dVector& A, const dVector& B) const
312  {
313  dFloat32 cofactor[3][3];
314  dFloat32 array[4][4];
315 
316  const dVector& me = *this;
317  for (dInt32 i = 0; i < 4; i ++) {
318  array[0][i] = me[i];
319  array[1][i] = A[i];
320  array[2][i] = B[i];
321  array[3][i] = dFloat32 (1.0f);
322  }
323 
324  dVector normal;
325  dFloat32 sign = dFloat32 (-1.0f);
326  for (dInt32 i = 0; i < 4; i ++)
327  {
328  for (dInt32 j = 0; j < 3; j ++)
329  {
330  dInt32 k0 = 0;
331  for (dInt32 k = 0; k < 4; k ++)
332  {
333  if (k != i)
334  {
335  cofactor[j][k0] = array[j][k];
336  k0 ++;
337  }
338  }
339  }
340  dFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
341  dFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
342  dFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
343  dFloat32 det = x + y + z;
344 
345  normal[i] = sign * det;
346  sign *= dFloat32 (-1.0f);
347  }
348 
349  return normal;
350  }
351 
352  D_INLINE dVector GetInt () const
353  {
354  return vcvtq_u32_f32(m_type);
355  }
356 
357  D_INLINE dVector GetFloat () const
358  {
359  return vcvtq_f32_u32(m_type);
360  }
361 
362  D_INLINE dVector TestZero() const
363  {
364  return m_negOne & (*this == m_zero);
365  }
366 
367  D_INLINE dVector Floor () const
368  {
369 #if DG_ARCH >= DG_ARCH_NEON_64
370  // #if __ARM_ARCH >= 8 && defined(__ARM_FEATURE_DIRECTED_ROUNDING)
371  return vec_floor(m_type);
372 #else
373  return dVector (dgFloor (m_x), dgFloor (m_y), dgFloor (m_z), dgFloor (m_w));
374 
375 #endif
376  }
377 
378  D_INLINE dVector DotProduct (const dVector &A) const
379  {
380  auto tmp = vec_mul(m_type, A.m_type);
381  return vec_hadd4(tmp);
382  }
383 
384 
385 
386  D_INLINE dVector InvMagSqrt() const
387  {
388  return dVector(dgRsqrt(DotProduct(*this).m_x));
389  }
390 
391  D_INLINE dVector Reciproc() const
392  {
393  float32x4_t reciprocal = vrecpeq_f32(m_type);
394  reciprocal = vrecpsq_f32(m_type, reciprocal) * reciprocal;
395  return reciprocal;
396 
397  }
398 
399 
400  D_INLINE dVector MulAdd(const dVector& A, const dVector& B) const
401  {
402  // a * b + this
403  return vec_madd(A.m_type, B.m_type, m_type);
404  }
405 
406  D_INLINE dVector MulSub(const dVector& A, const dVector& B) const
407  {
408  // a * b - this
409  return vec_msub(A.m_type, B.m_type, m_type);
410  }
411 
412  D_INLINE dVector InvSqrt() const
413  {
414  float32x4_t sqrt_reciprocal = vrsqrteq_f32(m_type);
415  return vrsqrtsq_f32(m_type * sqrt_reciprocal, sqrt_reciprocal) * sqrt_reciprocal;
416  }
417 
418  D_INLINE dVector Sqrt() const
419  {
420  float32x4_t sqrt_reciprocal = vrsqrteq_f32(m_type);
421  float32x4_t tmp = vrsqrtsq_f32(m_type * sqrt_reciprocal, sqrt_reciprocal) * sqrt_reciprocal;
422  return vec_mul(m_type, tmp);
423  }
424 
425  D_INLINE dVector Normalize () const
426  {
427  dAssert (m_w == dFloat32 (0.0f));
428  const dVector& me = *this;
429  return me * InvMagSqrt();
430  }
431 
432  dVector Abs () const
433  {
434  return vec_abs(m_type);
435  }
436 
437  dFloat32 GetMax () const
438  {
439  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
440  }
441 
442  dVector GetMax (const dVector& data) const
443  {
444  return vec_max(m_type, data.m_type);
445  }
446 
447  dVector GetMin (const dVector& data) const
448  {
449  return vec_min(m_type, data.m_type);
450  }
451 
452  // relational operators
453  D_INLINE dVector operator== (const dVector& data) const
454  {
455  return vec_cmpeq(m_typeInt, data.m_typeInt);
456  }
457 
458  D_INLINE dVector operator!= (const dVector& data) const
459  {
460  return vec_cmpne(m_typeInt, data.m_typeInt);
461  }
462 
463  D_INLINE dVector operator> (const dVector& data) const
464  {
465  return vec_cmpgt(m_typeInt, data.m_typeInt);
466  }
467 
468  D_INLINE dVector operator< (const dVector& data) const
469  {
470  return vec_cmplt(m_typeInt, data.m_typeInt);
471  }
472 
473  D_INLINE dVector operator>= (const dVector& data) const
474  {
475  return vec_cmpge(m_typeInt, data.m_typeInt);
476  }
477 
478  D_INLINE dVector operator<= (const dVector& data) const
479  {
480  return vec_cmple(m_typeInt, data.m_typeInt);
481  }
482 
483  // logical operations
484  D_INLINE dVector operator& (const dVector& data) const
485  {
486  return vec_and(m_typeInt, data.m_typeInt);
487  }
488 
489  D_INLINE dVector operator| (const dVector& data) const
490  {
491  return vec_or(m_typeInt, data.m_typeInt);
492  }
493 
494  D_INLINE dVector operator^ (const dVector& data) const
495  {
496  return vec_xor(m_typeInt, data.m_typeInt);
497  }
498 
499  D_INLINE dVector AndNot (const dVector& data) const
500  {
501  return vec_andnot(m_typeInt, data.m_typeInt);
502  }
503 
504  D_INLINE dInt32 GetSignMask() const
505  {
506  const dInt32* const a = (dInt32*)&m_x;
507  return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0));
508  }
509 
510  D_INLINE dVector ShiftTripleRight () const
511  {
512  return dVector (m_z, m_x, m_y, m_w);
513  }
514 
515  D_INLINE dVector ShiftTripleLeft () const
516  {
517  return dVector (m_y, m_z, m_x, m_w);
518  }
519 
520  D_INLINE dVector ShiftRightLogical (dInt32 bits) const
521  {
522  return dVector (dInt32 (dUnsigned32 (m_ix) >> bits), dInt32 (dUnsigned32 (m_iy) >> bits), dInt32 (dUnsigned32 (m_iz) >> bits), dInt32 (dUnsigned32 (m_iw) >> bits));
523  }
524 
525  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)
526  {
527  float32x4x2_t vtrn1 = vzipq_f32(src0.m_type, src2.m_type);
528  float32x4x2_t vtrn2 = vzipq_f32(src1.m_type, src3.m_type);
529  float32x4x2_t res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]);
530  float32x4x2_t res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]);
531  dst0.m_type = res1.val[0];
532  dst1.m_type = res1.val[1];
533  dst2.m_type = res2.val[0];
534  dst3.m_type = res2.val[1];
535 
536  }
537 
538  DG_CLASS_ALLOCATOR(allocator)
539 
540  union {
541  float32x4_t m_type;
542  uint32x4_t m_typeInt;
543  dInt32 m_i[4];
544  struct {
545  dFloat32 m_x;
546  dFloat32 m_y;
547  dFloat32 m_z;
548  dFloat32 m_w;
549  };
550  struct {
551  dInt32 m_ix;
552  dInt32 m_iy;
553  dInt32 m_iz;
554  dInt32 m_iw;
555  };
556  };
557 
558  static dVector m_zero;
559  static dVector m_one;
560  static dVector m_wOne;
561  static dVector m_half;
562  static dVector m_two;
563  static dVector m_three;
564  static dVector m_negOne;
565  static dVector m_xMask;
566  static dVector m_yMask;
567  static dVector m_zMask;
568  static dVector m_wMask;
569  static dVector m_signMask;
570  static dVector m_triplexMask;
571 } DG_GCC_VECTOR_ALIGMENT;
572 
573 #endif
574 
575 DG_MSC_VECTOR_ALIGMENT
576 class dBigVector
577 {
578 public:
579  D_INLINE dBigVector()
580  {
581  }
582 
583  D_INLINE dBigVector(dFloat64 val)
584  :m_x(val), m_y(val), m_z(val), m_w(val)
585  {
586  }
587 
588  D_INLINE dBigVector(const dBigVector& v)
589  : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
590  {
591  }
592 
593 #ifndef D_NEWTON_USE_DOUBLE
594  D_INLINE dBigVector(const dVector& v)
595  : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
596  {
597  }
598 
599  D_INLINE dBigVector(const dFloat32* const ptr)
600  : m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dFloat32(0.0f))
601  {
602  dAssert(dgCheckVector((*this)));
603  }
604 #endif
605 
606  D_INLINE dBigVector(const dFloat64* const ptr)
607  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dFloat32(0.0f))
608  {
609  dAssert(dgCheckVector((*this)));
610  }
611 
612  D_INLINE dBigVector(dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w)
613  : m_x(x), m_y(y), m_z(z), m_w(w)
614  {
615  dAssert(dgCheckVector((*this)));
616  }
617 
618  D_INLINE dBigVector(dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
619  : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
620  {
621  }
622 
623  D_INLINE dBigVector(dInt64 ix, dInt64 iy, dInt64 iz, dInt64 iw)
624  : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
625  {
626  }
627 
628  D_INLINE dFloat64 GetScalar() const
629  {
630  return m_x;
631  }
632 
633  D_INLINE void Store(dFloat64* const dst) const
634  {
635  dst[0] = m_x;
636  dst[1] = m_y;
637  dst[2] = m_z;
638  dst[3] = m_w;
639  }
640 
641  D_INLINE dBigVector BroadcastX() const
642  {
643  return dBigVector(m_x);
644  }
645 
646  D_INLINE dBigVector BroadcastY() const
647  {
648  return dBigVector(m_y);
649  }
650 
651  D_INLINE dBigVector BroadcastZ() const
652  {
653  return dBigVector(m_z);
654  }
655 
656  D_INLINE dBigVector BroadcastW() const
657  {
658  return dBigVector(m_w);
659  }
660 
661 
662  D_INLINE dFloat64& operator[] (dInt32 i)
663  {
664  dAssert(i < 4);
665  dAssert(i >= 0);
666  return (&m_x)[i];
667  }
668 
669  D_INLINE const dFloat64& operator[] (dInt32 i) const
670  {
671  dAssert(i < 4);
672  dAssert(i >= 0);
673  return (&m_x)[i];
674  }
675 
676  D_INLINE dBigVector operator+ (const dBigVector& A) const
677  {
678  return dBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
679  }
680 
681  D_INLINE dBigVector operator- (const dBigVector& A) const
682  {
683  return dBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
684  }
685 
686  D_INLINE dBigVector operator* (const dBigVector& A) const
687  {
688  return dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
689  }
690 
691  D_INLINE dBigVector& operator+= (const dBigVector& A)
692  {
693  return (*this = dBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
694  }
695 
696  D_INLINE dBigVector& operator-= (const dBigVector& A)
697  {
698  return (*this = 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& operator*= (const dBigVector& A)
702  {
703  return (*this = dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
704  }
705 
706  D_INLINE dBigVector AddHorizontal() const
707  {
708  return dBigVector(m_x + m_y + m_z + m_w);
709  }
710 
711  D_INLINE dBigVector Scale3(dFloat64 scale) const
712  {
713  return dBigVector(m_x * scale, m_y * scale, m_z * scale, m_w);
714  }
715 
716  D_INLINE dBigVector Scale(dFloat64 scale) const
717  {
718  return dBigVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale);
719  }
720 
721  // component wise multiplication
722  D_INLINE dBigVector CompProduct3(const dBigVector& A) const
723  {
724  return dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
725  }
726 
727  // return cross product
728  D_INLINE dBigVector CrossProduct(const dBigVector& B) const
729  {
730  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);
731  }
732 
733  D_INLINE dBigVector CrossProduct(const dBigVector& A, const dBigVector& B) const
734  {
735  dFloat64 cofactor[3][3];
736  dFloat64 array[4][4];
737 
738  const dBigVector& me = *this;
739  for (dInt32 i = 0; i < 4; i++) {
740  array[0][i] = me[i];
741  array[1][i] = A[i];
742  array[2][i] = B[i];
743  array[3][i] = dFloat32(1.0f);
744  }
745 
746  dBigVector normal;
747  dFloat64 sign = dFloat64(-1.0f);
748  for (dInt32 i = 0; i < 4; i++) {
749 
750  for (dInt32 j = 0; j < 3; j++) {
751  dInt32 k0 = 0;
752  for (dInt32 k = 0; k < 4; k++) {
753  if (k != i) {
754  cofactor[j][k0] = array[j][k];
755  k0++;
756  }
757  }
758  }
759  dFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
760  dFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
761  dFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
762  dFloat64 det = x + y + z;
763 
764  normal[i] = sign * det;
765  sign *= dFloat64(-1.0f);
766  }
767 
768  return normal;
769  }
770 
771  D_INLINE dBigVector GetInt() const
772  {
773  return dBigVector(dInt64(floor(m_x)), dInt64(floor(m_y)), dInt64(floor(m_z)), dInt64(floor(m_w)));
774  }
775 
776  D_INLINE dBigVector TestZero() const
777  {
778  const dInt64* const a = (dInt64*)&m_x;
779  return dBigVector((a[0] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
780  (a[1] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
781  (a[2] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
782  (a[3] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f));
783  }
784 
785 
786  D_INLINE dBigVector Floor() const
787  {
788  return dBigVector(floor(m_x), floor(m_y), floor(m_z), floor(m_w));
789  }
790 
791  D_INLINE dBigVector DotProduct(const dBigVector &A) const
792  {
793  return dBigVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
794  }
795 
796  D_INLINE dBigVector Reciproc() const
797  {
798  return dBigVector(dFloat64(1.0f) / m_x, dFloat64(1.0f) / m_y, dFloat64(1.0f) / m_z, dFloat64(1.0f) / m_w);
799  }
800 
801  D_INLINE dBigVector Sqrt() const
802  {
803  return dBigVector(sqrt(m_x), sqrt(m_y), sqrt(m_z), sqrt(m_w));
804  }
805 
806  D_INLINE dBigVector InvSqrt() const
807  {
808  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));
809  }
810 
811  D_INLINE dBigVector InvMagSqrt() const
812  {
813  return dBigVector(dFloat64(1.0f) / sqrt(DotProduct(*this).m_x));
814  }
815 
816  D_INLINE dBigVector Normalize() const
817  {
818  dAssert(m_w == dFloat64(0.0f));
819  //const dBigVector& me = *this;
820  //return *this * dBigVector (dgRsqrt(DotProduct(*this).m_x));
821  return *this * InvMagSqrt();
822  }
823 
824  dBigVector Abs() const
825  {
826  return dBigVector((m_x > dFloat64(0.0f)) ? m_x : -m_x,
827  (m_y > dFloat64(0.0f)) ? m_y : -m_y,
828  (m_z > dFloat64(0.0f)) ? m_z : -m_z,
829  (m_w > dFloat64(0.0f)) ? m_w : -m_w);
830  }
831 
832  dFloat64 GetMax() const
833  {
834  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
835  }
836 
837  dBigVector GetMax(const dBigVector& data) const
838  {
839  return dBigVector((m_x > data.m_x) ? m_x : data.m_x,
840  (m_y > data.m_y) ? m_y : data.m_y,
841  (m_z > data.m_z) ? m_z : data.m_z,
842  (m_w > data.m_w) ? m_w : data.m_w);
843  }
844 
845  dBigVector GetMin(const dBigVector& data) const
846  {
847  return dBigVector((m_x < data.m_x) ? m_x : data.m_x,
848  (m_y < data.m_y) ? m_y : data.m_y,
849  (m_z < data.m_z) ? m_z : data.m_z,
850  (m_w < data.m_w) ? m_w : data.m_w);
851  }
852 
853  // relational operators
854  D_INLINE dBigVector operator== (const dBigVector& data) const
855  {
856  return dBigVector((m_x == data.m_x) ? dInt64(-1) : dInt64(0),
857  (m_y == data.m_y) ? dInt64(-1) : dInt64(0),
858  (m_z == data.m_z) ? dInt64(-1) : dInt64(0),
859  (m_w == data.m_w) ? dInt64(-1) : dInt64(0));
860  }
861 
862  D_INLINE dBigVector operator> (const dBigVector& data) const
863  {
864  return dBigVector((m_x > data.m_x) ? dInt64(-1) : dInt64(0),
865  (m_y > data.m_y) ? dInt64(-1) : dInt64(0),
866  (m_z > data.m_z) ? dInt64(-1) : dInt64(0),
867  (m_w > data.m_w) ? dInt64(-1) : dInt64(0));
868  }
869 
870  D_INLINE dBigVector operator< (const dBigVector& data) const
871  {
872  return dBigVector((m_x < data.m_x) ? dInt64(-1) : dInt64(0),
873  (m_y < data.m_y) ? dInt64(-1) : dInt64(0),
874  (m_z < data.m_z) ? dInt64(-1) : dInt64(0),
875  (m_w < data.m_w) ? dInt64(-1) : dInt64(0));
876  }
877 
878  D_INLINE dBigVector operator>= (const dBigVector& data) const
879  {
880  return dBigVector((m_x >= data.m_x) ? dInt64(-1) : dInt64(0),
881  (m_y >= data.m_y) ? dInt64(-1) : dInt64(0),
882  (m_z >= data.m_z) ? dInt64(-1) : dInt64(0),
883  (m_w >= data.m_w) ? dInt64(-1) : dInt64(0));
884  }
885 
886  D_INLINE dBigVector operator<= (const dBigVector& data) const
887  {
888  return dBigVector((m_x <= data.m_x) ? dInt64(-1) : dInt64(0),
889  (m_y <= data.m_y) ? dInt64(-1) : dInt64(0),
890  (m_z <= data.m_z) ? dInt64(-1) : dInt64(0),
891  (m_w <= data.m_w) ? dInt64(-1) : dInt64(0));
892  }
893 
894 
895  // logical operations
896  D_INLINE dBigVector operator& (const dBigVector& data) const
897  {
898  const dInt64* const a = (dInt64*)&m_x;
899  const dInt64* const b = (dInt64*)&data.m_x;
900  return dBigVector(a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
901  }
902 
903  D_INLINE dBigVector operator| (const dBigVector& data) const
904  {
905  const dInt64* const a = (dInt64*)&m_x;
906  const dInt64* const b = (dInt64*)&data.m_x;
907  return dBigVector(a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
908  }
909 
910  D_INLINE dBigVector operator^ (const dBigVector& data) const
911  {
912  const dInt64* const a = (dInt64*)&m_x;
913  const dInt64* const b = (dInt64*)&data.m_x;
914  return dBigVector(a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
915  }
916 
917  D_INLINE dBigVector AndNot(const dBigVector& data) const
918  {
919  const dInt64* const a = (dInt64*)&m_x;
920  const dInt64* const b = (dInt64*)&data.m_x;
921  return dBigVector(a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
922  }
923 
924  D_INLINE dInt32 GetSignMask() const
925  {
926  const dInt64* const a = (dInt64*)&m_x;
927  return (((a[0] >> 63) ? 1 : 0) | ((a[1] >> 63) ? 2 : 0) | ((a[2] >> 63) ? 4 : 0) | ((a[3] >> 63) ? 8 : 0));
928  }
929 
930  D_INLINE dBigVector ShiftTripleRight() const
931  {
932  return dBigVector(m_z, m_x, m_y, m_w);
933  }
934 
935  D_INLINE dBigVector ShiftTripleLeft() const
936  {
937  return dBigVector(m_y, m_z, m_x, m_w);
938  }
939 
940  D_INLINE dBigVector ShiftRightLogical(dInt32 bits) const
941  {
942  return dBigVector(dInt64(dUnsigned64(m_ix) >> bits), dInt64(dUnsigned64(m_iy) >> bits), dInt64(dUnsigned64(m_iz) >> bits), dInt64(dUnsigned64(m_iw) >> bits));
943  }
944 
945  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)
946  {
947  dBigVector tmp0(src0);
948  dBigVector tmp1(src1);
949  dBigVector tmp2(src2);
950  dBigVector tmp3(src3);
951 
952  dst0 = dBigVector(tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
953  dst1 = dBigVector(tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
954  dst2 = dBigVector(tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
955  dst3 = dBigVector(tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
956  }
957 
958  DG_CLASS_ALLOCATOR(allocator)
959 
960  union
961  {
962 #if DG_ARCH >= DG_ARCH_NEON_64
963  struct {
964  float64x2_t m_xy;
965  float64x2_t m_zw;
966  };
967  struct {
968  int64x2_t m_ixy;
969  int64x2_t m_izw;
970  };
971 #endif
972  dInt64 m_i[4];
973  struct
974  {
975  dFloat64 m_x;
976  dFloat64 m_y;
977  dFloat64 m_z;
978  dFloat64 m_w;
979  };
980  struct
981  {
982  dInt64 m_ix;
983  dInt64 m_iy;
984  dInt64 m_iz;
985  dInt64 m_iw;
986  };
987  };
988 
989  static dBigVector m_zero;
990  static dBigVector m_one;
991  static dBigVector m_wOne;
992  static dBigVector m_half;
993  static dBigVector m_two;
994  static dBigVector m_three;
995  static dBigVector m_negOne;
996  static dBigVector m_xMask;
997  static dBigVector m_yMask;
998  static dBigVector m_zMask;
999  static dBigVector m_wMask;
1000  static dBigVector m_signMask;
1001  static dBigVector m_triplexMask;
1002 } DG_GCC_VECTOR_ALIGMENT;
1003 
1004 
1005 DG_MSC_VECTOR_ALIGMENT
1006 class dSpatialVector
1007 {
1008 public:
1009  D_INLINE dSpatialVector()
1010  {
1011  }
1012 
1013  D_INLINE dSpatialVector(const dFloat32 a)
1014  {
1015  for (dInt32 i = 0; i < 6; i++) {
1016  m_d[i] = a;
1017  }
1018  }
1019 
1020  D_INLINE dSpatialVector(const dVector& low, const dVector& high)
1021  {
1022  for (dInt32 i = 0; i < 3; i++) {
1023  m_d[i] = low[i];
1024  m_d[i + 3] = high[i];
1025  }
1026  }
1027 
1028  D_INLINE dSpatialVector(const dSpatialVector& src)
1029  {
1030  for (dInt32 i = 0; i < 6; i++) {
1031  m_d[i] = src[i];
1032  }
1033  }
1034 
1035  D_INLINE dFloat64& operator[] (dInt32 i)
1036  {
1037  dAssert(i < 6);
1038  dAssert(i >= 0);
1039  return m_d[i];
1040  }
1041 
1042  D_INLINE const dFloat64& operator[] (dInt32 i) const
1043  {
1044  dAssert(i < 6);
1045  dAssert(i >= 0);
1046  return m_d[i];
1047  }
1048 
1049  D_INLINE dSpatialVector operator+ (const dSpatialVector& A) const
1050  {
1051  dSpatialVector tmp;
1052  for (dInt32 i = 0; i < 6; i++) {
1053  tmp[i] = m_d[i] + A.m_d[i];
1054  }
1055  return tmp;
1056  }
1057 
1058  D_INLINE dSpatialVector operator* (const dSpatialVector& A) const
1059  {
1060  dSpatialVector tmp;
1061  for (dInt32 i = 0; i < 6; i++) {
1062  tmp[i] = m_d[i] * A.m_d[i];
1063  }
1064  return tmp;
1065  }
1066 
1067  D_INLINE dFloat64 DotProduct(const dSpatialVector& v) const
1068  {
1069  dFloat64 ret = dFloat64(0.0f);
1070  for (dInt32 i = 0; i < 6; i++) {
1071  ret += m_d[i] * v.m_d[i];
1072  }
1073  return ret;
1074  }
1075 
1076  D_INLINE dSpatialVector Scale(dFloat64 s) const
1077  {
1078  dSpatialVector tmp;
1079  for (dInt32 i = 0; i < 6; i++) {
1080  tmp[i] = m_d[i] * s;
1081  }
1082  return tmp;
1083  }
1084 
1085  dFloat64 m_d[6];
1086  static dSpatialVector m_zero;
1087 } DG_GCC_VECTOR_ALIGMENT;
1088 
1089 #endif
1090 
1091 
1092 // *****************************************************************************************
1093 //
1094 // 4 x 1 single precision vector class declaration
1095 //
1096 // *****************************************************************************************
1097 #ifdef D_NEWTON_USE_DOUBLE
1098 #define dVector dBigVector
1099 #else
1100 
1101 class dBigVector;
1102 D_MSV_NEWTON_ALIGN_16
1103 class dVector
1104 {
1105  public:
1106  D_INLINE dVector()
1107  {
1108  }
1109 
1110  D_INLINE dVector(dFloat32 val)
1111  :m_type(vmovq_n_f32(val))
1112  {
1113  }
1114 
1115  D_INLINE dVector(const dVector& v)
1116  :m_type(v.m_type)
1117  {
1118  }
1119 
1120  D_INLINE dVector(const float32x4_t type)
1121  :m_type(type)
1122  {
1123  }
1124 
1125  D_INLINE dVector(const dFloat32* const ptr)
1126  //: m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(ptr[3])
1127  :m_type(vld1q_f32 (ptr))
1128  {
1129  dAssert(dgCheckVector((*this)));
1130  }
1131 
1132 #ifndef D_NEWTON_USE_DOUBLE
1133  D_INLINE dVector(const dFloat64* const ptr)
1134  :m_x(dFloat32(ptr[0]))
1135  ,m_y(dFloat32(ptr[1]))
1136  ,m_z(dFloat32(ptr[2]))
1137  ,m_w(dFloat32(ptr[3]))
1138  {
1139  }
1140 #endif
1141 
1142  D_INLINE dVector(dFloat32 x, dFloat32 y, dFloat32 z, dFloat32 w)
1143  :m_x(x), m_y(y), m_z(z), m_w(w)
1144  {
1145  dAssert(dgCheckVector((*this)));
1146  }
1147 
1148  D_INLINE dVector(dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
1149  : m_x(*((dFloat32*)&ix)), m_y(*((dFloat32*)&iy)), m_z(*((dFloat32*)&iz)), m_w(*((dFloat32*)&iw))
1150  {
1151  }
1152 
1153 #ifndef D_NEWTON_USE_DOUBLE
1154  D_INLINE dVector(const dBigVector& copy)
1155  :m_x(dFloat32(((dFloat64*)&copy)[0]))
1156  ,m_y(dFloat32(((dFloat64*)&copy)[1]))
1157  ,m_z(dFloat32(((dFloat64*)&copy)[2]))
1158  ,m_w(dFloat32(((dFloat64*)&copy)[3]))
1159  {
1160  dAssert(dgCheckVector((*this)));
1161  }
1162 #endif
1163 
1164  D_INLINE dFloat32 GetScalar() const
1165  {
1166  return m_x;
1167  }
1168 
1169  D_INLINE void Store(dFloat32* const dst) const
1170  {
1171  vst1q_f32(dst, m_type);
1172  }
1173 
1174  D_INLINE dVector BroadcastX() const
1175  {
1176  return dVector(m_x);
1177  }
1178 
1179  D_INLINE dVector BroadcastY() const
1180  {
1181  return dVector(m_y);
1182  }
1183 
1184  D_INLINE dVector BroadcastZ() const
1185  {
1186  return dVector(m_z);
1187  }
1188 
1189  D_INLINE dVector BroadcastW() const
1190  {
1191  return dVector(m_w);
1192  }
1193 
1194 
1195  D_INLINE dFloat32& operator[] (dInt32 i)
1196  {
1197  dAssert(i < 4);
1198  dAssert(i >= 0);
1199  return (&m_x)[i];
1200  }
1201 
1202  D_INLINE const dFloat32& operator[] (dInt32 i) const
1203  {
1204  dAssert(i < 4);
1205  dAssert(i >= 0);
1206  return (&m_x)[i];
1207  }
1208 
1209  D_INLINE dVector operator+ (const dVector& A) const
1210  {
1211  return vaddq_f32(m_type, A.m_type);
1212  }
1213 
1214  D_INLINE dVector operator- (const dVector& A) const
1215  {
1216  return vsubq_f32(m_type, A.m_type);
1217  }
1218 
1219  D_INLINE dVector operator* (const dVector& A) const
1220  {
1221  return vmulq_f32(m_type, A.m_type);
1222  }
1223 
1224  D_INLINE dVector& operator+= (const dVector& A)
1225  {
1226  return (*this = vsubq_f32(m_type, A.m_type));
1227  }
1228 
1229  D_INLINE dVector& operator-= (const dVector& A)
1230  {
1231  return (*this = vsubq_f32(m_type, A.m_type));
1232  }
1233 
1234  D_INLINE dVector& operator*= (const dVector& A)
1235  {
1236  return (*this = vmulq_f32(m_type, A.m_type));
1237  }
1238 
1239  D_INLINE dVector MulAdd(const dVector& A, const dVector& B) const
1240  {
1241  //return *this + A * B;
1242  //return vfmaq_f32(A.m_type, B.m_type, m_type);
1243  return vmlaq_f32(m_type, A.m_type, B.m_type);
1244  }
1245 
1246  D_INLINE dVector MulSub(const dVector& A, const dVector& B) const
1247  {
1248  //return *this - A * B;
1249  return vmlsq_f32(m_type, A.m_type, B.m_type);
1250  }
1251 
1252  D_INLINE dVector AddHorizontal() const
1253  {
1254  return dVector(m_x + m_y + m_z + m_w);
1255  //float32x2_t temp = vpadd_f32(vget_low_f32(m_type), vget_low_f32(m_type));
1256  //temp = vadd_f32(temp, vget_high_f32(m_type));
1257  //return vget_lane_f32(temp, 0);
1258  }
1259 
1260  D_INLINE dVector Scale(dFloat32 scale) const
1261  {
1262  return dVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale);
1263  }
1264 
1265  // return cross product
1266  D_INLINE dVector CrossProduct(const dVector& B) const
1267  {
1268  return dVector(m_y * B.m_z - m_z * B.m_y,
1269  m_z * B.m_x - m_x * B.m_z,
1270  m_x * B.m_y - m_y * B.m_x, m_w);
1271  }
1272 
1273  D_INLINE dVector CrossProduct(const dVector& A, const dVector& B) const
1274  {
1275  dFloat32 cofactor[3][3];
1276  dFloat32 array[4][4];
1277 
1278  const dVector& me = *this;
1279  for (dInt32 i = 0; i < 4; i++)
1280  {
1281  array[0][i] = me[i];
1282  array[1][i] = A[i];
1283  array[2][i] = B[i];
1284  array[3][i] = dFloat32(1.0f);
1285  }
1286 
1287  dVector normal;
1288  dFloat32 sign = dFloat32(-1.0f);
1289  for (dInt32 i = 0; i < 4; i++)
1290  {
1291  for (dInt32 j = 0; j < 3; j++)
1292  {
1293  dInt32 k0 = 0;
1294  for (dInt32 k = 0; k < 4; k++)
1295  {
1296  if (k != i)
1297  {
1298  cofactor[j][k0] = array[j][k];
1299  k0++;
1300  }
1301  }
1302  }
1303  dFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
1304  dFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
1305  dFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
1306  dFloat32 det = x + y + z;
1307 
1308  normal[i] = sign * det;
1309  sign *= dFloat32(-1.0f);
1310  }
1311 
1312  return normal;
1313  }
1314 
1315  D_INLINE dVector GetInt() const
1316  {
1317  return dVector(dInt32(dgFloor(m_x)), dInt32(dgFloor(m_y)), dInt32(dgFloor(m_z)), dInt32(dgFloor(m_w)));
1318  }
1319 
1320  D_INLINE dVector TestZero() const
1321  {
1322  const dInt32* const a = (dInt32*)&m_x;
1323  return dVector((a[0] == 0) ? dFloat32(-1.0f) : dFloat32(1.0f),
1324  (a[1] == 0) ? dFloat32(-1.0f) : dFloat32(1.0f),
1325  (a[2] == 0) ? dFloat32(-1.0f) : dFloat32(1.0f),
1326  (a[3] == 0) ? dFloat32(-1.0f) : dFloat32(1.0f));
1327  }
1328 
1329  D_INLINE dVector Floor() const
1330  {
1331  return dVector(dgFloor(m_x), dgFloor(m_y), dgFloor(m_z), dgFloor(m_w));
1332  }
1333 
1334  D_INLINE dVector DotProduct(const dVector &A) const
1335  {
1336  return dVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
1337  }
1338 
1339  D_INLINE dVector Reciproc() const
1340  {
1341  return dVector(dFloat32(1.0f) / m_x, dFloat32(1.0f) / m_y, dFloat32(1.0f) / m_z, dFloat32(1.0f) / m_w);
1342  }
1343 
1344  D_INLINE dVector Sqrt() const
1345  {
1346  return dVector(dgSqrt(m_x), dgSqrt(m_y), dgSqrt(m_z), dgSqrt(m_w));
1347  }
1348 
1349  D_INLINE dVector InvSqrt() const
1350  {
1351  return dVector(dgRsqrt(m_x), dgRsqrt(m_y), dgRsqrt(m_z), dgRsqrt(m_w));
1352  }
1353 
1354  D_INLINE dVector InvMagSqrt() const
1355  {
1356  return dVector(dgRsqrt(DotProduct(*this).m_x));
1357  }
1358 
1359  D_INLINE dVector Normalize() const
1360  {
1361  dAssert(m_w == dFloat32(0.0f));
1362  const dVector& me = *this;
1363  return me * InvMagSqrt();
1364  }
1365 
1366  dVector Abs() const
1367  {
1368  return vabsq_f32(m_type);
1369  }
1370 
1371  dFloat32 GetMax() const
1372  {
1373  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
1374  }
1375 
1376  dVector GetMax(const dVector& data) const
1377  {
1378  return vmaxq_f32(m_type, data.m_type);
1379  }
1380 
1381  dVector GetMin(const dVector& data) const
1382  {
1383  return vminq_f32(m_type, data.m_type);
1384  }
1385 
1386  // relational operators
1387  D_INLINE dVector operator== (const dVector& data) const
1388  {
1389  return vceqq_f32(m_typeInt, data.m_typeInt);
1390  }
1391 
1392  D_INLINE dVector operator> (const dVector& data) const
1393  {
1394  return vcgtq_f32(m_typeInt, data.m_typeInt);
1395  }
1396 
1397  D_INLINE dVector operator< (const dVector& data) const
1398  {
1399  return vcltq_f32(m_typeInt, data.m_typeInt);
1400  }
1401 
1402  D_INLINE dVector operator>= (const dVector& data) const
1403  {
1404  return vcgeq_f32(m_typeInt, data.m_typeInt);
1405  }
1406 
1407  D_INLINE dVector operator<= (const dVector& data) const
1408  {
1409  return vcleq_f32(m_typeInt, data.m_typeInt);
1410  }
1411 
1412  // logical operations
1413  D_INLINE dVector operator& (const dVector& data) const
1414  {
1415  return vandq_u32(m_typeInt, data.m_typeInt);
1416  }
1417 
1418  D_INLINE dVector operator| (const dVector& data) const
1419  {
1420  return vorrq_u32(m_typeInt, data.m_typeInt);
1421  }
1422 
1423  D_INLINE dVector operator^ (const dVector& data) const
1424  {
1425  return veorq_u32(m_typeInt, data.m_typeInt);
1426  }
1427 
1428  D_INLINE dVector AndNot(const dVector& data) const
1429  {
1430  return vbicq_u32(m_typeInt, data.m_typeInt);
1431  }
1432 
1433  D_INLINE dVector Select(const dVector& data, const dVector& mask) const
1434  {
1435  // (((b ^ a) & mask)^a)
1436  return (*this) ^ (mask & (data ^ (*this)));
1437  }
1438 
1439  D_INLINE dInt32 GetSignMask() const
1440  {
1441  const dInt32* const a = (dInt32*)&m_x;
1442  return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0));
1443  }
1444 
1445  D_INLINE dVector ShiftRight() const
1446  {
1447  return dVector(m_w, m_x, m_y, m_z);
1448  }
1449 
1450  D_INLINE dVector ShiftTripleRight() const
1451  {
1452  return dVector(m_z, m_x, m_y, m_w);
1453  }
1454 
1455  D_INLINE dVector ShiftTripleLeft() const
1456  {
1457  return dVector(m_y, m_z, m_x, m_w);
1458  }
1459 
1460  D_INLINE dVector ShiftRightLogical(dInt32 bits) const
1461  {
1462  return dVector(dInt32(dUnsigned32(m_ix) >> bits), dInt32(dUnsigned32(m_iy) >> bits), dInt32(dUnsigned32(m_iz) >> bits), dInt32(dUnsigned32(m_iw) >> bits));
1463  }
1464 
1465  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)
1466  {
1467  float32x4x2_t vtrn1 = vzipq_f32(src0.m_type, src2.m_type);
1468  float32x4x2_t vtrn2 = vzipq_f32(src1.m_type, src3.m_type);
1469  float32x4x2_t res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]);
1470  float32x4x2_t res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]);
1471  dst0.m_type = res1.val[0];
1472  dst1.m_type = res1.val[1];
1473  dst2.m_type = res2.val[0];
1474  dst3.m_type = res2.val[1];
1475  }
1476 
1477  DG_CLASS_ALLOCATOR(allocator)
1478 
1479  union
1480  {
1481  dFloat32 m_f[4];
1482  dInt32 m_i[4];
1483  float32x4_t m_type;
1484  uint32x4_t m_typeInt;
1485  struct
1486  {
1487  dFloat32 m_x;
1488  dFloat32 m_y;
1489  dFloat32 m_z;
1490  dFloat32 m_w;
1491  };
1492  struct
1493  {
1494  dInt32 m_ix;
1495  dInt32 m_iy;
1496  dInt32 m_iz;
1497  dInt32 m_iw;
1498  };
1499  };
1500 
1501  static dVector m_zero;
1502  static dVector m_one;
1503  static dVector m_wOne;
1504  static dVector m_half;
1505  static dVector m_two;
1506  static dVector m_three;
1507  static dVector m_negOne;
1508  static dVector m_xMask;
1509  static dVector m_yMask;
1510  static dVector m_zMask;
1511  static dVector m_wMask;
1512  static dVector m_epsilon;
1513  static dVector m_signMask;
1514  static dVector m_triplexMask;
1515 } D_GCC_NEWTON_ALIGN_32 ;
1516 
1517 #endif
1518 
1519 D_MSV_NEWTON_ALIGN_32
1521 {
1522  public:
1523  D_INLINE dBigVector()
1524  {
1525  }
1526 
1527  D_INLINE dBigVector(dFloat64 val)
1528  :m_x(val), m_y(val), m_z(val), m_w(val)
1529  {
1530  }
1531 
1532  D_INLINE dBigVector(const dBigVector& v)
1533  : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
1534  {
1535  }
1536 
1537 #ifndef D_NEWTON_USE_DOUBLE
1538  D_INLINE dBigVector(const dVector& v)
1539  : m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
1540  {
1541  }
1542 
1543  D_INLINE dBigVector(const dFloat32* const ptr)
1544  : m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(dFloat32(0.0f))
1545  {
1546  dAssert(dgCheckVector((*this)));
1547  }
1548 #endif
1549 
1550  D_INLINE dBigVector(const dFloat64* const ptr)
1551  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(ptr[3])
1552  {
1553  dAssert(dgCheckVector((*this)));
1554  }
1555 
1556  D_INLINE dBigVector(dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w)
1557  : m_x(x), m_y(y), m_z(z), m_w(w)
1558  {
1559  dAssert(dgCheckVector((*this)));
1560  }
1561 
1562  D_INLINE dBigVector(dInt32 ix, dInt32 iy, dInt32 iz, dInt32 iw)
1563  : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
1564  {
1565  }
1566 
1567  D_INLINE dBigVector(dInt64 ix, dInt64 iy, dInt64 iz, dInt64 iw)
1568  : m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
1569  {
1570  }
1571 
1572  D_INLINE dFloat64 GetScalar() const
1573  {
1574  return m_x;
1575  }
1576 
1577  D_INLINE void Store(dFloat64* const dst) const
1578  {
1579  dst[0] = m_x;
1580  dst[1] = m_y;
1581  dst[2] = m_z;
1582  dst[3] = m_w;
1583  }
1584 
1585  D_INLINE dBigVector BroadcastX() const
1586  {
1587  return dBigVector(m_x);
1588  }
1589 
1590  D_INLINE dBigVector BroadcastY() const
1591  {
1592  return dBigVector(m_y);
1593  }
1594 
1595  D_INLINE dBigVector BroadcastZ() const
1596  {
1597  return dBigVector(m_z);
1598  }
1599 
1600  D_INLINE dBigVector BroadcastW() const
1601  {
1602  return dBigVector(m_w);
1603  }
1604 
1605  D_INLINE dFloat64& operator[] (dInt32 i)
1606  {
1607  dAssert(i < 4);
1608  dAssert(i >= 0);
1609  return (&m_x)[i];
1610  }
1611 
1612  D_INLINE const dFloat64& operator[] (dInt32 i) const
1613  {
1614  dAssert(i < 4);
1615  dAssert(i >= 0);
1616  return (&m_x)[i];
1617  }
1618 
1619  D_INLINE dBigVector operator+ (const dBigVector& A) const
1620  {
1621  return dBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
1622  }
1623 
1624  D_INLINE dBigVector operator- (const dBigVector& A) const
1625  {
1626  return dBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
1627  }
1628 
1629  D_INLINE dBigVector operator* (const dBigVector& A) const
1630  {
1631  return dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
1632  }
1633 
1634  D_INLINE dBigVector& operator+= (const dBigVector& A)
1635  {
1636  return (*this = dBigVector(m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
1637  }
1638 
1639  D_INLINE dBigVector& operator-= (const dBigVector& A)
1640  {
1641  return (*this = dBigVector(m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
1642  }
1643 
1644  D_INLINE dBigVector& operator*= (const dBigVector& A)
1645  {
1646  return (*this = dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
1647  }
1648 
1649  D_INLINE dBigVector MulAdd(const dBigVector& A, const dBigVector& B) const
1650  {
1651  return *this + A * B;
1652  }
1653 
1654  D_INLINE dBigVector MulSub(const dVector& A, const dBigVector& B) const
1655  {
1656  return *this - A * B;
1657  }
1658 
1659 
1660  D_INLINE dBigVector AddHorizontal() const
1661  {
1662  return dBigVector(m_x + m_y + m_z + m_w);
1663  }
1664 
1665  D_INLINE dBigVector Scale(dFloat64 scale) const
1666  {
1667  return dBigVector(m_x * scale, m_y * scale, m_z * scale, m_w * scale);
1668  }
1669 
1670  // return cross product
1671  D_INLINE dBigVector CrossProduct(const dBigVector& B) const
1672  {
1673  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);
1674  }
1675 
1676  D_INLINE dBigVector CrossProduct(const dBigVector& A, const dBigVector& B) const
1677  {
1678  dFloat64 cofactor[3][3];
1679  dFloat64 array[4][4];
1680 
1681  const dBigVector& me = *this;
1682  for (dInt32 i = 0; i < 4; i++) {
1683  array[0][i] = me[i];
1684  array[1][i] = A[i];
1685  array[2][i] = B[i];
1686  array[3][i] = dFloat32(1.0f);
1687  }
1688 
1689  dBigVector normal;
1690  dFloat64 sign = dFloat64(-1.0f);
1691  for (dInt32 i = 0; i < 4; i++)
1692  {
1693  for (dInt32 j = 0; j < 3; j++)
1694  {
1695  dInt32 k0 = 0;
1696  for (dInt32 k = 0; k < 4; k++)
1697  {
1698  if (k != i)
1699  {
1700  cofactor[j][k0] = array[j][k];
1701  k0++;
1702  }
1703  }
1704  }
1705  dFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
1706  dFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
1707  dFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
1708  dFloat64 det = x + y + z;
1709 
1710  normal[i] = sign * det;
1711  sign *= dFloat64(-1.0f);
1712  }
1713 
1714  return normal;
1715  }
1716 
1717  D_INLINE dBigVector GetInt() const
1718  {
1719  return dBigVector(dInt64(floor(m_x)), dInt64(floor(m_y)), dInt64(floor(m_z)), dInt64(floor(m_w)));
1720  }
1721 
1722  D_INLINE dBigVector TestZero() const
1723  {
1724  const dInt64* const a = (dInt64*)&m_x;
1725  return dBigVector((a[0] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
1726  (a[1] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
1727  (a[2] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f),
1728  (a[3] == 0) ? dFloat64(-1.0f) : dFloat64(1.0f));
1729  }
1730 
1731 
1732  D_INLINE dBigVector Floor() const
1733  {
1734  return dBigVector(floor(m_x), floor(m_y), floor(m_z), floor(m_w));
1735  }
1736 
1737  D_INLINE dBigVector DotProduct(const dBigVector &A) const
1738  {
1739  return dBigVector(m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
1740  }
1741 
1742  D_INLINE dBigVector Reciproc() const
1743  {
1744  return dBigVector(dFloat64(1.0f) / m_x, dFloat64(1.0f) / m_y, dFloat64(1.0f) / m_z, dFloat64(1.0f) / m_w);
1745  }
1746 
1747  D_INLINE dBigVector Sqrt() const
1748  {
1749  return dBigVector(sqrt(m_x), sqrt(m_y), sqrt(m_z), sqrt(m_w));
1750  }
1751 
1752  D_INLINE dBigVector InvSqrt() const
1753  {
1754  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));
1755  }
1756 
1757  D_INLINE dBigVector InvMagSqrt() const
1758  {
1759  return dBigVector(dFloat64(1.0f) / sqrt(DotProduct(*this).m_x));
1760  }
1761 
1762  D_INLINE dBigVector Normalize() const
1763  {
1764  dAssert(m_w == dFloat64(0.0f));
1765  //const dBigVector& me = *this;
1766  //return *this * dBigVector (dgRsqrt(DotProduct(*this).m_x));
1767  return *this * InvMagSqrt();
1768  }
1769 
1770  dBigVector Abs() const
1771  {
1772  return dBigVector((m_x > dFloat64(0.0f)) ? m_x : -m_x,
1773  (m_y > dFloat64(0.0f)) ? m_y : -m_y,
1774  (m_z > dFloat64(0.0f)) ? m_z : -m_z,
1775  (m_w > dFloat64(0.0f)) ? m_w : -m_w);
1776  }
1777 
1778  dFloat64 GetMax() const
1779  {
1780  return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
1781  }
1782 
1783  dBigVector GetMax(const dBigVector& data) const
1784  {
1785  return dBigVector((m_x > data.m_x) ? m_x : data.m_x,
1786  (m_y > data.m_y) ? m_y : data.m_y,
1787  (m_z > data.m_z) ? m_z : data.m_z,
1788  (m_w > data.m_w) ? m_w : data.m_w);
1789  }
1790 
1791  dBigVector GetMin(const dBigVector& data) const
1792  {
1793  return dBigVector((m_x < data.m_x) ? m_x : data.m_x,
1794  (m_y < data.m_y) ? m_y : data.m_y,
1795  (m_z < data.m_z) ? m_z : data.m_z,
1796  (m_w < data.m_w) ? m_w : data.m_w);
1797  }
1798 
1799  // relational operators
1800  D_INLINE dBigVector operator== (const dBigVector& data) const
1801  {
1802  return dBigVector((m_x == data.m_x) ? dInt64(-1) : dInt64(0),
1803  (m_y == data.m_y) ? dInt64(-1) : dInt64(0),
1804  (m_z == data.m_z) ? dInt64(-1) : dInt64(0),
1805  (m_w == data.m_w) ? dInt64(-1) : dInt64(0));
1806  }
1807 
1808  D_INLINE dBigVector operator> (const dBigVector& data) const
1809  {
1810  return dBigVector((m_x > data.m_x) ? dInt64(-1) : dInt64(0),
1811  (m_y > data.m_y) ? dInt64(-1) : dInt64(0),
1812  (m_z > data.m_z) ? dInt64(-1) : dInt64(0),
1813  (m_w > data.m_w) ? dInt64(-1) : dInt64(0));
1814  }
1815 
1816  D_INLINE dBigVector operator< (const dBigVector& data) const
1817  {
1818  return dBigVector((m_x < data.m_x) ? dInt64(-1) : dInt64(0),
1819  (m_y < data.m_y) ? dInt64(-1) : dInt64(0),
1820  (m_z < data.m_z) ? dInt64(-1) : dInt64(0),
1821  (m_w < data.m_w) ? dInt64(-1) : dInt64(0));
1822  }
1823 
1824  D_INLINE dBigVector operator>= (const dBigVector& data) const
1825  {
1826  return dBigVector((m_x >= data.m_x) ? dInt64(-1) : dInt64(0),
1827  (m_y >= data.m_y) ? dInt64(-1) : dInt64(0),
1828  (m_z >= data.m_z) ? dInt64(-1) : dInt64(0),
1829  (m_w >= data.m_w) ? dInt64(-1) : dInt64(0));
1830  }
1831 
1832  D_INLINE dBigVector operator<= (const dBigVector& data) const
1833  {
1834  return dBigVector((m_x <= data.m_x) ? dInt64(-1) : dInt64(0),
1835  (m_y <= data.m_y) ? dInt64(-1) : dInt64(0),
1836  (m_z <= data.m_z) ? dInt64(-1) : dInt64(0),
1837  (m_w <= data.m_w) ? dInt64(-1) : dInt64(0));
1838  }
1839 
1840 
1841  // logical operations
1842  D_INLINE dBigVector operator& (const dBigVector& data) const
1843  {
1844  const dInt64* const a = (dInt64*)&m_x;
1845  const dInt64* const b = (dInt64*)&data.m_x;
1846  return dBigVector(a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
1847  }
1848 
1849  D_INLINE dBigVector operator| (const dBigVector& data) const
1850  {
1851  const dInt64* const a = (dInt64*)&m_x;
1852  const dInt64* const b = (dInt64*)&data.m_x;
1853  return dBigVector(a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
1854  }
1855 
1856  D_INLINE dBigVector operator^ (const dBigVector& data) const
1857  {
1858  const dInt64* const a = (dInt64*)&m_x;
1859  const dInt64* const b = (dInt64*)&data.m_x;
1860  return dBigVector(a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
1861  }
1862 
1863  D_INLINE dBigVector AndNot(const dBigVector& data) const
1864  {
1865  const dInt64* const a = (dInt64*)&m_x;
1866  const dInt64* const b = (dInt64*)&data.m_x;
1867  return dBigVector(a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
1868  }
1869 
1870  D_INLINE dBigVector Select(const dBigVector& data, const dBigVector& mask) const
1871  {
1872  // (((b ^ a) & mask)^a)
1873  return (*this) ^ (mask & (data ^ (*this)));
1874  }
1875 
1876  D_INLINE dInt32 GetSignMask() const
1877  {
1878  const dInt64* const a = (dInt64*)&m_x;
1879  return (((a[0] >> 63) ? 1 : 0) | ((a[1] >> 63) ? 2 : 0) | ((a[2] >> 63) ? 4 : 0) | ((a[3] >> 63) ? 8 : 0));
1880  }
1881 
1882  D_INLINE dVector ShiftRight() const
1883  {
1884  return dBigVector(m_w, m_x, m_y, m_z);
1885  }
1886 
1887  D_INLINE dBigVector ShiftTripleRight() const
1888  {
1889  return dBigVector(m_z, m_x, m_y, m_w);
1890  }
1891 
1892  D_INLINE dBigVector ShiftTripleLeft() const
1893  {
1894  return dBigVector(m_y, m_z, m_x, m_w);
1895  }
1896 
1897  D_INLINE dBigVector ShiftRightLogical(dInt32 bits) const
1898  {
1899  return dBigVector(dInt64(dUnsigned64(m_ix) >> bits), dInt64(dUnsigned64(m_iy) >> bits), dInt64(dUnsigned64(m_iz) >> bits), dInt64(dUnsigned64(m_iw) >> bits));
1900  }
1901 
1902  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)
1903  {
1904  dBigVector tmp0(src0);
1905  dBigVector tmp1(src1);
1906  dBigVector tmp2(src2);
1907  dBigVector tmp3(src3);
1908 
1909  dst0 = dBigVector(tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
1910  dst1 = dBigVector(tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
1911  dst2 = dBigVector(tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
1912  dst3 = dBigVector(tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
1913  }
1914 
1915  DG_CLASS_ALLOCATOR(allocator)
1916 
1917  union
1918  {
1919  dInt64 m_i[4];
1920  struct
1921  {
1922  dFloat64 m_x;
1923  dFloat64 m_y;
1924  dFloat64 m_z;
1925  dFloat64 m_w;
1926  };
1927  struct
1928  {
1929  dInt64 m_ix;
1930  dInt64 m_iy;
1931  dInt64 m_iz;
1932  dInt64 m_iw;
1933  };
1934  };
1935 
1936  static dBigVector m_zero;
1937  static dBigVector m_one;
1938  static dBigVector m_wOne;
1939  static dBigVector m_half;
1940  static dBigVector m_two;
1941  static dBigVector m_three;
1942  static dBigVector m_negOne;
1943  static dBigVector m_xMask;
1944  static dBigVector m_yMask;
1945  static dBigVector m_zMask;
1946  static dBigVector m_wMask;
1947  static dBigVector m_epsilon;
1948  static dBigVector m_signMask;
1949  static dBigVector m_triplexMask;
1950 } D_GCC_NEWTON_ALIGN_32 ;
1951 
1952 D_MSV_NEWTON_ALIGN_32
1954 {
1955  public:
1956  D_INLINE dSpatialVector()
1957  {
1958  }
1959 
1960  D_INLINE dSpatialVector(const dFloat32 a)
1961  {
1962  for (dInt32 i = 0; i < 6; i++)
1963  {
1964  m_d[i] = a;
1965  }
1966  }
1967 
1968  D_INLINE dSpatialVector(const dVector& low, const dVector& high)
1969  {
1970  for (dInt32 i = 0; i < 3; i++)
1971  {
1972  m_d[i] = low[i];
1973  m_d[i + 3] = high[i];
1974  }
1975  }
1976 
1977  D_INLINE dSpatialVector(const dSpatialVector& src)
1978  {
1979  for (dInt32 i = 0; i < 6; i++)
1980  {
1981  m_d[i] = src[i];
1982  }
1983  }
1984 
1985  D_INLINE dFloat64& operator[] (dInt32 i)
1986  {
1987  dAssert(i < 6);
1988  dAssert(i >= 0);
1989  return m_d[i];
1990  }
1991 
1992  D_INLINE const dFloat64& operator[] (dInt32 i) const
1993  {
1994  dAssert(i < 6);
1995  dAssert(i >= 0);
1996  return m_d[i];
1997  }
1998 
1999  D_INLINE dSpatialVector operator+ (const dSpatialVector& A) const
2000  {
2001  dSpatialVector tmp;
2002  for (dInt32 i = 0; i < 6; i++)
2003  {
2004  tmp[i] = m_d[i] + A.m_d[i];
2005  }
2006  return tmp;
2007  }
2008 
2009  D_INLINE dSpatialVector operator* (const dSpatialVector& A) const
2010  {
2011  dSpatialVector tmp;
2012  for (dInt32 i = 0; i < 6; i++)
2013  {
2014  tmp[i] = m_d[i] * A.m_d[i];
2015  }
2016  return tmp;
2017  }
2018 
2019  D_INLINE dFloat64 DotProduct(const dSpatialVector& v) const
2020  {
2021  dFloat64 ret = dFloat64(0.0f);
2022  for (dInt32 i = 0; i < 6; i++)
2023  {
2024  ret += m_d[i] * v.m_d[i];
2025  }
2026  return ret;
2027  }
2028 
2029  D_INLINE dSpatialVector Scale(dFloat64 s) const
2030  {
2031  dSpatialVector tmp;
2032  for (dInt32 i = 0; i < 6; i++)
2033  {
2034  tmp[i] = m_d[i] * s;
2035  }
2036  return tmp;
2037  }
2038 
2039  dFloat64 m_d[6];
2040  static dSpatialVector m_zero;
2041 } D_GCC_NEWTON_ALIGN_32 ;
2042 
2043 #endif
dBigVector
Definition: dVectorArmNeon.h:1521
dVector
Definition: dVectorArmNeon.h:1104
dSpatialVector
Definition: dVectorArmNeon.h:1954