22 #ifndef __ND_VECTOR_SCALAR_H__
23 #define __ND_VECTOR_SCALAR_H__
30 #ifdef D_NEWTON_USE_DOUBLE
31 #define ndVector ndBigVector
44 D_OPERATOR_NEW_AND_DELETE
51 :m_x(val), m_y(val), m_z(val), m_w(val)
56 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
60 inline ndVector (
const ndFloat32*
const ptr)
66 ndAssert (ndCheckVector ((*
this)));
70 inline ndVector(
const ndFloat32*
const baseAddr,
const ndInt32*
const index)
71 :m_x(baseAddr[index[0]])
72 ,m_y(baseAddr[index[1]])
73 ,m_z(baseAddr[index[2]])
74 ,m_w(baseAddr[index[3]])
78 #ifndef D_NEWTON_USE_DOUBLE
79 inline ndVector(
const ndFloat64*
const ptr)
80 :m_x(ndFloat32(ptr[0]))
81 ,m_y(ndFloat32(ptr[1]))
82 ,m_z(ndFloat32(ptr[2]))
83 ,m_w(ndFloat32(ptr[3]))
89 inline ndVector (ndFloat32 x, ndFloat32 y, ndFloat32 z, ndFloat32 w)
90 :m_x(x), m_y(y), m_z(z), m_w(w)
92 ndAssert (ndCheckVector ((*
this)));
95 inline ndVector (ndInt32 ix, ndInt32 iy, ndInt32 iz, ndInt32 iw)
96 :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
100 #ifndef D_NEWTON_USE_DOUBLE
102 :m_x(ndFloat32 (((ndFloat64*)©)[0]))
103 ,m_y(ndFloat32 (((ndFloat64*)©)[1]))
104 ,m_z(ndFloat32 (((ndFloat64*)©)[2]))
105 ,m_w(ndFloat32 (((ndFloat64*)©)[3]))
107 ndAssert (ndCheckVector ((*
this)));
111 inline ndFloat32 GetScalar ()
const
116 inline void Store (ndFloat32*
const dst)
const
145 inline ndFloat32& operator[] (ndInt32 i)
152 inline const ndFloat32& operator[] (ndInt32 i)
const
161 return ndVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
166 return ndVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
171 return ndVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
176 return (*
this =
ndVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
181 return (*
this =
ndVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
186 return (*
this =
ndVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
191 return *
this + A * B;
196 return *
this - A * B;
199 inline ndVector AddHorizontal ()
const
201 return ndVector (m_x + m_y + m_z + m_w);
204 inline ndVector Scale (ndFloat32 scale)
const
206 return ndVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
212 return ndVector (m_y * B.m_z - m_z * B.m_y,
213 m_z * B.m_x - m_x * B.m_z,
214 m_x * B.m_y - m_y * B.m_x, m_w);
219 ndFloat32 cofactor[3][3];
220 ndFloat32 array[4][4];
223 for (ndInt32 i = 0; i < 4; ++i)
228 array[3][i] = ndFloat32 (1.0f);
232 ndFloat32 sign = ndFloat32 (-1.0f);
233 for (ndInt32 i = 0; i < 4; ++i)
235 for (ndInt32 j = 0; j < 3; ++j)
238 for (ndInt32 k = 0; k < 4; ++k)
242 cofactor[j][k0] = array[j][k];
247 ndFloat32 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
248 ndFloat32 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
249 ndFloat32 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
250 ndFloat32 det = x + y + z;
252 normal[i] = sign * det;
253 sign *= ndFloat32 (-1.0f);
261 return ndVector (ndInt32 (ndFloor (m_x)), ndInt32(ndFloor (m_y)), ndInt32(ndFloor (m_z)), ndInt32 (ndFloor (m_w)));
266 const ndInt32*
const a = (ndInt32*)&m_x;
268 (a[0] == 0) ? ndFloat32 (-1.0f) : ndFloat32 (0.0f),
269 (a[1] == 0) ? ndFloat32 (-1.0f) : ndFloat32 (0.0f),
270 (a[2] == 0) ? ndFloat32 (-1.0f) : ndFloat32 (0.0f),
271 (a[3] == 0) ? ndFloat32 (-1.0f) : ndFloat32 (0.0f));
276 return ndVector (ndFloor (m_x), ndFloor (m_y), ndFloor (m_z), ndFloor (m_w));
281 return ndVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
286 return ndVector (ndFloat32 (1.0f) / m_x, ndFloat32 (1.0f) / m_y, ndFloat32 (1.0f) / m_z, ndFloat32 (1.0f) / m_w);
291 return ndVector (ndSqrt (m_x), ndSqrt (m_y), ndSqrt (m_z), ndSqrt (m_w));
296 return ndVector (ndRsqrt (m_x), ndRsqrt (m_y), ndRsqrt (m_z), ndRsqrt (m_w));
301 return ndVector (ndRsqrt (DotProduct(*this).m_x));
307 return me * InvMagSqrt();
313 (m_x > ndFloat32 (0.0f)) ? m_x : -m_x,
314 (m_y > ndFloat32 (0.0f)) ? m_y : -m_y,
315 (m_z > ndFloat32 (0.0f)) ? m_z : -m_z,
316 (m_w > ndFloat32 (0.0f)) ? m_w : -m_w);
321 return ndVector (ndMax(ndMax(m_x, m_y), ndMax(m_z, m_w)));
327 (m_x > data.m_x) ? m_x : data.m_x,
328 (m_y > data.m_y) ? m_y : data.m_y,
329 (m_z > data.m_z) ? m_z : data.m_z,
330 (m_w > data.m_w) ? m_w : data.m_w);
336 (m_x < data.m_x) ? m_x : data.m_x,
337 (m_y < data.m_y) ? m_y : data.m_y,
338 (m_z < data.m_z) ? m_z : data.m_z,
339 (m_w < data.m_w) ? m_w : data.m_w);
347 (m_x == data.m_x) ? ndInt32 (0xffffffff) : 0,
348 (m_y == data.m_y) ? ndInt32 (0xffffffff) : 0,
349 (m_z == data.m_z) ? ndInt32 (0xffffffff) : 0,
350 (m_w == data.m_w) ? ndInt32 (0xffffffff) : 0);
356 (m_x > data.m_x) ? ndInt32 (0xffffffff) : 0,
357 (m_y > data.m_y) ? ndInt32 (0xffffffff) : 0,
358 (m_z > data.m_z) ? ndInt32 (0xffffffff) : 0,
359 (m_w > data.m_w) ? ndInt32 (0xffffffff) : 0);
365 (m_x < data.m_x) ? ndInt32 (0xffffffff) : 0,
366 (m_y < data.m_y) ? ndInt32 (0xffffffff) : 0,
367 (m_z < data.m_z) ? ndInt32 (0xffffffff) : 0,
368 (m_w < data.m_w) ? ndInt32 (0xffffffff) : 0);
374 (m_x >= data.m_x) ? ndInt32 (0xffffffff) : 0,
375 (m_y >= data.m_y) ? ndInt32 (0xffffffff) : 0,
376 (m_z >= data.m_z) ? ndInt32 (0xffffffff) : 0,
377 (m_w >= data.m_w) ? ndInt32 (0xffffffff) : 0);
383 (m_x <= data.m_x) ? ndInt32 (0xffffffff) : 0,
384 (m_y <= data.m_y) ? ndInt32 (0xffffffff) : 0,
385 (m_z <= data.m_z) ? ndInt32 (0xffffffff) : 0,
386 (m_w <= data.m_w) ? ndInt32 (0xffffffff) : 0);
392 const ndInt32*
const a = (ndInt32*)&m_x;
393 const ndInt32*
const b = (ndInt32*)&data.m_x;
394 return ndVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
399 const ndInt32*
const a = (ndInt32*)&m_x;
400 const ndInt32*
const b = (ndInt32*)&data.m_x;
401 return ndVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
406 const ndInt32*
const a = (ndInt32*)&m_x;
407 const ndInt32*
const b = (ndInt32*)&data.m_x;
408 return ndVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
413 const ndInt32*
const a = (ndInt32*)&m_x;
414 const ndInt32*
const b = (ndInt32*)&data.m_x;
415 return ndVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
421 return (*
this) ^ (mask & (data ^ (*this)));
424 inline ndInt32 GetSignMask()
const
426 const ndInt32*
const a = (ndInt32*)&m_x;
427 return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0));
432 return ndVector (m_w, m_x, m_y, m_z);
435 inline ndVector ShiftTripleRight ()
const
437 return ndVector (m_z, m_x, m_y, m_w);
440 inline ndVector ShiftTripleLeft ()
const
442 return ndVector (m_y, m_z, m_x, m_w);
445 inline ndVector ShiftRightLogical (ndInt32 bits)
const
447 return ndVector (ndInt32 (ndUnsigned32 (m_ix) >> bits), ndInt32 (ndUnsigned32 (m_iy) >> bits), ndInt32 (ndUnsigned32 (m_iz) >> bits), ndInt32 (ndUnsigned32 (m_iw) >> bits));
457 dst0 =
ndVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
458 dst1 =
ndVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
459 dst2 =
ndVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
460 dst3 =
ndVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
488 D_CORE_API
static ndVector m_negOne;
493 D_CORE_API
static ndVector m_xyzwMask;
494 D_CORE_API
static ndVector m_epsilon;
495 D_CORE_API
static ndVector m_signMask;
496 D_CORE_API
static ndVector m_triplexMask;
497 } D_GCC_NEWTON_ALIGN_16;
505 D_MSV_NEWTON_ALIGN_32
509 D_OPERATOR_NEW_AND_DELETE
516 :m_x(val), m_y(val), m_z(val), m_w(val)
521 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
525 #ifndef D_NEWTON_USE_DOUBLE
527 :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
532 :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ndFloat32 (0.0f))
534 ndAssert (ndCheckVector ((*
this)));
539 :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3])
541 ndAssert (ndCheckVector ((*
this)));
544 inline ndBigVector (ndFloat64 x, ndFloat64 y, ndFloat64 z, ndFloat64 w)
545 :m_x(x), m_y(y), m_z(z), m_w(w)
547 ndAssert (ndCheckVector ((*
this)));
550 inline ndBigVector (ndInt32 ix, ndInt32 iy, ndInt32 iz, ndInt32 iw)
551 :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
555 inline ndBigVector (ndInt64 ix, ndInt64 iy, ndInt64 iz, ndInt64 iw)
556 :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
560 inline ndFloat64 GetScalar ()
const
565 inline void Store (ndFloat64*
const dst)
const
594 inline ndFloat64& operator[] (ndInt32 i)
601 inline const ndFloat64& operator[] (ndInt32 i)
const
610 return ndBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
615 return ndBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
620 return ndBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
625 return (*
this =
ndBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
630 return (*
this =
ndBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
635 return (*
this =
ndBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
640 return *
this + A * B;
645 return *
this - A * B;
656 return ndBigVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
662 return ndBigVector (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);
667 ndFloat64 cofactor[3][3];
668 ndFloat64 array[4][4];
671 for (ndInt32 i = 0; i < 4; ++i)
676 array[3][i] = ndFloat32 (1.0f);
680 ndFloat64 sign = ndFloat64 (-1.0f);
681 for (ndInt32 i = 0; i < 4; ++i)
683 for (ndInt32 j = 0; j < 3; ++j)
686 for (ndInt32 k = 0; k < 4; ++k)
690 cofactor[j][k0] = array[j][k];
695 ndFloat64 x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
696 ndFloat64 y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
697 ndFloat64 z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
698 ndFloat64 det = x + y + z;
700 normal[i] = sign * det;
701 sign *= ndFloat64 (-1.0f);
709 return ndBigVector (ndInt64 (floor (m_x)), ndInt64(floor (m_y)), ndInt64(floor (m_z)), ndInt64 (floor (m_w)));
714 const ndInt64*
const a = (ndInt64*)&m_x;
715 return ndBigVector ((a[0] == 0) ? ndFloat64 (-1.0f) : ndFloat64 (1.0f),
716 (a[1] == 0) ? ndFloat64 (-1.0f) : ndFloat64 (1.0f),
717 (a[2] == 0) ? ndFloat64 (-1.0f) : ndFloat64 (1.0f),
718 (a[3] == 0) ? ndFloat64 (-1.0f) : ndFloat64 (1.0f));
724 return ndBigVector (floor (m_x), floor (m_y), floor (m_z), floor (m_w));
729 return ndBigVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
734 return ndBigVector (ndFloat64 (1.0f) / m_x, ndFloat64 (1.0f) / m_y, ndFloat64 (1.0f) / m_z, ndFloat64 (1.0f) / m_w);
739 return ndBigVector (sqrt (m_x), sqrt (m_y), sqrt (m_z), sqrt (m_w));
744 return ndBigVector (ndFloat64 (1.0f) / sqrt (m_x), ndFloat64 (1.0f) / sqrt (m_y), ndFloat64 (1.0f) / sqrt (m_z), ndFloat64 (1.0f) / sqrt (m_w));
749 return ndBigVector (ndFloat64 (1.0f) / sqrt (DotProduct(*this).m_x));
754 return *
this * InvMagSqrt();
759 return ndBigVector ((m_x > ndFloat64 (0.0f)) ? m_x : -m_x,
760 (m_y > ndFloat64 (0.0f)) ? m_y : -m_y,
761 (m_z > ndFloat64 (0.0f)) ? m_z : -m_z,
762 (m_w > ndFloat64 (0.0f)) ? m_w : -m_w);
767 return ndBigVector (ndMax(ndMax(m_x, m_y), ndMax(m_z, m_w)));
772 return ndBigVector ((m_x > data.m_x) ? m_x : data.m_x,
773 (m_y > data.m_y) ? m_y : data.m_y,
774 (m_z > data.m_z) ? m_z : data.m_z,
775 (m_w > data.m_w) ? m_w : data.m_w);
780 return ndBigVector ((m_x < data.m_x) ? m_x : data.m_x,
781 (m_y < data.m_y) ? m_y : data.m_y,
782 (m_z < data.m_z) ? m_z : data.m_z,
783 (m_w < data.m_w) ? m_w : data.m_w);
789 return ndBigVector ((m_x == data.m_x) ? ndInt64 (-1) : ndInt64 (0),
790 (m_y == data.m_y) ? ndInt64 (-1) : ndInt64 (0),
791 (m_z == data.m_z) ? ndInt64 (-1) : ndInt64 (0),
792 (m_w == data.m_w) ? ndInt64 (-1) : ndInt64 (0));
797 return ndBigVector ((m_x > data.m_x) ? ndInt64 (-1) : ndInt64 (0),
798 (m_y > data.m_y) ? ndInt64 (-1) : ndInt64 (0),
799 (m_z > data.m_z) ? ndInt64 (-1) : ndInt64 (0),
800 (m_w > data.m_w) ? ndInt64 (-1) : ndInt64 (0));
805 return ndBigVector ((m_x < data.m_x) ? ndInt64 (-1) : ndInt64 (0),
806 (m_y < data.m_y) ? ndInt64 (-1) : ndInt64 (0),
807 (m_z < data.m_z) ? ndInt64 (-1) : ndInt64 (0),
808 (m_w < data.m_w) ? ndInt64 (-1) : ndInt64 (0));
813 return ndBigVector ((m_x >= data.m_x) ? ndInt64 (-1) : ndInt64 (0),
814 (m_y >= data.m_y) ? ndInt64 (-1) : ndInt64 (0),
815 (m_z >= data.m_z) ? ndInt64 (-1) : ndInt64 (0),
816 (m_w >= data.m_w) ? ndInt64 (-1) : ndInt64 (0));
821 return ndBigVector ((m_x <= data.m_x) ? ndInt64 (-1) : ndInt64 (0),
822 (m_y <= data.m_y) ? ndInt64 (-1) : ndInt64 (0),
823 (m_z <= data.m_z) ? ndInt64 (-1) : ndInt64 (0),
824 (m_w <= data.m_w) ? ndInt64 (-1) : ndInt64 (0));
830 const ndInt64*
const a = (ndInt64*)&m_x;
831 const ndInt64*
const b = (ndInt64*)&data.m_x;
832 return ndBigVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]);
837 const ndInt64*
const a = (ndInt64*)&m_x;
838 const ndInt64*
const b = (ndInt64*)&data.m_x;
839 return ndBigVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]);
844 const ndInt64*
const a = (ndInt64*)&m_x;
845 const ndInt64*
const b = (ndInt64*)&data.m_x;
846 return ndBigVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]);
851 const ndInt64*
const a = (ndInt64*)&m_x;
852 const ndInt64*
const b = (ndInt64*)&data.m_x;
853 return ndBigVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]);
859 return (*
this) ^ (mask & (data ^ (*this)));
862 inline ndInt32 GetSignMask()
const
864 const ndInt64*
const a = (ndInt64*)&m_x;
865 return (((a[0]>>63) ? 1 : 0) | ((a[1]>>63) ? 2 : 0) | ((a[2]>>63) ? 4 : 0) | ((a[3]>>63) ? 8 : 0));
883 inline ndBigVector ShiftRightLogical (ndInt32 bits)
const
885 return ndBigVector (ndInt64 (ndUnsigned64 (m_ix) >> bits), ndInt64 (ndUnsigned64 (m_iy) >> bits), ndInt64 (ndUnsigned64 (m_iz) >> bits), ndInt64 (ndUnsigned64 (m_iw) >> bits));
895 dst0 =
ndBigVector (tmp0.m_x, tmp1.m_x, tmp2.m_x, tmp3.m_x);
896 dst1 =
ndBigVector (tmp0.m_y, tmp1.m_y, tmp2.m_y, tmp3.m_y);
897 dst2 =
ndBigVector (tmp0.m_z, tmp1.m_z, tmp2.m_z, tmp3.m_z);
898 dst3 =
ndBigVector (tmp0.m_w, tmp1.m_w, tmp2.m_w, tmp3.m_w);
935 } D_GCC_NEWTON_ALIGN_32;