Newton Dynamics  4.00
ndCudaBodyProxy.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_BODY_BUFFER_H__
23 #define __ND_BODY_BUFFER_H__
24 
25 #include <cuda.h>
26 #include <cuda_runtime.h>
27 #include "ndCudaQuat.h"
28 #include "ndCudaVector.h"
29 #include "ndCudaMatrix3x3.h"
30 #include "ndCudaDeviceBuffer.h"
31 
33 {
34  public:
35  inline ndCudaMatrix3x3 __device__ CalculateInvInertiaMatrix(const ndCudaMatrix3x3& matrix) const
36  {
37  const ndCudaVector invIxx(m_invIntertia.GetElement(0));
38  const ndCudaVector invIyy(m_invIntertia.GetElement(1));
39  const ndCudaVector invIzz(m_invIntertia.GetElement(2));
40  return ndCudaMatrix3x3(
41  matrix.m_front.Scale(matrix.m_front.GetElement(0)) * invIxx +
42  matrix.m_up.Scale(matrix.m_up.GetElement(0)) * invIyy +
43  matrix.m_right.Scale(matrix.m_right.GetElement(0)) * invIzz,
44 
45  matrix.m_front.Scale(matrix.m_front.GetElement(1)) * invIxx +
46  matrix.m_up.Scale(matrix.m_up.GetElement(1)) * invIyy +
47  matrix.m_right.Scale(matrix.m_right.GetElement(1)) * invIzz,
48 
49  matrix.m_front.Scale(matrix.m_front.GetElement(2)) * invIxx +
50  matrix.m_up.Scale(matrix.m_up.GetElement(2)) * invIyy +
51  matrix.m_right.Scale(matrix.m_right.GetElement(2)) * invIzz);
52  }
53 
54  inline void __device__ AddDampingAcceleration(const ndCudaMatrix3x3& matrix)
55  {
56  const ndCudaVector omega(matrix.UnrotateVector(m_omega) * m_dampCoef);
57  m_omega = matrix.RotateVector(omega);
58  m_veloc = m_veloc.Scale(m_dampCoef.w);
59  }
60 
61  inline void __device__ IntegrateExternalForce(const ndCudaMatrix3x3& matrix, float timestep)
62  {
63  //if (!m_equilibrium && (m_invMass.m_w > float(0.0f)))
64  {
65  //const ndVector accel(GetForce().Scale(m_invMass.m_w));
66  //const ndVector torque(GetTorque());
67  const ndCudaVector accel(0.0);
68  const ndCudaVector torque(0.0);
69 
70  ndCudaVector localOmega(matrix.UnrotateVector(m_omega));
71  const ndCudaVector localAngularMomentum(localOmega * m_mass);
72  const ndCudaVector angularMomentum(matrix.RotateVector(localAngularMomentum));
73  const ndCudaVector gyroTorque(m_omega.CrossProduct(angularMomentum));
74  const ndCudaVector localTorque(matrix.UnrotateVector(torque - gyroTorque));
75 
76  // and solving for alpha we get the angular acceleration at t + dt
77  // calculate gradient at a full time step
78  // derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
79  const ndCudaVector dw(localOmega.Scale(0.5f * timestep));
80 
81  const ndCudaMatrix3x3 jacobianMatrix(
82  ndCudaVector(m_mass.x, (m_mass.z - m_mass.y) * dw.z, (m_mass.z - m_mass.y) * dw.y, 0.0f),
83  ndCudaVector((m_mass.x - m_mass.z) * dw.z, m_mass.y, (m_mass.x - m_mass.z) * dw.x, 0.0f),
84  ndCudaVector((m_mass.y - m_mass.x) * dw.y, (m_mass.y - m_mass.x) * dw.x, m_mass.z, 0.0f));
85 
86  const ndCudaVector gradientStep (jacobianMatrix.SolveByGaussianElimination(localTorque.Scale(timestep)));
87  localOmega = localOmega + gradientStep;
88  const ndCudaVector alpha(matrix.RotateVector(localTorque * m_invIntertia));
89 
90  //SetAccel(accel);
91  //SetAlpha(alpha);
92  m_veloc = m_veloc + accel.Scale(timestep);
93  m_omega = matrix.RotateVector(localOmega);
94  }
95  //else
96  //{
97  // SetAccel(ndVector::m_zero);
98  // SetAlpha(ndVector::m_zero);
99  //}
100  }
101 
102  inline void __device__ IntegrateVelocity(float timestep)
103  {
104  m_posit = m_posit + m_veloc.Scale(timestep);
105  const float omegaMag2 = m_omega.DotProduct(m_omega);
106 
107  const float tol = (float(0.0125f) * 3.141592f / 180.0f);
108  const float tol2 = tol * tol;
109  if (omegaMag2 > tol2)
110  {
111  // this is correct
112  const float omegaAngle = sqrtf(omegaMag2);
113  const ndCudaVector omegaAxis(m_omega.Scale(float(1.0f) / omegaAngle));
114  const ndCudaQuat rotationStep(omegaAxis, omegaAngle * timestep);
115  const ndCudaQuat rotation(m_rotation * rotationStep);
116  m_rotation = rotation.Normalize();
117  }
118  }
119 
120  //void ProxyToBody(ndBodyKinematic* const body) const
121  //void ProxyToBody(ndBodyKinematic* const) const
122  void ProxyToBody(void* const) const
123  {
124  ndAssert(0);
125  //const ndVector veloc(m_veloc.x, m_veloc.y, m_veloc.z, float(0.0f));
126  //const ndVector omega(m_omega.x, m_omega.y, m_omega.z, float(0.0f));
127  //const ndVector position(m_posit.x, m_posit.y, m_posit.z, float(1.0f));
128  //const ndQuaternion rotation(ndVector(m_rotation.x, m_rotation.y, m_rotation.z, m_rotation.w));
129  //
130  //body->SetOmegaNoSleep(omega);
131  //body->SetVelocityNoSleep(veloc);
132  //body->SetMatrixAndCentreOfMass(rotation, position);
133  }
134 
135  ndCudaQuat m_rotation;
136  ndCudaVector m_posit;
137  ndCudaVector m_veloc;
138  ndCudaVector m_omega;
139 
140  // scene Management data
141  ndCudaQuat m_globalSphapeRotation;
142  ndCudaVector m_globalSphapePosition;
143  ndCudaVector m_minAabb;
144  ndCudaVector m_maxAabb;
145 
146  // constant Data
147  ndCudaVector m_mass;
148  ndCudaVector m_dampCoef;
149  ndCudaVector m_invIntertia;
150  ndCudaVector m_obbSize;
151  ndCudaVector m_obbOrigin;
152  ndCudaVector m_scale;
153  ndCudaVector m_localPosition;
154  ndCudaQuat m_localRotation;
155  ndCudaQuat m_alignRotation;
156 };
157 
158 
159 #endif
ndCudaBodyProxy
Definition: ndCudaBodyProxy.h:33
ndCudaQuat
Definition: ndCudaQuat.h:29
ndCudaMatrix3x3
Definition: ndCudaMatrix3x3.h:29
ndCudaVector
Definition: ndCudaVector.h:31