Newton Dynamics  4.00
ndJointBilateralConstraint.h
1 /* Copyright (c) <2003-2022> <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_JOINT_BILATERAL_CONSTRAINT_H__
23 #define __ND_JOINT_BILATERAL_CONSTRAINT_H__
24 
25 #include "ndCollisionStdafx.h"
26 #include "ndConstraint.h"
27 #include "ndBodyKinematic.h"
28 
29 #define ND_BILATERAL_CONTRAINT_DOF 12
30 
31 enum ndJointBilateralSolverModel
32 {
33  m_jointIterativeSoft,
34  m_jointkinematicOpenLoop,
35  m_jointkinematicCloseLoop,
36  m_jointkinematicAttachment,
37  m_jointModesCount
38 };
39 
40 #define D_ADD_IK_INTERFACE() \
41 virtual void SetIkMode(bool mode) \
42 { \
43  m_ikMode = mode; \
44 } \
45 virtual void SetIkSetAccel(const ndJacobian& body0Accel, const ndJacobian& body1Accel) \
46 { \
47  m_accel0 = body0Accel; \
48  m_accel1 = body1Accel; \
49 }
50 
51 D_MSV_NEWTON_ALIGN_32
53 {
54  public:
55  class ndJointList : public ndList<ndSharedPtr<ndJointBilateralConstraint>, ndContainersFreeListAlloc<ndSharedPtr<ndJointBilateralConstraint>*>>
56  {
57  public:
58  ndJointList()
60  {
61  }
62  };
63 
65  {
66  public:
68  :m_defualRegularizer(ndFloat32 (1.0e-3f))
69  ,m_ikMode(true)
70  {}
71 
72  ndJacobian m_accel0;
73  ndJacobian m_accel1;
74  ndFloat32 m_defualRegularizer;
75  bool m_ikMode;
76  };
77 
78  D_CLASS_REFLECTION(ndJointBilateralConstraint);
80  D_COLLISION_API ndJointBilateralConstraint(ndInt32 maxDof, ndBodyKinematic* const body0, ndBodyKinematic* const body1, const ndMatrix& globalMatrix);
81  D_COLLISION_API ndJointBilateralConstraint(ndInt32 maxDof, ndBodyKinematic* const body0, ndBodyKinematic* const body1, const ndMatrix& globalMatrixBody0, const ndMatrix& globalMatrixBody1);
82  D_COLLISION_API virtual ~ndJointBilateralConstraint();
83 
84  virtual ndBodyKinematic* GetBody0() const;
85  virtual ndBodyKinematic* GetBody1() const;
86  void ReplaceSentinel(ndBodyKinematic* const sentinel);
87 
88  virtual ndUnsigned32 GetRowsCount() const;
89  virtual ndJointBilateralConstraint* GetAsBilateral() { return this; }
90  virtual void JacobianDerivative(ndConstraintDescritor& desc);
91  virtual ndJointBilateralSolverModel GetSolverModel() const;
92  virtual void SetSolverModel(ndJointBilateralSolverModel model);
93 
94  D_COLLISION_API ndFloat32 CalculateAngle(const ndVector& planeDir, const ndVector& cosDir, const ndVector& sinDir) const;
95  D_COLLISION_API virtual void JointAccelerations(ndJointAccelerationDecriptor* const desc);
96  D_COLLISION_API void CalculateLocalMatrix(const ndMatrix& pinsAndPivotFrame, ndMatrix& localMatrix0, ndMatrix& localMatrix1) const;
97  D_COLLISION_API void AddAngularRowJacobian(ndConstraintDescritor& desc, const ndVector& dir, ndFloat32 relAngle);
98  D_COLLISION_API void AddLinearRowJacobian(ndConstraintDescritor& desc, const ndVector& pivot0, const ndVector& pivot1, const ndVector& dir);
99 
100  D_COLLISION_API virtual void Save(const ndLoadSaveBase::ndSaveDescriptor& desc) const;
101  D_COLLISION_API virtual void DebugJoint(ndConstraintDebugCallback& debugCallback) 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);
104 
105  const ndMatrix& GetLocalMatrix0() const;
106  const ndMatrix& GetLocalMatrix1() const;
107 
108  ndVector GetForceBody0() const;
109  ndVector GetTorqueBody0() const;
110  ndVector GetForceBody1() const;
111  ndVector GetTorqueBody1() const;
112 
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);
119  void CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const;
120  ndFloat32 GetMotorZeroAcceleration(ndConstraintDescritor& desc) const;
121  void SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction);
122  void SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction);
123  void SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration);
124  ndFloat32 GetDiagonalRegularizer(const ndConstraintDescritor& desc) const;
125  void SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer);
126 
127  bool IsSkeleton() const;
128 
129  protected:
130  // inverse dynamics interface
131  D_COLLISION_API virtual void SetIkMode(bool mode);
132  D_COLLISION_API virtual void SetIkSetAccel(const ndJacobian& body0Accel, const ndJacobian& body1Accel);
133 
134  ndMatrix m_localMatrix0;
135  ndMatrix m_localMatrix1;
136  ndVector m_forceBody0;
137  ndVector m_torqueBody0;
138  ndVector m_forceBody1;
139  ndVector m_torqueBody1;
140 
141  ndVector m_r0[ND_BILATERAL_CONTRAINT_DOF];
142  ndVector m_r1[ND_BILATERAL_CONTRAINT_DOF];
143  ndForceImpactPair m_jointForce[ND_BILATERAL_CONTRAINT_DOF];
144  ndFloat32 m_motorAcceleration[ND_BILATERAL_CONTRAINT_DOF];
145  ndBodyKinematic* m_body0;
146  ndBodyKinematic* m_body1;
147  ndJointList::ndNode* m_worldNode;
148  ndBodyKinematic::ndJointList::ndNode* m_body0Node;
149  ndBodyKinematic::ndJointList::ndNode* m_body1Node;
150 
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;
157  ndInt8 m_rowIsMotor;
158  ndInt8 m_markedForRemoved;
159  ndJointBilateralSolverModel m_solverModel;
160 
161  friend class ndWorld;
162  friend class ndIkSolver;
163  friend class ndDynamicsUpdate;
164  friend class ndSkeletonContainer;
165  friend class ndDynamicsUpdateSoa;
166  friend class ndDynamicsUpdateAvx2;
167  friend class ndDynamicsUpdateCuda;
168  friend class ndDynamicsUpdateOpencl;
169 };
170 
171 inline ndJointBilateralSolverModel ndJointBilateralConstraint::GetSolverModel() const
172 {
173  return m_solverModel;
174 }
175 
176 inline void ndJointBilateralConstraint::SetSolverModel(ndJointBilateralSolverModel model)
177 {
178  ndAssert(model < m_jointModesCount);
179  ndAssert(model >= m_jointIterativeSoft);
180  m_solverModel = ndClamp(model, m_jointIterativeSoft, m_jointModesCount);
181 }
182 
183 inline ndUnsigned32 ndJointBilateralConstraint::GetRowsCount() const
184 {
185  return m_maxDof;
186 }
187 
188 inline void ndJointBilateralConstraint::CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const
189 {
190  matrix0 = m_localMatrix0 * m_body0->GetMatrix();
191  matrix1 = m_localMatrix1 * m_body1->GetMatrix();
192 }
193 
194 inline ndBodyKinematic* ndJointBilateralConstraint::GetBody0() const
195 {
196  return m_body0;
197 }
198 
199 inline ndBodyKinematic* ndJointBilateralConstraint::GetBody1() const
200 {
201  return m_body1;
202 }
203 
204 inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix0() const
205 {
206  return m_localMatrix0;
207 }
208 
209 inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix1() const
210 {
211  return m_localMatrix1;
212 }
213 
214 inline ndFloat32 ndJointBilateralConstraint::GetMotorZeroAcceleration(ndConstraintDescritor& desc) const
215 {
216  const ndInt32 index = desc.m_rowsCount - 1;
217  ndAssert(index >= 0);
218  ndAssert(index < ndInt32(m_maxDof));
219  return desc.m_zeroRowAcceleration[index];
220 }
221 
222 inline void ndJointBilateralConstraint::SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration)
223 {
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;
231 }
232 
233 inline void ndJointBilateralConstraint::SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction)
234 {
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);
240 
241  #ifdef _DEBUG
242  ndInt32 i0 = 0;
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);
247  if ((i0 - i1) != 1)
248  {
249  ndTrace(("make sure that friction joint are issue at last\n"));
250  }
251  #endif
252 }
253 
254 inline void ndJointBilateralConstraint::SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction)
255 {
256  const ndInt32 index = desc.m_rowsCount - 1;
257  ndAssert(index >= 0);
258  ndAssert(index < ndInt32(m_maxDof));
259 
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);
262 
263  #ifdef _DEBUG
264  ndInt32 i0 = 0;
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);
269  if ((i0 - i1) != 1)
270  {
271  ndTrace(("make sure that friction joint are issue at last\n"));
272  }
273  #endif
274 }
275 
276 inline void ndJointBilateralConstraint::JacobianDerivative(ndConstraintDescritor&)
277 {
278  //ndAssert(0);
279  ndTrace(("error: this joint is an interface\n"));
280 }
281 
282 inline bool ndJointBilateralConstraint::GetSkeletonFlag() const
283 {
284  return m_isInSkeleton ? true : false;
285 }
286 
287 inline void ndJointBilateralConstraint::SetSkeletonFlag(bool flag)
288 {
289  m_isInSkeleton = ndUnsigned32 (flag ? 1 : 0);
290 }
291 
292 inline bool ndJointBilateralConstraint::IsCollidable() const
293 {
294  return m_enableCollision ? true : false;
295 }
296 
297 inline bool ndJointBilateralConstraint::IsBilateral() const
298 {
299  return true;
300 }
301 
302 inline void ndJointBilateralConstraint::SetCollidable(bool state)
303 {
304  m_enableCollision = ndUnsigned32 (state ? 1 : 0);
305 }
306 
307 inline ndVector ndJointBilateralConstraint::GetForceBody0() const
308 {
309  return m_forceBody0;
310 }
311 
312 inline ndVector ndJointBilateralConstraint::GetTorqueBody0() const
313 {
314  return m_torqueBody0;
315 }
316 
317 inline ndVector ndJointBilateralConstraint::GetForceBody1() const
318 {
319  return m_forceBody1;
320 }
321 
322 inline ndVector ndJointBilateralConstraint::GetTorqueBody1() const
323 {
324  return m_torqueBody1;
325 }
326 
327 inline ndFloat32 ndJointBilateralConstraint::GetDiagonalRegularizer(const ndConstraintDescritor& desc) const
328 {
329  const ndInt32 index = desc.m_rowsCount - 1;
330  ndAssert(index >= 0);
331  ndAssert(index < ndInt32(m_maxDof));
332  return desc.m_diagonalRegularizer[index];
333 }
334 
335 inline void ndJointBilateralConstraint::SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer)
336 {
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));
341 }
342 
343 inline bool ndJointBilateralConstraint::IsInWorld() const
344 {
345  return m_worldNode ? true : false;
346 }
347 
348 inline bool ndJointBilateralConstraint::IsSkeleton() const
349 {
350  const ndJointBilateralSolverModel mode = GetSolverModel();
351  bool test = false;
352  test = test || (mode == m_jointkinematicOpenLoop);
353  test = test || (mode == m_jointkinematicCloseLoop);
354  //test = test || (mode == m_jointkinematicHintOpenLoop);
355  return test;
356 }
357 
358 inline void ndJointBilateralConstraint::ReplaceSentinel(ndBodyKinematic* const sentinel)
359 {
360  m_body1 = sentinel;
361 }
362 
363 #endif
364 
ndSkeletonContainer
Definition: ndSkeletonContainer.h:31
ndJointBilateralConstraint::ndIkInterface
Definition: ndJointBilateralConstraint.h:65
ndDynamicsUpdateCuda
Definition: ndDynamicsUpdateCuda.h:31
ndDynamicsUpdateSoa
Definition: ndDynamicsUpdateSoa.h:70
ndConstraintDescritor
Definition: ndConstraint.h:179
ndJointBilateralConstraint
Definition: ndJointBilateralConstraint.h:53
ndDynamicsUpdateOpencl
Definition: ndDynamicsUpdateOpencl.h:31
ndForceImpactPair
Definition: ndConstraint.h:114
ndConstraintDebugCallback
Definition: ndConstraint.h:42
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndDynamicsUpdateAvx2
Definition: ndDynamicsUpdateAvx2.h:31
ndMatrix
Definition: ndMatrix.h:42
ndList
Definition: ndList.h:33
ndJacobian
Definition: ndConstraint.h:99
ndJointBilateralConstraint::ndJointList
Definition: ndJointBilateralConstraint.h:56
ndConstraint
Definition: ndConstraint.h:229
ndContainersFreeListAlloc
Definition: ndContainersAlloc.h:60
ndDynamicsUpdate
Definition: ndDynamicsUpdate.h:45
ndJointAccelerationDecriptor
Definition: ndConstraint.h:158
ndLoadSaveBase::ndLoadDescriptor
Definition: ndSaveLoadSytem.h:59
ndLoadSaveBase::ndSaveDescriptor
Definition: ndSaveLoadSytem.h:93
ndVector
Definition: ndVectorArmNeon.h:41
ndIkSolver
Definition: ndIkSolver.h:32
ndWorld
Definition: ndWorld.h:47