Newton Dynamics  4.00
ndBodyKinematic.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_BODY_KINEMATIC_H__
23 #define __ND_BODY_KINEMATIC_H__
24 
25 #include "ndCollisionStdafx.h"
26 #include "ndBody.h"
27 #include "ndConstraint.h"
28 #include "ndBodyListView.h"
29 
30 class ndScene;
31 class ndModel;
34 
35 #define D_FREEZZING_VELOCITY_DRAG ndFloat32 (0.9f)
36 #define D_SOLVER_MAX_ERROR (D_FREEZE_MAG * ndFloat32 (0.5f))
37 
38 D_MSV_NEWTON_ALIGN_32
39 class ndBodyKinematic : public ndBody
40 {
41  class ndContactkey
42  {
43  public:
44  ndContactkey(ndUnsigned32 tag0, ndUnsigned32 tag1);
45 
46  bool operator> (const ndContactkey& key) const;
47  bool operator< (const ndContactkey& key) const;
48  bool operator== (const ndContactkey& key) const;
49  private:
50  union
51  {
52  ndUnsigned64 m_tag;
53  struct
54  {
55  ndUnsigned32 m_tagLow;
56  ndUnsigned32 m_tagHigh;
57  };
58  };
59  };
60 
61  public:
62  class ndJointList : public ndList<ndJointBilateralConstraint*, ndContainersFreeListAlloc<ndJointBilateralConstraint*>>
63  {
64  public:
65  ndJointList()
67  {
68  }
69  };
70 
71  class ndContactMap: public ndTree<ndContact*, ndContactkey, ndContainersFreeListAlloc<ndContact*>>
72  {
73  public:
74  D_COLLISION_API ndContact* FindContact(const ndBody* const body0, const ndBody* const body1) const;
75 
76  private:
77  ndContactMap();
78  ~ndContactMap();
79  void AttachContact(ndContact* const contact);
80  void DetachContact(ndContact* const contact);
81  friend class ndBodyKinematic;
82  };
83 
84  D_CLASS_REFLECTION(ndBodyKinematic);
85  D_COLLISION_API ndBodyKinematic();
86  D_COLLISION_API ndBodyKinematic(const ndLoadSaveBase::ndLoadDescriptor& desc);
87  D_COLLISION_API virtual ~ndBodyKinematic();
88 
89  ndScene* GetScene() const;
90 
91  ndUnsigned32 GetIndex() const;
92  ndFloat32 GetInvMass() const;
93  const ndVector GetInvInertia() const;
94  const ndVector& GetMassMatrix() const;
95  const ndMatrix& GetInvInertiaMatrix() const;
96 
97  ndVector GetGyroAlpha() const;
98  ndVector GetGyroTorque() const;
99 
100  bool GetSleepState() const;
101  void RestoreSleepState(bool state);
102  D_COLLISION_API void SetSleepState(bool state);
103 
104  bool GetAutoSleep() const;
105  void SetAutoSleep(bool state);
106  ndFloat32 GetMaxLinearStep() const;
107  ndFloat32 GetMaxAngularStep() const;
108  void SetDebugMaxLinearAndAngularIntegrationStep(ndFloat32 angleInRadian, ndFloat32 stepInUnitPerSeconds);
109 
110  virtual ndFloat32 GetLinearDamping() const;
111  virtual void SetLinearDamping(ndFloat32 linearDamp);
112 
113  virtual ndVector GetCachedDamping() const;
114  virtual ndVector GetAngularDamping() const;
115  virtual void SetAngularDamping(const ndVector& angularDamp);
116 
117  D_COLLISION_API ndShapeInstance& GetCollisionShape();
118  D_COLLISION_API const ndShapeInstance& GetCollisionShape() const;
119  D_COLLISION_API virtual void SetCollisionShape(const ndShapeInstance& shapeInstance);
120  D_COLLISION_API virtual bool RayCast(ndRayCastNotify& callback, const ndFastRay& ray, const ndFloat32 maxT) const;
121 
122  D_COLLISION_API ndVector CalculateLinearMomentum() const;
123  D_COLLISION_API virtual ndVector CalculateAngularMomentum() const;
124  D_COLLISION_API ndFloat32 TotalEnergy() const;
125 
126  D_COLLISION_API ndMatrix CalculateInertiaMatrix() const;
127  D_COLLISION_API virtual ndMatrix CalculateInvInertiaMatrix() const;
128 
129  D_COLLISION_API virtual void IntegrateVelocity(ndFloat32 timestep);
130  D_COLLISION_API virtual void Save(const ndLoadSaveBase::ndSaveDescriptor& desc) const;
131 
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);
137 
138  void GetMassMatrix(ndFloat32& Ixx, ndFloat32& Iyy, ndFloat32& Izz, ndFloat32& mass);
139 
140  D_COLLISION_API void SetMatrixUpdateScene(const ndMatrix& matrix);
141  D_COLLISION_API virtual ndContact* FindContact(const ndBody* const otherBody) const;
142 
143  virtual ndBodyKinematic* GetAsBodyKinematic();
144 
145  ndSkeletonContainer* GetSkeleton() const;
146  void SetSkeleton(ndSkeletonContainer* const skeleton);
147 
148  virtual ndVector GetForce() const;
149  virtual ndVector GetTorque() const;
150 
151  virtual void SetForce(const ndVector& force);
152  virtual void SetTorque(const ndVector& torque);
153 
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);
157 
158  ndVector GetAccel() const;
159  ndVector GetAlpha() const;
160  void SetAccel(const ndVector& accel);
161  void SetAlpha(const ndVector& alpha);
162 
163  ndContactMap& GetContactMap();
164  const ndContactMap& GetContactMap() const;
165  const ndJointList& GetJointList() const;
166 
167  protected:
168  D_COLLISION_API virtual void AttachContact(ndContact* const contact);
169  D_COLLISION_API virtual void DetachContact(ndContact* const contact);
170 
171  D_COLLISION_API virtual void DetachJoint(ndJointList::ndNode* const node);
172  D_COLLISION_API virtual ndJointList::ndNode* AttachJoint(ndJointBilateralConstraint* const joint);
173 
174  D_COLLISION_API virtual void IntegrateExternalForce(ndFloat32 timestep);
175 
176  void SetAccel(const ndJacobian& accel);
177  virtual void SpecialUpdate(ndFloat32 timestep);
178  virtual void IntegrateGyroSubstep(const ndVector& timestep);
179  virtual void ApplyExternalForces(ndInt32 threadIndex, ndFloat32 timestep);
180  virtual ndJacobian IntegrateForceAndToque(const ndVector& force, const ndVector& torque, const ndVector& timestep) const;
181 
182  void UpdateCollisionMatrix();
183  void PrepareStep(ndInt32 index);
184  void SetSceneNodes(ndScene* const scene, ndBodyListView::ndNode* const node);
185 
186  virtual void AddDampingAcceleration(ndFloat32 timestep);
187 
188  D_COLLISION_API virtual void EvaluateSleepState(ndFloat32 freezeSpeed2, ndFloat32 freezeAccel2);
189 
190  ndMatrix m_invWorldInertiaMatrix;
191  ndShapeInstance m_shapeInstance;
192  ndVector m_mass;
193  ndVector m_invMass;
194  ndVector m_accel;
195  ndVector m_alpha;
196  ndVector m_gyroAlpha;
197  ndVector m_gyroTorque;
198  ndQuaternion m_gyroRotation;
199  ndJointList m_jointList;
200  ndContactMap m_contactList;
201  mutable ndSpinLock m_lock;
202  ndScene* m_scene;
203  ndBodyKinematic* m_islandParent;
204  ndBodyListView::ndNode* m_sceneNode;
205  ndSkeletonContainer* m_skeletonContainer;
206  ndSpecialList<ndBodyKinematic>::ndNode* m_spetialUpdateNode;
207 
208  ndFloat32 m_maxAngleStep;
209  ndFloat32 m_maxLinearStep;
210  ndFloat32 m_weigh;
211  ndInt32 m_index;
212  ndInt32 m_bodyNodeIndex;
213  ndInt32 m_buildSkelIndex;
214  ndInt32 m_sceneNodeIndex;
215  ndInt32 m_buildBodyNodeIndex;
216  ndInt32 m_buildSceneNodeIndex;
217 
218  D_COLLISION_API static ndVector m_velocTol;
219 
220  friend class ndWorld;
221  friend class ndScene;
222  friend class ndContact;
223  friend class ndIkSolver;
224  friend class ndBvhLeafNode;
225  friend class ndDynamicsUpdate;
226  friend class ndWorldSceneCuda;
227  friend class ndBvhSceneManager;
228  friend class ndSkeletonContainer;
229  friend class ndDynamicsUpdateSoa;
230  friend class ndDynamicsUpdateAvx2;
231  friend class ndDynamicsUpdateCuda;
232  friend class ndDynamicsUpdateOpencl;
233  friend class ndJointBilateralConstraint;
234 } D_GCC_NEWTON_ALIGN_32;
235 
236 
238 {
239  ndBodySentinel* GetAsBodySentinel() { return this; }
240 };
241 
242 inline ndUnsigned32 ndBodyKinematic::GetIndex() const
243 {
244  return ndUnsigned32(m_index);
245 }
246 
247 inline ndFloat32 ndBodyKinematic::GetInvMass() const
248 {
249  return m_invMass.m_w;
250 }
251 
252 inline const ndVector ndBodyKinematic::GetInvInertia() const
253 {
254  return m_invMass & ndVector::m_triplexMask;
255 }
256 
257 inline const ndVector& ndBodyKinematic::GetMassMatrix() const
258 {
259  return m_mass;
260 }
261 
262 inline const ndMatrix& ndBodyKinematic::GetInvInertiaMatrix() const
263 {
264  return m_invWorldInertiaMatrix;
265 }
266 
267 inline ndVector ndBodyKinematic::GetGyroAlpha() const
268 {
269  return m_gyroAlpha;
270 }
271 
272 inline ndVector ndBodyKinematic::GetGyroTorque() const
273 {
274  return m_gyroTorque;
275 }
276 
277 inline void ndBodyKinematic::GetMassMatrix(ndFloat32& Ixx, ndFloat32& Iyy, ndFloat32& Izz, ndFloat32& mass)
278 {
279  Ixx = m_mass.m_x;
280  Iyy = m_mass.m_y;
281  Izz = m_mass.m_z;
282  mass = m_mass.m_w;
283 }
284 
285 inline void ndBodyKinematic::SetMassMatrix(const ndVector& massMatrix)
286 {
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);
292 }
293 
294 inline void ndBodyKinematic::SetMassMatrix(ndFloat32 Ixx, ndFloat32 Iyy, ndFloat32 Izz, ndFloat32 mass)
295 {
296  SetMassMatrix(ndVector(Ixx, Iyy, Izz, mass));
297 }
298 
299 inline void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndShapeInstance& shapeInstance)
300 {
301  ndMatrix inertia(shapeInstance.CalculateInertia());
302 
303  ndVector origin(inertia.m_posit);
304  for (ndInt32 i = 0; i < 3; ++i)
305  {
306  inertia[i] = inertia[i].Scale(mass);
307  //inertia[i][i] = (inertia[i][i] + origin[i] * origin[i]) * mass;
308  //for (ndInt32 j = i + 1; j < 3; ++j) {
309  // ndFloat32 crossIJ = origin[i] * origin[j];
310  // inertia[i][j] = (inertia[i][j] + crossIJ) * mass;
311  // inertia[j][i] = (inertia[j][i] + crossIJ) * mass;
312  //}
313  }
314 
315  // although the engine fully supports asymmetric inertia, I will ignore cross inertia for now
316  SetCentreOfMass(origin);
317  SetMassMatrix(mass, inertia);
318 }
319 
320 inline ndBodyKinematic* ndBodyKinematic::GetAsBodyKinematic()
321 {
322  return this;
323 }
324 
325 inline ndScene* ndBodyKinematic::GetScene() const
326 {
327  return m_scene;
328 }
329 
330 inline void ndBodyKinematic::SetSceneNodes(ndScene* const scene, ndBodyListView::ndNode* const node)
331 {
332  m_scene = scene;
333  m_sceneNode = node;
334 }
335 
336 inline ndVector ndBodyKinematic::GetForce() const
337 {
338  return ndVector::m_zero;
339 }
340 
341 inline ndVector ndBodyKinematic::GetTorque() const
342 {
343  return ndVector::m_zero;
344 }
345 
346 inline void ndBodyKinematic::SetForce(const ndVector&)
347 {
348 }
349 
350 inline void ndBodyKinematic::SetTorque(const ndVector&)
351 {
352 }
353 
354 inline ndVector ndBodyKinematic::GetAccel() const
355 {
356  return m_accel;
357 }
358 
359 inline void ndBodyKinematic::SetAccel(const ndVector& accel)
360 {
361  m_accel = accel;
362 }
363 
364 inline ndVector ndBodyKinematic::GetAlpha() const
365 {
366  return m_alpha;
367 }
368 
369 inline void ndBodyKinematic::SetAlpha(const ndVector& alpha)
370 {
371  m_alpha = alpha;
372 }
373 
374 inline void ndBodyKinematic::AddDampingAcceleration(ndFloat32)
375 {
376 }
377 
378 inline void ndBodyKinematic::SetAccel(const ndJacobian& accel)
379 {
380  SetAccel(accel.m_linear);
381  SetAlpha(accel.m_angular);
382 }
383 
384 inline void ndBodyKinematic::PrepareStep(ndInt32 index)
385 {
386  m_index = index;
387  m_isJointFence0 = 1;
388  m_isJointFence1 = 1;
389  m_isConstrained = 0;
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;
396 }
397 
398 inline ndBodyKinematic::ndContactMap& ndBodyKinematic::GetContactMap()
399 {
400  return m_contactList;
401 }
402 
403 inline const ndBodyKinematic::ndContactMap& ndBodyKinematic::GetContactMap() const
404 {
405  return m_contactList;
406 }
407 
408 inline const ndBodyKinematic::ndJointList& ndBodyKinematic::GetJointList() const
409 {
410  return m_jointList;
411 }
412 
413 inline ndShapeInstance& ndBodyKinematic::GetCollisionShape()
414 {
415  return (ndShapeInstance&)m_shapeInstance;
416 }
417 
418 inline const ndShapeInstance& ndBodyKinematic::GetCollisionShape() const
419 {
420  return m_shapeInstance;
421 }
422 
423 inline bool ndBodyKinematic::GetAutoSleep() const
424 {
425  return m_autoSleep ? true : false;
426 }
427 
428 inline bool ndBodyKinematic::GetSleepState() const
429 {
430  return m_equilibrium ? true : false;
431 }
432 
433 inline void ndBodyKinematic::RestoreSleepState(bool state)
434 {
435  m_equilibrium = ndUnsigned8 (state ? 1 : 0);
436 }
437 
438 inline void ndBodyKinematic::SetAutoSleep(bool state)
439 {
440  m_autoSleep = ndUnsigned8 (state ? 1 : 0);
441  SetSleepState(false);
442 }
443 
444 inline ndSkeletonContainer* ndBodyKinematic::GetSkeleton() const
445 {
446  return m_skeletonContainer;
447 }
448 
449 inline void ndBodyKinematic::SetSkeleton(ndSkeletonContainer* const skeleton)
450 {
451  m_skeletonContainer = skeleton;
452 }
453 
454 inline ndFloat32 ndBodyKinematic::GetMaxLinearStep() const
455 {
456  return m_maxLinearStep;
457 }
458 
459 inline ndFloat32 ndBodyKinematic::GetMaxAngularStep() const
460 {
461  return m_maxAngleStep;
462 }
463 
464 inline void ndBodyKinematic::SetDebugMaxLinearAndAngularIntegrationStep(ndFloat32 angleInRadian, ndFloat32 stepInUnitPerSeconds)
465 {
466  m_maxLinearStep = ndMax(ndAbs(stepInUnitPerSeconds), ndFloat32(1.0f));
467  m_maxAngleStep = ndMax(ndAbs(angleInRadian), ndFloat32(90.0f) * ndDegreeToRad);
468 }
469 
470 inline void ndBodyKinematic::SetLinearDamping(ndFloat32)
471 {
472 }
473 
474 inline ndFloat32 ndBodyKinematic::GetLinearDamping() const
475 {
476  return ndFloat32(0.0f);
477 }
478 
479 inline void ndBodyKinematic::SetAngularDamping(const ndVector&)
480 {
481 }
482 
483 inline ndVector ndBodyKinematic::GetAngularDamping() const
484 {
485  return ndVector::m_zero;
486 }
487 
488 inline ndVector ndBodyKinematic::GetCachedDamping() const
489 {
490  return ndVector::m_one;
491 }
492 
493 inline void ndBodyKinematic::UpdateInvInertiaMatrix()
494 {
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));
499 
500  m_invWorldInertiaMatrix = CalculateInvInertiaMatrix();
501 
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));
506 }
507 
508 inline void ndBodyKinematic::IntegrateGyroSubstep(const ndVector&)
509 {
510 }
511 
512 inline ndJacobian ndBodyKinematic::IntegrateForceAndToque(const ndVector&, const ndVector&, const ndVector&) const
513 {
514  ndJacobian step;
515  step.m_linear = ndVector::m_zero;
516  step.m_angular = ndVector::m_zero;
517  return step;
518 }
519 
520 inline void ndBodyKinematic::AddImpulse(const ndVector&, const ndVector&, ndFloat32)
521 {
522 }
523 
524 inline void ndBodyKinematic::ApplyImpulsePair(const ndVector&, const ndVector&, ndFloat32)
525 {
526 }
527 
528 inline void ndBodyKinematic::ApplyImpulsesAtPoint(ndInt32, const ndVector* const, const ndVector* const, ndFloat32)
529 {
530 }
531 
532 inline void ndBodyKinematic::SpecialUpdate(ndFloat32)
533 {
534  ndAssert(0);
535 }
536 
537 inline void ndBodyKinematic::ApplyExternalForces(ndInt32, ndFloat32)
538 {
539 }
540 
541 #endif
542 
ndSpinLock
Simple spin lock for synchronizing threads for very short period of time.
Definition: ndUtils.h:212
ndSkeletonContainer
Definition: ndSkeletonContainer.h:31
ndScene
Definition: ndScene.h:59
ndModel
Definition: ndModel.h:33
ndBody
Definition: ndBody.h:43
ndTree
Definition: ndTree.h:82
ndContact
Definition: ndContact.h:97
ndDynamicsUpdateCuda
Definition: ndDynamicsUpdateCuda.h:31
ndRayCastNotify
Definition: ndRayCastNotify.h:31
ndDynamicsUpdateSoa
Definition: ndDynamicsUpdateSoa.h:70
ndJointBilateralConstraint
Definition: ndJointBilateralConstraint.h:53
ndDynamicsUpdateOpencl
Definition: ndDynamicsUpdateOpencl.h:31
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndBvhLeafNode
Definition: ndBvhNode.h:93
ndDynamicsUpdateAvx2
Definition: ndDynamicsUpdateAvx2.h:31
ndBodySentinel
Definition: ndBodyKinematic.h:238
ndMatrix
Definition: ndMatrix.h:42
ndSpecialList
Definition: ndBodyListView.h:32
ndList
Definition: ndList.h:33
ndBvhSceneManager
Definition: ndBvhNode.h:172
ndJacobian
Definition: ndConstraint.h:99
ndQuaternion
Definition: ndQuaternion.h:32
ndFastRay
Definition: ndFastRay.h:48
ndWorldSceneCuda
Definition: ndWorldSceneCuda.h:28
ndDynamicsUpdate
Definition: ndDynamicsUpdate.h:45
ndBodyKinematic::ndJointList
Definition: ndBodyKinematic.h:63
ndBodyKinematic::ndContactMap
Definition: ndBodyKinematic.h:72
ndShapeInstance
Definition: ndShapeInstance.h:62
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