Newton Dynamics  4.00
ndVectorScalar.h
1 /* Copyright (c) <2003-2022> <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 __ND_VECTOR_SCALAR_H__
23 #define __ND_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 ndVector ndBigVector
32 #else
33 
34 class ndBigVector;
35 // *****************************************************************************************
36 //
37 // 4 x 1 single precision SSE vector class declaration
38 //
39 // *****************************************************************************************
40 D_MSV_NEWTON_ALIGN_16
41 class ndVector
42 {
43  public:
44  D_OPERATOR_NEW_AND_DELETE
45 
46  inline ndVector()
47  {
48  }
49 
50  inline ndVector(ndFloat32 val)
51  :m_x(val), m_y(val), m_z(val), m_w(val)
52  {
53  }
54 
55  inline ndVector (const ndVector& v)
56  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
57  {
58  }
59 
60  inline ndVector (const ndFloat32* const ptr)
61  :m_x(ptr[0])
62  ,m_y(ptr[1])
63  ,m_z(ptr[2])
64  ,m_w(ptr[3])
65  {
66  ndAssert (ndCheckVector ((*this)));
67  }
68 
69  // emulate gather instruction
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]])
75  {
76  }
77 
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]))
84  {
85  }
86 #endif
87 
88 
89  inline ndVector (ndFloat32 x, ndFloat32 y, ndFloat32 z, ndFloat32 w)
90  :m_x(x), m_y(y), m_z(z), m_w(w)
91  {
92  ndAssert (ndCheckVector ((*this)));
93  }
94 
95  inline ndVector (ndInt32 ix, ndInt32 iy, ndInt32 iz, ndInt32 iw)
96  :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
97  {
98  }
99 
100 #ifndef D_NEWTON_USE_DOUBLE
101  inline ndVector (const ndBigVector& copy)
102  :m_x(ndFloat32 (((ndFloat64*)&copy)[0]))
103  ,m_y(ndFloat32 (((ndFloat64*)&copy)[1]))
104  ,m_z(ndFloat32 (((ndFloat64*)&copy)[2]))
105  ,m_w(ndFloat32 (((ndFloat64*)&copy)[3]))
106  {
107  ndAssert (ndCheckVector ((*this)));
108  }
109 #endif
110 
111  inline ndFloat32 GetScalar () const
112  {
113  return m_x;
114  }
115 
116  inline void Store (ndFloat32* const dst) const
117  {
118  dst[0] = m_x;
119  dst[1] = m_y;
120  dst[2] = m_z;
121  dst[3] = m_w;
122  }
123 
124  inline ndVector BroadcastX () const
125  {
126  return ndVector (m_x);
127  }
128 
129  inline ndVector BroadcastY () const
130  {
131  return ndVector (m_y);
132  }
133 
134  inline ndVector BroadcastZ () const
135  {
136  return ndVector (m_z);
137  }
138 
139  inline ndVector BroadcastW () const
140  {
141  return ndVector (m_w);
142  }
143 
144 
145  inline ndFloat32& operator[] (ndInt32 i)
146  {
147  ndAssert (i < 4);
148  ndAssert (i >= 0);
149  return (&m_x)[i];
150  }
151 
152  inline const ndFloat32& operator[] (ndInt32 i) const
153  {
154  ndAssert (i < 4);
155  ndAssert (i >= 0);
156  return (&m_x)[i];
157  }
158 
159  inline ndVector operator+ (const ndVector& A) const
160  {
161  return ndVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
162  }
163 
164  inline ndVector operator- (const ndVector& A) const
165  {
166  return ndVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
167  }
168 
169  inline ndVector operator* (const ndVector& A) const
170  {
171  return ndVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
172  }
173 
174  inline ndVector& operator+= (const ndVector& A)
175  {
176  return (*this = ndVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
177  }
178 
179  inline ndVector& operator-= (const ndVector& A)
180  {
181  return (*this = ndVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
182  }
183 
184  inline ndVector& operator*= (const ndVector& A)
185  {
186  return (*this = ndVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
187  }
188 
189  inline ndVector MulAdd(const ndVector& A, const ndVector& B) const
190  {
191  return *this + A * B;
192  }
193 
194  inline ndVector MulSub(const ndVector& A, const ndVector& B) const
195  {
196  return *this - A * B;
197  }
198 
199  inline ndVector AddHorizontal () const
200  {
201  return ndVector (m_x + m_y + m_z + m_w);
202  }
203 
204  inline ndVector Scale (ndFloat32 scale) const
205  {
206  return ndVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
207  }
208 
209  // return cross product
210  inline ndVector CrossProduct (const ndVector& B) const
211  {
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);
215  }
216 
217  inline ndVector CrossProduct (const ndVector& A, const ndVector& B) const
218  {
219  ndFloat32 cofactor[3][3];
220  ndFloat32 array[4][4];
221 
222  const ndVector& me = *this;
223  for (ndInt32 i = 0; i < 4; ++i)
224  {
225  array[0][i] = me[i];
226  array[1][i] = A[i];
227  array[2][i] = B[i];
228  array[3][i] = ndFloat32 (1.0f);
229  }
230 
231  ndVector normal;
232  ndFloat32 sign = ndFloat32 (-1.0f);
233  for (ndInt32 i = 0; i < 4; ++i)
234  {
235  for (ndInt32 j = 0; j < 3; ++j)
236  {
237  ndInt32 k0 = 0;
238  for (ndInt32 k = 0; k < 4; ++k)
239  {
240  if (k != i)
241  {
242  cofactor[j][k0] = array[j][k];
243  k0 ++;
244  }
245  }
246  }
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;
251 
252  normal[i] = sign * det;
253  sign *= ndFloat32 (-1.0f);
254  }
255 
256  return normal;
257  }
258 
259  inline ndVector GetInt () const
260  {
261  return ndVector (ndInt32 (ndFloor (m_x)), ndInt32(ndFloor (m_y)), ndInt32(ndFloor (m_z)), ndInt32 (ndFloor (m_w)));
262  }
263 
264  inline ndVector TestZero() const
265  {
266  const ndInt32* const a = (ndInt32*)&m_x;
267  return ndVector (
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));
272  }
273 
274  inline ndVector Floor () const
275  {
276  return ndVector (ndFloor (m_x), ndFloor (m_y), ndFloor (m_z), ndFloor (m_w));
277  }
278 
279  inline ndVector DotProduct (const ndVector &A) const
280  {
281  return ndVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
282  }
283 
284  inline ndVector Reciproc () const
285  {
286  return ndVector (ndFloat32 (1.0f) / m_x, ndFloat32 (1.0f) / m_y, ndFloat32 (1.0f) / m_z, ndFloat32 (1.0f) / m_w);
287  }
288 
289  inline ndVector Sqrt () const
290  {
291  return ndVector (ndSqrt (m_x), ndSqrt (m_y), ndSqrt (m_z), ndSqrt (m_w));
292  }
293 
294  inline ndVector InvSqrt () const
295  {
296  return ndVector (ndRsqrt (m_x), ndRsqrt (m_y), ndRsqrt (m_z), ndRsqrt (m_w));
297  }
298 
299  inline ndVector InvMagSqrt () const
300  {
301  return ndVector (ndRsqrt (DotProduct(*this).m_x));
302  }
303 
304  inline ndVector Normalize () const
305  {
306  const ndVector& me = *this;
307  return me * InvMagSqrt();
308  }
309 
310  inline ndVector Abs () const
311  {
312  return ndVector (
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);
317  }
318 
319  inline ndVector GetMax () const
320  {
321  return ndVector (ndMax(ndMax(m_x, m_y), ndMax(m_z, m_w)));
322  }
323 
324  inline ndVector GetMax (const ndVector& data) const
325  {
326  return ndVector (
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);
331  }
332 
333  inline ndVector GetMin (const ndVector& data) const
334  {
335  return ndVector (
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);
340  }
341 
342 
343  // relational operators
344  inline ndVector operator== (const ndVector& data) const
345  {
346  return ndVector (
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);
351  }
352 
353  inline ndVector operator> (const ndVector& data) const
354  {
355  return ndVector (
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);
360  }
361 
362  inline ndVector operator< (const ndVector& data) const
363  {
364  return ndVector (
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);
369  }
370 
371  inline ndVector operator>= (const ndVector& data) const
372  {
373  return ndVector (
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);
378  }
379 
380  inline ndVector operator<= (const ndVector& data) const
381  {
382  return ndVector (
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);
387  }
388 
389  // logical operations
390  inline ndVector operator& (const ndVector& data) const
391  {
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]);
395  }
396 
397  inline ndVector operator| (const ndVector& data) const
398  {
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]);
402  }
403 
404  inline ndVector operator^ (const ndVector& data) const
405  {
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]);
409  }
410 
411  inline ndVector AndNot (const ndVector& data) const
412  {
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]);
416  }
417 
418  inline ndVector Select (const ndVector& data, const ndVector& mask) const
419  {
420  // (((b ^ a) & mask)^a)
421  return (*this) ^ (mask & (data ^ (*this)));
422  }
423 
424  inline ndInt32 GetSignMask() const
425  {
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));
428  }
429 
430  inline ndVector ShiftRight() const
431  {
432  return ndVector (m_w, m_x, m_y, m_z);
433  }
434 
435  inline ndVector ShiftTripleRight () const
436  {
437  return ndVector (m_z, m_x, m_y, m_w);
438  }
439 
440  inline ndVector ShiftTripleLeft () const
441  {
442  return ndVector (m_y, m_z, m_x, m_w);
443  }
444 
445  inline ndVector ShiftRightLogical (ndInt32 bits) const
446  {
447  return ndVector (ndInt32 (ndUnsigned32 (m_ix) >> bits), ndInt32 (ndUnsigned32 (m_iy) >> bits), ndInt32 (ndUnsigned32 (m_iz) >> bits), ndInt32 (ndUnsigned32 (m_iw) >> bits));
448  }
449 
450  inline static void Transpose4x4 (ndVector& dst0, ndVector& dst1, ndVector& dst2, ndVector& dst3, const ndVector& src0, const ndVector& src1, const ndVector& src2, const ndVector& src3)
451  {
452  ndVector tmp0 (src0);
453  ndVector tmp1 (src1);
454  ndVector tmp2 (src2);
455  ndVector tmp3 (src3);
456 
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);
461  }
462 
463  union
464  {
465  struct
466  {
467  ndFloat32 m_x;
468  ndFloat32 m_y;
469  ndFloat32 m_z;
470  ndFloat32 m_w;
471  };
472  struct
473  {
474  ndInt32 m_ix;
475  ndInt32 m_iy;
476  ndInt32 m_iz;
477  ndInt32 m_iw;
478  };
479  ndInt32 m_i[4];
480  };
481 
482  D_CORE_API static ndVector m_zero;
483  D_CORE_API static ndVector m_one;
484  D_CORE_API static ndVector m_wOne;
485  D_CORE_API static ndVector m_half;
486  D_CORE_API static ndVector m_two;
487  D_CORE_API static ndVector m_three;
488  D_CORE_API static ndVector m_negOne;
489  D_CORE_API static ndVector m_xMask;
490  D_CORE_API static ndVector m_yMask;
491  D_CORE_API static ndVector m_zMask;
492  D_CORE_API static ndVector m_wMask;
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;
498 
499 #endif
500 // *****************************************************************************************
501 //
502 // 4 x 1 double precision SSE2 vector class declaration
503 //
504 // *****************************************************************************************
505 D_MSV_NEWTON_ALIGN_32
506 class ndBigVector
507 {
508  public:
509  D_OPERATOR_NEW_AND_DELETE
510 
511  inline ndBigVector()
512  {
513  }
514 
515  inline ndBigVector(ndFloat64 val)
516  :m_x(val), m_y(val), m_z(val), m_w(val)
517  {
518  }
519 
520  inline ndBigVector (const ndBigVector& v)
521  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
522  {
523  }
524 
525 #ifndef D_NEWTON_USE_DOUBLE
526  inline ndBigVector (const ndVector& v)
527  :m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
528  {
529  }
530 
531  inline ndBigVector (const ndFloat32* const ptr)
532  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ndFloat32 (0.0f))
533  {
534  ndAssert (ndCheckVector ((*this)));
535  }
536 #endif
537 
538  inline ndBigVector (const ndFloat64* const ptr)
539  :m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (ptr[3])
540  {
541  ndAssert (ndCheckVector ((*this)));
542  }
543 
544  inline ndBigVector (ndFloat64 x, ndFloat64 y, ndFloat64 z, ndFloat64 w)
545  :m_x(x), m_y(y), m_z(z), m_w(w)
546  {
547  ndAssert (ndCheckVector ((*this)));
548  }
549 
550  inline ndBigVector (ndInt32 ix, ndInt32 iy, ndInt32 iz, ndInt32 iw)
551  :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
552  {
553  }
554 
555  inline ndBigVector (ndInt64 ix, ndInt64 iy, ndInt64 iz, ndInt64 iw)
556  :m_ix(ix), m_iy(iy), m_iz(iz), m_iw(iw)
557  {
558  }
559 
560  inline ndFloat64 GetScalar () const
561  {
562  return m_x;
563  }
564 
565  inline void Store (ndFloat64* const dst) const
566  {
567  dst[0] = m_x;
568  dst[1] = m_y;
569  dst[2] = m_z;
570  dst[3] = m_w;
571  }
572 
573  inline ndBigVector BroadcastX () const
574  {
575  return ndBigVector (m_x);
576  }
577 
578  inline ndBigVector BroadcastY () const
579  {
580  return ndBigVector (m_y);
581  }
582 
583  inline ndBigVector BroadcastZ () const
584  {
585  return ndBigVector (m_z);
586  }
587 
588  inline ndBigVector BroadcastW () const
589  {
590  return ndBigVector (m_w);
591  }
592 
593 
594  inline ndFloat64& operator[] (ndInt32 i)
595  {
596  ndAssert (i < 4);
597  ndAssert (i >= 0);
598  return (&m_x)[i];
599  }
600 
601  inline const ndFloat64& operator[] (ndInt32 i) const
602  {
603  ndAssert (i < 4);
604  ndAssert (i >= 0);
605  return (&m_x)[i];
606  }
607 
608  inline ndBigVector operator+ (const ndBigVector& A) const
609  {
610  return ndBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
611  }
612 
613  inline ndBigVector operator- (const ndBigVector& A) const
614  {
615  return ndBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
616  }
617 
618  inline ndBigVector operator* (const ndBigVector& A) const
619  {
620  return ndBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
621  }
622 
623  inline ndBigVector& operator+= (const ndBigVector& A)
624  {
625  return (*this = ndBigVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
626  }
627 
628  inline ndBigVector& operator-= (const ndBigVector& A)
629  {
630  return (*this = ndBigVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
631  }
632 
633  inline ndBigVector& operator*= (const ndBigVector& A)
634  {
635  return (*this = ndBigVector(m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w));
636  }
637 
638  inline ndBigVector MulAdd(const ndBigVector& A, const ndBigVector& B) const
639  {
640  return *this + A * B;
641  }
642 
643  inline ndBigVector MulSub(const ndVector& A, const ndBigVector& B) const
644  {
645  return *this - A * B;
646  }
647 
648 
649  inline ndBigVector AddHorizontal () const
650  {
651  return ndBigVector (m_x + m_y + m_z + m_w);
652  }
653 
654  inline ndBigVector Scale (ndFloat64 scale) const
655  {
656  return ndBigVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
657  }
658 
659  // return cross product
660  inline ndBigVector CrossProduct (const ndBigVector& B) const
661  {
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);
663  }
664 
665  inline ndBigVector CrossProduct (const ndBigVector& A, const ndBigVector& B) const
666  {
667  ndFloat64 cofactor[3][3];
668  ndFloat64 array[4][4];
669 
670  const ndBigVector& me = *this;
671  for (ndInt32 i = 0; i < 4; ++i)
672  {
673  array[0][i] = me[i];
674  array[1][i] = A[i];
675  array[2][i] = B[i];
676  array[3][i] = ndFloat32 (1.0f);
677  }
678 
679  ndBigVector normal;
680  ndFloat64 sign = ndFloat64 (-1.0f);
681  for (ndInt32 i = 0; i < 4; ++i)
682  {
683  for (ndInt32 j = 0; j < 3; ++j)
684  {
685  ndInt32 k0 = 0;
686  for (ndInt32 k = 0; k < 4; ++k)
687  {
688  if (k != i)
689  {
690  cofactor[j][k0] = array[j][k];
691  k0 ++;
692  }
693  }
694  }
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;
699 
700  normal[i] = sign * det;
701  sign *= ndFloat64 (-1.0f);
702  }
703 
704  return normal;
705  }
706 
707  inline ndBigVector GetInt () const
708  {
709  return ndBigVector (ndInt64 (floor (m_x)), ndInt64(floor (m_y)), ndInt64(floor (m_z)), ndInt64 (floor (m_w)));
710  }
711 
712  inline ndBigVector TestZero() const
713  {
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));
719  }
720 
721 
722  inline ndBigVector Floor () const
723  {
724  return ndBigVector (floor (m_x), floor (m_y), floor (m_z), floor (m_w));
725  }
726 
727  inline ndBigVector DotProduct (const ndBigVector &A) const
728  {
729  return ndBigVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
730  }
731 
732  inline ndBigVector Reciproc () const
733  {
734  return ndBigVector (ndFloat64 (1.0f) / m_x, ndFloat64 (1.0f) / m_y, ndFloat64 (1.0f) / m_z, ndFloat64 (1.0f) / m_w);
735  }
736 
737  inline ndBigVector Sqrt () const
738  {
739  return ndBigVector (sqrt (m_x), sqrt (m_y), sqrt (m_z), sqrt (m_w));
740  }
741 
742  inline ndBigVector InvSqrt () const
743  {
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));
745  }
746 
747  inline ndBigVector InvMagSqrt () const
748  {
749  return ndBigVector (ndFloat64 (1.0f) / sqrt (DotProduct(*this).m_x));
750  }
751 
752  inline ndBigVector Normalize() const
753  {
754  return *this * InvMagSqrt();
755  }
756 
757  inline ndBigVector Abs () const
758  {
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);
763  }
764 
765  inline ndBigVector GetMax () const
766  {
767  return ndBigVector (ndMax(ndMax(m_x, m_y), ndMax(m_z, m_w)));
768  }
769 
770  inline ndBigVector GetMax (const ndBigVector& data) const
771  {
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);
776  }
777 
778  inline ndBigVector GetMin (const ndBigVector& data) const
779  {
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);
784  }
785 
786  // relational operators
787  inline ndBigVector operator== (const ndBigVector& data) const
788  {
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));
793  }
794 
795  inline ndBigVector operator> (const ndBigVector& data) const
796  {
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));
801  }
802 
803  inline ndBigVector operator< (const ndBigVector& data) const
804  {
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));
809  }
810 
811  inline ndBigVector operator>= (const ndBigVector& data) const
812  {
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));
817  }
818 
819  inline ndBigVector operator<= (const ndBigVector& data) const
820  {
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));
825  }
826 
827  // logical operations
828  inline ndBigVector operator& (const ndBigVector& data) const
829  {
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]);
833  }
834 
835  inline ndBigVector operator| (const ndBigVector& data) const
836  {
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]);
840  }
841 
842  inline ndBigVector operator^ (const ndBigVector& data) const
843  {
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]);
847  }
848 
849  inline ndBigVector AndNot (const ndBigVector& data) const
850  {
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]);
854  }
855 
856  inline ndBigVector Select(const ndBigVector& data, const ndBigVector& mask) const
857  {
858  // (((b ^ a) & mask)^a)
859  return (*this) ^ (mask & (data ^ (*this)));
860  }
861 
862  inline ndInt32 GetSignMask() const
863  {
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));
866  }
867 
868  inline ndVector ShiftRight() const
869  {
870  return ndBigVector (m_w, m_x, m_y, m_z);
871  }
872 
873  inline ndBigVector ShiftTripleRight () const
874  {
875  return ndBigVector (m_z, m_x, m_y, m_w);
876  }
877 
878  inline ndBigVector ShiftTripleLeft () const
879  {
880  return ndBigVector (m_y, m_z, m_x, m_w);
881  }
882 
883  inline ndBigVector ShiftRightLogical (ndInt32 bits) const
884  {
885  return ndBigVector (ndInt64 (ndUnsigned64 (m_ix) >> bits), ndInt64 (ndUnsigned64 (m_iy) >> bits), ndInt64 (ndUnsigned64 (m_iz) >> bits), ndInt64 (ndUnsigned64 (m_iw) >> bits));
886  }
887 
888  inline static void Transpose4x4 (ndBigVector& dst0, ndBigVector& dst1, ndBigVector& dst2, ndBigVector& dst3, const ndBigVector& src0, const ndBigVector& src1, const ndBigVector& src2, const ndBigVector& src3)
889  {
890  ndBigVector tmp0 (src0);
891  ndBigVector tmp1 (src1);
892  ndBigVector tmp2 (src2);
893  ndBigVector tmp3 (src3);
894 
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);
899  }
900 
901  union
902  {
903  ndInt64 m_i[4];
904  struct
905  {
906  ndFloat64 m_x;
907  ndFloat64 m_y;
908  ndFloat64 m_z;
909  ndFloat64 m_w;
910  };
911  struct
912  {
913  ndInt64 m_ix;
914  ndInt64 m_iy;
915  ndInt64 m_iz;
916  ndInt64 m_iw;
917  };
918  };
919 
920  D_CORE_API static ndBigVector m_zero;
921  D_CORE_API static ndBigVector m_one;
922  D_CORE_API static ndBigVector m_wOne;
923  D_CORE_API static ndBigVector m_half;
924  D_CORE_API static ndBigVector m_two;
925  D_CORE_API static ndBigVector m_three;
926  D_CORE_API static ndBigVector m_negOne;
927  D_CORE_API static ndBigVector m_xMask;
928  D_CORE_API static ndBigVector m_yMask;
929  D_CORE_API static ndBigVector m_zMask;
930  D_CORE_API static ndBigVector m_wMask;
931  D_CORE_API static ndBigVector m_xyzwMask;
932  D_CORE_API static ndBigVector m_epsilon;
933  D_CORE_API static ndBigVector m_signMask;
934  D_CORE_API static ndBigVector m_triplexMask;
935 } D_GCC_NEWTON_ALIGN_32;
936 
937 #endif
ndBigVector
Definition: ndVectorArmNeon.h:463
ndVector
Definition: ndVectorArmNeon.h:41