Newton Dynamics  4.00
ndCudaMatrix3x3.h
1 /* Copyright (c) <2003-2021> <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_CUDA_MATRIX3x3_H__
23 #define __ND_CUDA_MATRIX3x3_H__
24 
25 #include "ndCudaVector.h"
26 #include "ndCudaIntrinsics.h"
27 
29 {
30  public:
31  inline __device__ __host__ ndCudaMatrix3x3()
32  {
33  }
34 
35  inline __device__ __host__ ndCudaMatrix3x3(const ndCudaMatrix3x3& src)
36  :m_front(src.m_front)
37  ,m_up(src.m_up)
38  ,m_right(src.m_right)
39  {
40  }
41 
42  inline __device__ __host__ ndCudaMatrix3x3(const ndCudaVector& front, const ndCudaVector& up, const ndCudaVector& right)
43  :m_front(front)
44  ,m_up(up)
45  ,m_right(right)
46  {
47  }
48 
49  inline ndCudaMatrix3x3 __device__ __host__ operator* (const ndCudaMatrix3x3& A) const
50  {
51  return ndCudaMatrix3x3(A.RotateVector(m_front), A.RotateVector(m_up), A.RotateVector(m_right));
52  }
53 
54  inline ndCudaVector __device__ __host__ UnrotateVector(const ndCudaVector &v) const
55  {
56  return ndCudaVector((m_front * v).AddHorizontal(), (m_up * v).AddHorizontal(), (m_right * v).AddHorizontal(), 0.0f);
57  }
58 
59  inline ndCudaVector __device__ __host__ RotateVector(const ndCudaVector& v) const
60  {
61  return m_front.Scale(v.x) + m_up.Scale(v.y) + m_right.Scale(v.z);
62  }
63 
64  inline ndCudaVector __device__ __host__ SolveByGaussianElimination(const ndCudaVector &v) const
65  {
66  ndCudaMatrix3x3 tmp(*this);
67  ndCudaVector ret(v);
68  for (int i = 0; i < 3; ++i)
69  {
70  float pivot = cuAbs(tmp.m_data[i].GetElement(i));
71  if (pivot < float(0.01f))
72  {
73  int permute = i;
74  for (int j = i + 1; j < 3; ++j)
75  {
76  float pivot1 = cuAbs(tmp.m_data[j].GetElement(i));
77  if (pivot1 > pivot)
78  {
79  permute = j;
80  pivot = pivot1;
81  }
82  }
83 
84  if (permute != i)
85  {
86  float a0 = ret.GetElement(i);
87  ret.SetElement(i, ret.GetElement(permute));
88  ret.SetElement(permute, a0);
89  cuSwap(tmp.m_data[i], tmp.m_data[permute]);
90  }
91  }
92 
93  for (int j = i + 1; j < 3; ++j)
94  {
95  const ndCudaVector scale(tmp.m_data[j].GetElement(i) / tmp.m_data[i].GetElement(i));
96  tmp.m_data[j] = tmp.m_data[j] - tmp.m_data[i] * scale;
97  ret.SetElement(j, ret.GetElement(j) - ret.GetElement(i) * scale.x);
98  tmp.m_data[j].SetElement(i, 0.0f);
99  }
100  }
101 
102  for (int i = 2; i >= 0; --i)
103  {
104  const ndCudaVector pivot(tmp.m_data[i] * ret);
105  ret.SetElement(i, (ret.GetElement(i) - pivot.AddHorizontal() + tmp.m_data[i].GetElement(i) * ret.GetElement(i)) / tmp.m_data[i].GetElement(i));
106  }
107 
108  return ret;
109  }
110 
111  union
112  {
113  struct
114  {
115  ndCudaVector m_front;
116  ndCudaVector m_up;
117  ndCudaVector m_right;
118  };
119  ndCudaVector m_data[3];
120  };
121 };
122 
123 #endif
ndCudaMatrix3x3
Definition: ndCudaMatrix3x3.h:29
ndCudaVector
Definition: ndCudaVector.h:31