22 #ifndef __ND_BODY_BUFFER_H__
23 #define __ND_BODY_BUFFER_H__
26 #include <cuda_runtime.h>
27 #include <ndNewtonStdafx.h>
31 #include "cuMatrix3x3.h"
32 #include "cuDeviceBuffer.h"
39 const cuVector invIxx(m_invIntertia.GetElement(0));
40 const cuVector invIyy(m_invIntertia.GetElement(1));
41 const cuVector invIzz(m_invIntertia.GetElement(2));
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,
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,
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);
56 inline void __device__ AddDampingAcceleration(
const cuMatrix3x3& matrix)
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);
63 inline void __device__ IntegrateExternalForce(
const cuMatrix3x3& matrix,
float timestep)
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));
81 const cuVector dw(localOmega.Scale(0.5f * timestep));
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));
88 const cuVector gradientStep (jacobianMatrix.SolveByGaussianElimination(localTorque.Scale(timestep)));
89 localOmega = localOmega + gradientStep;
90 const cuVector alpha(matrix.RotateVector(localTorque * m_invIntertia));
94 m_veloc = m_veloc + accel.Scale(timestep);
95 m_omega = matrix.RotateVector(localOmega);
104 inline void __device__ IntegrateVelocity(
float timestep)
106 m_posit = m_posit + m_veloc.Scale(timestep);
107 const float omegaMag2 = m_omega.DotProduct(m_omega);
109 const float tol = (float(0.0125f) * 3.141592f / 180.0f);
110 const float tol2 = tol * tol;
111 if (omegaMag2 > tol2)
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();
142 cuQuat m_globalSphapeRotation;