22 #ifndef __ND_JOINT_BILATERAL_CONSTRAINT_H__
23 #define __ND_JOINT_BILATERAL_CONSTRAINT_H__
25 #include "ndCollisionStdafx.h"
26 #include "ndConstraint.h"
27 #include "ndBodyKinematic.h"
29 #define ND_BILATERAL_CONTRAINT_DOF 12
31 enum ndJointBilateralSolverModel
34 m_jointkinematicOpenLoop,
35 m_jointkinematicCloseLoop,
36 m_jointkinematicAttachment,
40 #define D_ADD_IK_INTERFACE() \
41 virtual void SetIkMode(bool mode) \
45 virtual void SetIkSetAccel(const ndJacobian& body0Accel, const ndJacobian& body1Accel) \
47 m_accel0 = body0Accel; \
48 m_accel1 = body1Accel; \
55 class ndJointList :
public ndList<ndSharedPtr<ndJointBilateralConstraint>, ndContainersFreeListAlloc<ndSharedPtr<ndJointBilateralConstraint>*>>
68 :m_defualRegularizer(ndFloat32 (1.0e-3f))
74 ndFloat32 m_defualRegularizer;
88 virtual ndUnsigned32 GetRowsCount()
const;
91 virtual ndJointBilateralSolverModel GetSolverModel()
const;
92 virtual void SetSolverModel(ndJointBilateralSolverModel model);
94 D_COLLISION_API ndFloat32 CalculateAngle(
const ndVector& planeDir,
const ndVector& cosDir,
const ndVector& sinDir)
const;
96 D_COLLISION_API
void CalculateLocalMatrix(
const ndMatrix& pinsAndPivotFrame,
ndMatrix& localMatrix0,
ndMatrix& localMatrix1)
const;
102 D_COLLISION_API ndFloat32 CalculateSpringDamperAcceleration(ndFloat32 dt, ndFloat32 ks, ndFloat32 x, ndFloat32 kd, ndFloat32 v)
const;
103 D_COLLISION_API
void SetMassSpringDamperAcceleration(
ndConstraintDescritor& desc, ndFloat32 regularizer, ndFloat32 spring, ndFloat32 damper);
105 const ndMatrix& GetLocalMatrix0()
const;
106 const ndMatrix& GetLocalMatrix1()
const;
113 bool IsInWorld()
const;
114 bool IsBilateral()
const;
115 bool IsCollidable()
const;
116 void SetCollidable(
bool state);
117 bool GetSkeletonFlag()
const;
118 void SetSkeletonFlag(
bool flag);
127 bool IsSkeleton()
const;
131 D_COLLISION_API
virtual void SetIkMode(
bool mode);
132 D_COLLISION_API
virtual void SetIkSetAccel(
const ndJacobian& body0Accel,
const ndJacobian& body1Accel);
141 ndVector m_r0[ND_BILATERAL_CONTRAINT_DOF];
142 ndVector m_r1[ND_BILATERAL_CONTRAINT_DOF];
144 ndFloat32 m_motorAcceleration[ND_BILATERAL_CONTRAINT_DOF];
147 ndJointList::ndNode* m_worldNode;
148 ndBodyKinematic::ndJointList::ndNode* m_body0Node;
149 ndBodyKinematic::ndJointList::ndNode* m_body1Node;
151 ndFloat32 m_defualtDiagonalRegularizer;
152 ndUnsigned32 m_maxDof : 6;
153 ndUnsigned32 m_mark0 : 1;
154 ndUnsigned32 m_mark1 : 1;
155 ndUnsigned32 m_isInSkeleton : 1;
156 ndUnsigned32 m_enableCollision : 1;
158 ndInt8 m_markedForRemoved;
159 ndJointBilateralSolverModel m_solverModel;
171 inline ndJointBilateralSolverModel ndJointBilateralConstraint::GetSolverModel()
const
173 return m_solverModel;
176 inline void ndJointBilateralConstraint::SetSolverModel(ndJointBilateralSolverModel model)
178 ndAssert(model < m_jointModesCount);
179 ndAssert(model >= m_jointIterativeSoft);
180 m_solverModel = ndClamp(model, m_jointIterativeSoft, m_jointModesCount);
183 inline ndUnsigned32 ndJointBilateralConstraint::GetRowsCount()
const
188 inline void ndJointBilateralConstraint::CalculateGlobalMatrix(
ndMatrix& matrix0,
ndMatrix& matrix1)
const
190 matrix0 = m_localMatrix0 * m_body0->GetMatrix();
191 matrix1 = m_localMatrix1 * m_body1->GetMatrix();
204 inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix0()
const
206 return m_localMatrix0;
209 inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix1()
const
211 return m_localMatrix1;
214 inline ndFloat32 ndJointBilateralConstraint::GetMotorZeroAcceleration(
ndConstraintDescritor& desc)
const
216 const ndInt32 index = desc.m_rowsCount - 1;
217 ndAssert(index >= 0);
218 ndAssert(index < ndInt32(m_maxDof));
219 return desc.m_zeroRowAcceleration[index];
222 inline void ndJointBilateralConstraint::SetMotorAcceleration(
ndConstraintDescritor& desc, ndFloat32 acceleration)
224 const ndInt32 index = desc.m_rowsCount - 1;
225 ndAssert(index >= 0);
226 ndAssert(index < ndInt32(m_maxDof));
227 m_rowIsMotor |= (1 << index);
228 desc.m_flags[index] = 0;
229 m_motorAcceleration[index] = acceleration;
230 desc.m_jointAccel[index] = acceleration;
233 inline void ndJointBilateralConstraint::SetLowerFriction(
ndConstraintDescritor& desc, ndFloat32 friction)
235 const ndInt32 index = desc.m_rowsCount - 1;
236 ndAssert(index >= 0);
237 ndAssert(index < ndInt32 (m_maxDof));
238 desc.m_forceBounds[index].m_low = ndClamp(friction, ndFloat32(D_MIN_BOUND), ndFloat32(-0.001f));
239 ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW);
243 ndInt32 i1 = index - 1;
244 while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++;
245 while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--;
246 ndAssert((i0 - i1) == 1);
249 ndTrace((
"make sure that friction joint are issue at last\n"));
254 inline void ndJointBilateralConstraint::SetHighFriction(
ndConstraintDescritor& desc, ndFloat32 friction)
256 const ndInt32 index = desc.m_rowsCount - 1;
257 ndAssert(index >= 0);
258 ndAssert(index < ndInt32(m_maxDof));
260 desc.m_forceBounds[index].m_upper = ndClamp(friction, ndFloat32(0.001f), ndFloat32(D_MAX_BOUND));
261 ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW);
265 ndInt32 i1 = index - 1;
266 while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++;
267 while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--;
268 ndAssert((i0 - i1) == 1);
271 ndTrace((
"make sure that friction joint are issue at last\n"));
279 ndTrace((
"error: this joint is an interface\n"));
282 inline bool ndJointBilateralConstraint::GetSkeletonFlag()
const
284 return m_isInSkeleton ? true :
false;
287 inline void ndJointBilateralConstraint::SetSkeletonFlag(
bool flag)
289 m_isInSkeleton = ndUnsigned32 (flag ? 1 : 0);
292 inline bool ndJointBilateralConstraint::IsCollidable()
const
294 return m_enableCollision ? true :
false;
297 inline bool ndJointBilateralConstraint::IsBilateral()
const
302 inline void ndJointBilateralConstraint::SetCollidable(
bool state)
304 m_enableCollision = ndUnsigned32 (state ? 1 : 0);
307 inline ndVector ndJointBilateralConstraint::GetForceBody0()
const
312 inline ndVector ndJointBilateralConstraint::GetTorqueBody0()
const
314 return m_torqueBody0;
317 inline ndVector ndJointBilateralConstraint::GetForceBody1()
const
322 inline ndVector ndJointBilateralConstraint::GetTorqueBody1()
const
324 return m_torqueBody1;
327 inline ndFloat32 ndJointBilateralConstraint::GetDiagonalRegularizer(
const ndConstraintDescritor& desc)
const
329 const ndInt32 index = desc.m_rowsCount - 1;
330 ndAssert(index >= 0);
331 ndAssert(index < ndInt32(m_maxDof));
332 return desc.m_diagonalRegularizer[index];
335 inline void ndJointBilateralConstraint::SetDiagonalRegularizer(
ndConstraintDescritor& desc, ndFloat32 regularizer)
337 const ndInt32 index = desc.m_rowsCount - 1;
338 ndAssert(index >= 0);
339 ndAssert(index < ndInt32(m_maxDof));
340 desc.m_diagonalRegularizer[index] = ndClamp(regularizer, ndFloat32(0.0f), ndFloat32(1.0f));
343 inline bool ndJointBilateralConstraint::IsInWorld()
const
345 return m_worldNode ? true :
false;
348 inline bool ndJointBilateralConstraint::IsSkeleton()
const
350 const ndJointBilateralSolverModel mode = GetSolverModel();
352 test = test || (mode == m_jointkinematicOpenLoop);
353 test = test || (mode == m_jointkinematicCloseLoop);
358 inline void ndJointBilateralConstraint::ReplaceSentinel(
ndBodyKinematic*
const sentinel)