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