22 #ifndef __ND_BODY_BUFFER_H__
23 #define __ND_BODY_BUFFER_H__
26 #include <cuda_runtime.h>
27 #include "ndCudaQuat.h"
28 #include "ndCudaVector.h"
29 #include "ndCudaMatrix3x3.h"
30 #include "ndCudaDeviceBuffer.h"
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,
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,
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);
54 inline void __device__ AddDampingAcceleration(
const ndCudaMatrix3x3& matrix)
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);
61 inline void __device__ IntegrateExternalForce(
const ndCudaMatrix3x3& matrix,
float timestep)
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));
79 const ndCudaVector dw(localOmega.Scale(0.5f * timestep));
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));
86 const ndCudaVector gradientStep (jacobianMatrix.SolveByGaussianElimination(localTorque.Scale(timestep)));
87 localOmega = localOmega + gradientStep;
88 const ndCudaVector alpha(matrix.RotateVector(localTorque * m_invIntertia));
92 m_veloc = m_veloc + accel.Scale(timestep);
93 m_omega = matrix.RotateVector(localOmega);
102 inline void __device__ IntegrateVelocity(
float timestep)
104 m_posit = m_posit + m_veloc.Scale(timestep);
105 const float omegaMag2 = m_omega.DotProduct(m_omega);
107 const float tol = (float(0.0125f) * 3.141592f / 180.0f);
108 const float tol2 = tol * tol;
109 if (omegaMag2 > tol2)
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();
122 void ProxyToBody(
void*
const)
const