22 #ifndef __D_VECTOR_SCALAR_H__
23 #define __D_VECTOR_SCALAR_H__
30 #ifdef D_NEWTON_USE_DOUBLE
31 #define dVector dBigVector
43 :m_x(val), m_y(val), m_z(val), m_w(val)
48 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
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])
56 dAssert (dCheckVector ((*
this)));
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]))
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)
73 dAssert (dCheckVector ((*
this)));
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))
81 #ifndef D_NEWTON_USE_DOUBLE
83 :m_x(dFloat32 (((dFloat64*)©)[0]))
84 ,m_y(dFloat32 (((dFloat64*)©)[1]))
85 ,m_z(dFloat32 (((dFloat64*)©)[2]))
86 ,m_w(dFloat32 (((dFloat64*)©)[3]))
88 dAssert (dCheckVector ((*
this)));
92 D_INLINE dFloat32 GetScalar ()
const
97 D_INLINE
void Store (dFloat32*
const dst)
const
105 D_INLINE
dVector BroadcastX ()
const
110 D_INLINE
dVector BroadcastY ()
const
115 D_INLINE
dVector BroadcastZ ()
const
120 D_INLINE
dVector BroadcastW ()
const
126 D_INLINE dFloat32& operator[] (dInt32 i)
133 D_INLINE
const dFloat32& operator[] (dInt32 i)
const
142 return dVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
147 return dVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
152 return dVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
157 return (*
this =
dVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
162 return (*
this =
dVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
167 return (*
this =
dVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
172 return *
this + A * B;
177 return *
this - A * B;
180 D_INLINE
dVector AddHorizontal ()
const
182 return dVector (m_x + m_y + m_z + m_w);
185 D_INLINE
dVector Scale (dFloat32 scale)
const
187 return dVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
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);
200 dFloat32 cofactor[3][3];
201 dFloat32 array[4][4];
204 for (dInt32 i = 0; i < 4; i ++)
209 array[3][i] = dFloat32 (1.0f);
213 dFloat32 sign = dFloat32 (-1.0f);
214 for (dInt32 i = 0; i < 4; i ++)
216 for (dInt32 j = 0; j < 3; j ++)
219 for (dInt32 k = 0; k < 4; k ++)
223 cofactor[j][k0] = array[j][k];
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;
233 normal[i] = sign * det;
234 sign *= dFloat32 (-1.0f);
240 D_INLINE
dVector GetInt ()
const
242 return dVector (dInt32 (dFloor (m_x)), dInt32(dFloor (m_y)), dInt32(dFloor (m_z)), dInt32 (dFloor (m_w)));
245 D_INLINE
dVector TestZero()
const
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));
255 D_INLINE
dVector Floor ()
const
257 return dVector (dFloor (m_x), dFloor (m_y), dFloor (m_z), dFloor (m_w));
262 return dVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
265 D_INLINE
dVector Reciproc ()
const
267 return dVector (dFloat32 (1.0f) / m_x, dFloat32 (1.0f) / m_y, dFloat32 (1.0f) / m_z, dFloat32 (1.0f) / m_w);
272 return dVector (dSqrt (m_x), dSqrt (m_y), dSqrt (m_z), dSqrt (m_w));
275 D_INLINE
dVector InvSqrt ()
const
277 return dVector (dRsqrt (m_x), dRsqrt (m_y), dRsqrt (m_z), dRsqrt (m_w));
280 D_INLINE
dVector InvMagSqrt ()
const
282 return dVector (dRsqrt (DotProduct(*this).m_x));
285 D_INLINE
dVector Normalize ()
const
287 dAssert (m_w == dFloat32 (0.0f));
291 return me * InvMagSqrt();
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);
302 dFloat32 GetMax ()
const
304 return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
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);
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);
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);
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);
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);
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);
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);
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]);
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]);
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]);
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]);
398 return (*
this) ^ (mask & (data ^ (*this)));
401 D_INLINE dInt32 GetSignMask()
const
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));
407 D_INLINE
dVector ShiftRight()
const
409 return dVector (m_w, m_x, m_y, m_z);
412 D_INLINE
dVector ShiftTripleRight ()
const
414 return dVector (m_z, m_x, m_y, m_w);
417 D_INLINE
dVector ShiftTripleLeft ()
const
419 return dVector (m_y, m_z, m_x, m_w);
422 D_INLINE
dVector ShiftRightLogical (dInt32 bits)
const
424 return dVector (dInt32 (dUnsigned32 (m_ix) >> bits), dInt32 (dUnsigned32 (m_iy) >> bits), dInt32 (dUnsigned32 (m_iz) >> bits), dInt32 (dUnsigned32 (m_iw) >> bits));
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);
473 } D_GCC_NEWTON_ALIGN_32 ;
485 :m_x(val), m_y(val), m_z(val), m_w(val)
490 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
494 #ifndef D_NEWTON_USE_DOUBLE
496 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
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))
503 dAssert (dCheckVector ((*
this)));
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])
510 dAssert (dCheckVector ((*
this)));
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)
516 dAssert (dCheckVector ((*
this)));
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)
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)
529 D_INLINE dFloat64 GetScalar ()
const
534 D_INLINE
void Store (dFloat64*
const dst)
const
563 D_INLINE dFloat64& operator[] (dInt32 i)
570 D_INLINE
const dFloat64& operator[] (dInt32 i)
const
579 return dBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
584 return dBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
589 return dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
594 return (*
this =
dBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
599 return (*
this =
dBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
604 return (*
this =
dBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
609 return *
this + A * B;
614 return *
this - A * B;
623 D_INLINE
dBigVector Scale (dFloat64 scale)
const
625 return dBigVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
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);
636 dFloat64 cofactor[3][3];
637 dFloat64 array[4][4];
640 for (dInt32 i = 0; i < 4; i ++)
645 array[3][i] = dFloat32 (1.0f);
649 dFloat64 sign = dFloat64 (-1.0f);
650 for (dInt32 i = 0; i < 4; i ++)
652 for (dInt32 j = 0; j < 3; j ++)
655 for (dInt32 k = 0; k < 4; k ++)
659 cofactor[j][k0] = array[j][k];
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;
669 normal[i] = sign * det;
670 sign *= dFloat64 (-1.0f);
678 return dBigVector (dInt64 (floor (m_x)), dInt64(floor (m_y)), dInt64(floor (m_z)), dInt64 (floor (m_w)));
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));
693 return dBigVector (floor (m_x), floor (m_y), floor (m_z), floor (m_w));
698 return dBigVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
703 return dBigVector (dFloat64 (1.0f) / m_x, dFloat64 (1.0f) / m_y, dFloat64 (1.0f) / m_z, dFloat64 (1.0f) / m_w);
708 return dBigVector (sqrt (m_x), sqrt (m_y), sqrt (m_z), sqrt (m_w));
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));
718 return dBigVector (dFloat64 (1.0f) / sqrt (DotProduct(*this).m_x));
723 dAssert (m_w == dFloat64 (0.0f));
726 return *
this * InvMagSqrt();
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);
737 dFloat64 GetMax ()
const
739 return dMax(dMax(m_x, m_y), dMax(m_z, m_w));
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);
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);
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));
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));
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));
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));
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));
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]);
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]);
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]);
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]);
831 return (*
this) ^ (mask & (data ^ (*this)));
834 D_INLINE dInt32 GetSignMask()
const
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));
840 D_INLINE
dVector ShiftRight()
const
855 D_INLINE
dBigVector ShiftRightLogical (dInt32 bits)
const
857 return dBigVector (dInt64 (dUnsigned64 (m_ix) >> bits), dInt64 (dUnsigned64 (m_iy) >> bits), dInt64 (dUnsigned64 (m_iz) >> bits), dInt64 (dUnsigned64 (m_iw) >> bits));
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);
906 } D_GCC_NEWTON_ALIGN_32;
917 for (dInt32 i = 0; i < 6; i ++)
925 for (dInt32 i = 0; i < 3; i ++)
928 m_d[i + 3] = high[i];
934 for (dInt32 i = 0; i < 6; i++)
940 D_INLINE dFloat64& operator[] (dInt32 i)
947 D_INLINE
const dFloat64& operator[] (dInt32 i)
const
957 for (dInt32 i = 0; i < 6; i++)
959 tmp[i] = m_d[i] + A.m_d[i];
967 for (dInt32 i = 0; i < 6; i++)
969 tmp[i] = m_d[i] * A.m_d[i];
976 dFloat64 ret = dFloat64 (0.0f);
977 for (dInt32 i = 0; i < 6; i++)
979 ret += m_d[i] * v.m_d[i];
987 for (dInt32 i = 0; i < 6; i++)
996 } D_GCC_NEWTON_ALIGN_32 ;