22 #ifndef __ND_BODY_KINEMATIC_H__
23 #define __ND_BODY_KINEMATIC_H__
25 #include "ndCollisionStdafx.h"
27 #include "ndConstraint.h"
28 #include "ndBodyListView.h"
35 #define D_FREEZZING_VELOCITY_DRAG ndFloat32 (0.9f)
36 #define D_SOLVER_MAX_ERROR (D_FREEZE_MAG * ndFloat32 (0.5f))
44 ndContactkey(ndUnsigned32 tag0, ndUnsigned32 tag1);
46 bool operator> (
const ndContactkey& key)
const;
47 bool operator< (
const ndContactkey& key)
const;
48 bool operator== (
const ndContactkey& key)
const;
55 ndUnsigned32 m_tagLow;
56 ndUnsigned32 m_tagHigh;
62 class ndJointList :
public ndList<ndJointBilateralConstraint*, ndContainersFreeListAlloc<ndJointBilateralConstraint*>>
71 class ndContactMap:
public ndTree<ndContact*, ndContactkey, ndContainersFreeListAlloc<ndContact*>>
79 void AttachContact(
ndContact*
const contact);
80 void DetachContact(
ndContact*
const contact);
91 ndUnsigned32 GetIndex()
const;
92 ndFloat32 GetInvMass()
const;
93 const ndVector GetInvInertia()
const;
94 const ndVector& GetMassMatrix()
const;
95 const ndMatrix& GetInvInertiaMatrix()
const;
100 bool GetSleepState()
const;
101 void RestoreSleepState(
bool state);
102 D_COLLISION_API
void SetSleepState(
bool state);
104 bool GetAutoSleep()
const;
105 void SetAutoSleep(
bool state);
106 ndFloat32 GetMaxLinearStep()
const;
107 ndFloat32 GetMaxAngularStep()
const;
108 void SetDebugMaxLinearAndAngularIntegrationStep(ndFloat32 angleInRadian, ndFloat32 stepInUnitPerSeconds);
110 virtual ndFloat32 GetLinearDamping()
const;
111 virtual void SetLinearDamping(ndFloat32 linearDamp);
113 virtual ndVector GetCachedDamping()
const;
114 virtual ndVector GetAngularDamping()
const;
115 virtual void SetAngularDamping(
const ndVector& angularDamp);
119 D_COLLISION_API
virtual void SetCollisionShape(
const ndShapeInstance& shapeInstance);
122 D_COLLISION_API
ndVector CalculateLinearMomentum()
const;
123 D_COLLISION_API
virtual ndVector CalculateAngularMomentum()
const;
124 D_COLLISION_API ndFloat32 TotalEnergy()
const;
126 D_COLLISION_API
ndMatrix CalculateInertiaMatrix()
const;
127 D_COLLISION_API
virtual ndMatrix CalculateInvInertiaMatrix()
const;
129 D_COLLISION_API
virtual void IntegrateVelocity(ndFloat32 timestep);
132 void UpdateInvInertiaMatrix();
133 void SetMassMatrix(
const ndVector& massMatrix);
134 void SetMassMatrix(ndFloat32 mass,
const ndShapeInstance& shapeInstance);
135 void SetMassMatrix(ndFloat32 Ixx, ndFloat32 Iyy, ndFloat32 Izz, ndFloat32 mass);
136 D_COLLISION_API
virtual void SetMassMatrix(ndFloat32 mass,
const ndMatrix& inertia);
138 void GetMassMatrix(ndFloat32& Ixx, ndFloat32& Iyy, ndFloat32& Izz, ndFloat32& mass);
140 D_COLLISION_API
void SetMatrixUpdateScene(
const ndMatrix& matrix);
141 D_COLLISION_API
virtual ndContact* FindContact(
const ndBody*
const otherBody)
const;
151 virtual void SetForce(
const ndVector& force);
152 virtual void SetTorque(
const ndVector& torque);
154 virtual void AddImpulse(
const ndVector& pointVeloc,
const ndVector& pointPosit, ndFloat32 timestep);
155 virtual void ApplyImpulsePair(
const ndVector& linearImpulse,
const ndVector& angularImpulse, ndFloat32 timestep);
156 virtual void ApplyImpulsesAtPoint(ndInt32 count,
const ndVector*
const impulseArray,
const ndVector*
const pointArray, ndFloat32 timestep);
160 void SetAccel(
const ndVector& accel);
161 void SetAlpha(
const ndVector& alpha);
168 D_COLLISION_API
virtual void AttachContact(
ndContact*
const contact);
169 D_COLLISION_API
virtual void DetachContact(
ndContact*
const contact);
171 D_COLLISION_API
virtual void DetachJoint(ndJointList::ndNode*
const node);
174 D_COLLISION_API
virtual void IntegrateExternalForce(ndFloat32 timestep);
177 virtual void SpecialUpdate(ndFloat32 timestep);
178 virtual void IntegrateGyroSubstep(
const ndVector& timestep);
179 virtual void ApplyExternalForces(ndInt32 threadIndex, ndFloat32 timestep);
182 void UpdateCollisionMatrix();
183 void PrepareStep(ndInt32 index);
184 void SetSceneNodes(
ndScene*
const scene, ndBodyListView::ndNode*
const node);
186 virtual void AddDampingAcceleration(ndFloat32 timestep);
188 D_COLLISION_API
virtual void EvaluateSleepState(ndFloat32 freezeSpeed2, ndFloat32 freezeAccel2);
204 ndBodyListView::ndNode* m_sceneNode;
208 ndFloat32 m_maxAngleStep;
209 ndFloat32 m_maxLinearStep;
212 ndInt32 m_bodyNodeIndex;
213 ndInt32 m_buildSkelIndex;
214 ndInt32 m_sceneNodeIndex;
215 ndInt32 m_buildBodyNodeIndex;
216 ndInt32 m_buildSceneNodeIndex;
218 D_COLLISION_API
static ndVector m_velocTol;
234 } D_GCC_NEWTON_ALIGN_32;
242 inline ndUnsigned32 ndBodyKinematic::GetIndex()
const
244 return ndUnsigned32(m_index);
247 inline ndFloat32 ndBodyKinematic::GetInvMass()
const
249 return m_invMass.m_w;
252 inline const ndVector ndBodyKinematic::GetInvInertia()
const
254 return m_invMass & ndVector::m_triplexMask;
257 inline const ndVector& ndBodyKinematic::GetMassMatrix()
const
262 inline const ndMatrix& ndBodyKinematic::GetInvInertiaMatrix()
const
264 return m_invWorldInertiaMatrix;
267 inline ndVector ndBodyKinematic::GetGyroAlpha()
const
272 inline ndVector ndBodyKinematic::GetGyroTorque()
const
277 inline void ndBodyKinematic::GetMassMatrix(ndFloat32& Ixx, ndFloat32& Iyy, ndFloat32& Izz, ndFloat32& mass)
285 inline void ndBodyKinematic::SetMassMatrix(
const ndVector& massMatrix)
287 ndMatrix inertia(ndGetZeroMatrix());
288 inertia[0][0] = massMatrix.m_x;
289 inertia[1][1] = massMatrix.m_y;
290 inertia[2][2] = massMatrix.m_z;
291 SetMassMatrix(massMatrix.m_w, inertia);
294 inline void ndBodyKinematic::SetMassMatrix(ndFloat32 Ixx, ndFloat32 Iyy, ndFloat32 Izz, ndFloat32 mass)
296 SetMassMatrix(
ndVector(Ixx, Iyy, Izz, mass));
299 inline void ndBodyKinematic::SetMassMatrix(ndFloat32 mass,
const ndShapeInstance& shapeInstance)
301 ndMatrix inertia(shapeInstance.CalculateInertia());
304 for (ndInt32 i = 0; i < 3; ++i)
306 inertia[i] = inertia[i].Scale(mass);
316 SetCentreOfMass(origin);
317 SetMassMatrix(mass, inertia);
325 inline ndScene* ndBodyKinematic::GetScene()
const
330 inline void ndBodyKinematic::SetSceneNodes(
ndScene*
const scene, ndBodyListView::ndNode*
const node)
336 inline ndVector ndBodyKinematic::GetForce()
const
338 return ndVector::m_zero;
341 inline ndVector ndBodyKinematic::GetTorque()
const
343 return ndVector::m_zero;
346 inline void ndBodyKinematic::SetForce(
const ndVector&)
350 inline void ndBodyKinematic::SetTorque(
const ndVector&)
354 inline ndVector ndBodyKinematic::GetAccel()
const
359 inline void ndBodyKinematic::SetAccel(
const ndVector& accel)
364 inline ndVector ndBodyKinematic::GetAlpha()
const
369 inline void ndBodyKinematic::SetAlpha(
const ndVector& alpha)
374 inline void ndBodyKinematic::AddDampingAcceleration(ndFloat32)
378 inline void ndBodyKinematic::SetAccel(
const ndJacobian& accel)
380 SetAccel(accel.m_linear);
381 SetAlpha(accel.m_angular);
384 inline void ndBodyKinematic::PrepareStep(ndInt32 index)
390 m_buildSkelIndex = 0;
391 m_islandParent =
this;
392 m_weigh = ndFloat32(0.0f);
393 m_isStatic = ndUnsigned8(m_invMass.m_w == ndFloat32(0.0f));
394 m_equilibrium = ndUnsigned8 (m_isStatic | m_equilibrium);
395 m_equilibrium0 = m_equilibrium;
400 return m_contactList;
405 return m_contactList;
418 inline const ndShapeInstance& ndBodyKinematic::GetCollisionShape()
const
420 return m_shapeInstance;
423 inline bool ndBodyKinematic::GetAutoSleep()
const
425 return m_autoSleep ? true :
false;
428 inline bool ndBodyKinematic::GetSleepState()
const
430 return m_equilibrium ? true :
false;
433 inline void ndBodyKinematic::RestoreSleepState(
bool state)
435 m_equilibrium = ndUnsigned8 (state ? 1 : 0);
438 inline void ndBodyKinematic::SetAutoSleep(
bool state)
440 m_autoSleep = ndUnsigned8 (state ? 1 : 0);
441 SetSleepState(
false);
446 return m_skeletonContainer;
451 m_skeletonContainer = skeleton;
454 inline ndFloat32 ndBodyKinematic::GetMaxLinearStep()
const
456 return m_maxLinearStep;
459 inline ndFloat32 ndBodyKinematic::GetMaxAngularStep()
const
461 return m_maxAngleStep;
464 inline void ndBodyKinematic::SetDebugMaxLinearAndAngularIntegrationStep(ndFloat32 angleInRadian, ndFloat32 stepInUnitPerSeconds)
466 m_maxLinearStep = ndMax(ndAbs(stepInUnitPerSeconds), ndFloat32(1.0f));
467 m_maxAngleStep = ndMax(ndAbs(angleInRadian), ndFloat32(90.0f) * ndDegreeToRad);
470 inline void ndBodyKinematic::SetLinearDamping(ndFloat32)
474 inline ndFloat32 ndBodyKinematic::GetLinearDamping()
const
476 return ndFloat32(0.0f);
479 inline void ndBodyKinematic::SetAngularDamping(
const ndVector&)
483 inline ndVector ndBodyKinematic::GetAngularDamping()
const
485 return ndVector::m_zero;
488 inline ndVector ndBodyKinematic::GetCachedDamping()
const
490 return ndVector::m_one;
493 inline void ndBodyKinematic::UpdateInvInertiaMatrix()
495 ndAssert(m_invWorldInertiaMatrix[0][3] == ndFloat32(0.0f));
496 ndAssert(m_invWorldInertiaMatrix[1][3] == ndFloat32(0.0f));
497 ndAssert(m_invWorldInertiaMatrix[2][3] == ndFloat32(0.0f));
498 ndAssert(m_invWorldInertiaMatrix[3][3] == ndFloat32(1.0f));
500 m_invWorldInertiaMatrix = CalculateInvInertiaMatrix();
502 ndAssert(m_invWorldInertiaMatrix[0][3] == ndFloat32(0.0f));
503 ndAssert(m_invWorldInertiaMatrix[1][3] == ndFloat32(0.0f));
504 ndAssert(m_invWorldInertiaMatrix[2][3] == ndFloat32(0.0f));
505 ndAssert(m_invWorldInertiaMatrix[3][3] == ndFloat32(1.0f));
508 inline void ndBodyKinematic::IntegrateGyroSubstep(
const ndVector&)
515 step.m_linear = ndVector::m_zero;
516 step.m_angular = ndVector::m_zero;
520 inline void ndBodyKinematic::AddImpulse(
const ndVector&,
const ndVector&, ndFloat32)
524 inline void ndBodyKinematic::ApplyImpulsePair(
const ndVector&,
const ndVector&, ndFloat32)
528 inline void ndBodyKinematic::ApplyImpulsesAtPoint(ndInt32,
const ndVector*
const,
const ndVector*
const, ndFloat32)
532 inline void ndBodyKinematic::SpecialUpdate(ndFloat32)
537 inline void ndBodyKinematic::ApplyExternalForces(ndInt32, ndFloat32)