Newton Dynamics  4.00
ndSkeletonContainer.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_SKELETON_CONTAINER_H__
23 #define __ND_SKELETON_CONTAINER_H__
24 
25 #include "ndNewtonStdafx.h"
26 
27 class ndIkSolver;
29 
31 {
32  public:
35 
36  protected:
37  class ndOrdinal
38  {
39  public:
40  ndOrdinal()
41  {
42  for (ndInt32 i = 0; i < ndInt32(sizeof(m_sourceJacobianIndex)); ++i)
43  {
44  m_sourceJacobianIndex[i] = ndInt8(i);
45  }
46  }
47 
48  ndInt8 m_sourceJacobianIndex[12];
49  };
50 
51  class ndNodePair
52  {
53  public:
54  ndInt32 m_m0;
55  ndInt32 m_m1;
56  const ndConstraint* m_joint;
57  };
58 
59  D_MSV_NEWTON_ALIGN_32
61  {
62  public:
63  ndSpatialVector m_joint;
64  ndSpatialVector m_body;
65  } D_GCC_NEWTON_ALIGN_32;
66 
67  D_MSV_NEWTON_ALIGN_32
69  {
70  public:
71  ndSpatialMatrix m_jt;
72  ndSpatialMatrix m_invMass;
73  } D_GCC_NEWTON_ALIGN_32;
74 
75  D_MSV_NEWTON_ALIGN_32
77  {
78  public:
79  ndMatriData m_body;
80  ndMatriData m_joint;
81  } D_GCC_NEWTON_ALIGN_32;
82 
83  class ndNode
84  {
85  public:
86  ndNode();
87  ~ndNode();
88  ndInt32 Factorize(const ndLeftHandSide* const leftHandSide, const ndRightHandSide* const rightHandSide, ndSpatialMatrix* const bodyMassArray, ndSpatialMatrix* const jointMassArray);
89 
90  inline void CalculateJacobianBlock();
91  inline void CalculateInertiaMatrix(ndSpatialMatrix* const bodyMassArray) const;
92  inline void CalculateJointDiagonal(const ndSpatialMatrix* const bodyMassArray, ndSpatialMatrix* const jointMassArray);
93  inline void CalculateBodyDiagonal(ndNode* const child, ndSpatialMatrix* const bodyMassArray, const ndSpatialMatrix* const jointMassArray);
94  inline void GetJacobians(const ndLeftHandSide* const leftHandSide, const ndRightHandSide* const rightHandSide, ndSpatialMatrix* const jointMassArray);
95 
96  inline void BodyDiagInvTimeSolution(ndForcePair& force);
97  inline void JointDiagInvTimeSolution(ndForcePair& force);
98  inline void JointJacobianTimeMassForward(ndForcePair& force);
99  inline void BodyJacobianTimeSolutionBackward(ndForcePair& force) const;
100  inline ndInt32 GetAuxiliaryRows(const ndRightHandSide* const rightHandSide) const;
101  inline void BodyJacobianTimeMassForward(const ndForcePair& force, ndForcePair& parentForce) const;
102  inline void JointJacobianTimeSolutionBackward(ndForcePair& force, const ndForcePair& parentForce) const;
103 
105  ndBodyKinematic* m_body;
107  ndNode* m_parent;
108  ndNode* m_child;
109  ndNode* m_sibling;
110  ndInt32 m_index;
111  ndOrdinal m_ordinal;
112  ndInt8 m_dof;
113  ndInt8 m_swapJacobianBodiesIndex;
114  };
115 
116  class ndNodeList : public ndList<ndNode, ndContainersFreeListAlloc<ndSkeletonContainer::ndNode> >
117  {
118  public:
119  ndNodeList()
121  {
122  }
123  };
124 
125  private:
126  ndNode* GetRoot() const;
127 
128  void Clear();
129  void CheckSleepState();
130  void Init(ndBodyKinematic* const rootBody);
131  ndNode* AddChild(ndJointBilateralConstraint* const joint, ndNode* const parent);
132  void Finalize(ndInt32 loopJoints, ndJointBilateralConstraint** const loopJointArray);
133 
134  void InitLoopMassMatrix();
135  void ClearCloseLoopJoints();
136  void AddCloseLoopJoint(ndConstraint* const joint);
137  void CalculateReactionForces(ndJacobian* const internalForces);
138  void InitMassMatrix(const ndLeftHandSide* const matrixRow, ndRightHandSide* const rightHandSide);
139  void CalculateBufferSizeInBytes();
140  void ConditionMassMatrix() const;
141  void SortGraph(ndNode* const root, ndInt32& index);
142  void RebuildMassMatrix(const ndFloat32* const diagDamp) const;
143  void CalculateLoopMassMatrixCoefficients(ndFloat32* const diagDamp);
144  void FactorizeMatrix(ndInt32 size, ndInt32 stride, ndFloat32* const matrix, ndFloat32* const diagDamp) const;
145  void SolveAuxiliary(ndJacobian* const internalForces, const ndForcePair* const accel, ndForcePair* const force) const;
146  void SolveBlockLcp(ndInt32 size, ndInt32 blockSize, const ndFloat32* const x0, ndFloat32* const x, ndFloat32* const b, const ndFloat32* const low, const ndFloat32* const high, const ndInt32* const normalIndex) const;
147  void SolveLcp(ndInt32 stride, ndInt32 size, const ndFloat32* const matrix, const ndFloat32* const x0, ndFloat32* const x, const ndFloat32* const b, const ndFloat32* const low, const ndFloat32* const high, const ndInt32* const normalIndex) const;
148 
149  inline void SolveBackward(ndForcePair* const force) const;
150  inline void CalculateForce(ndForcePair* const force, const ndForcePair* const accel) const;
151  inline void UpdateForces(ndJacobian* const internalForces, const ndForcePair* const force) const;
152  inline void CalculateJointAccel(const ndJacobian* const internalForces, ndForcePair* const accel) const;
153  inline void SolveForward(ndForcePair* const force, const ndForcePair* const accel, ndInt32 startNode) const;
154 
155  void SolveImmediate(ndIkSolver& solverInfo);
156  void UpdateForcesImmediate(ndArray<ndBodyKinematic*>& bodyArray, const ndForcePair* const force) const;
157  void CalculateJointAccelImmediate(const ndJacobian* const internalForces, ndForcePair* const accel) const;
158  void SolveAuxiliaryImmediate(ndArray<ndBodyKinematic*>& bodyArray, ndJacobian* const internalForces, const ndForcePair* const accel, ndForcePair* const force) const;
159 
160  ndNode* m_skeleton;
161  ndNode** m_nodesOrder;
162  ndRightHandSide* m_rightHandSide;
163  const ndLeftHandSide* m_leftHandSide;
164  ndNodePair* m_pairs;
165  ndInt32* m_frictionIndex;
166  ndInt32* m_matrixRowsIndex;
167  ndFloat32* m_massMatrix11;
168  ndFloat32* m_massMatrix10;
169  ndFloat32* m_deltaForce;
170 
171  ndNodeList m_nodeList;
172  ndArray<ndConstraint*> m_loopingJoints;
173  ndArray<ndInt8> m_auxiliaryMemoryBuffer;
174  ndSpinLock m_lock;
175  ndInt32 m_blockSize;
176  ndInt32 m_rowCount;
177  ndInt32 m_loopRowCount;
178  ndInt32 m_auxiliaryRowCount;
179  ndInt32 m_loopCount;
180  ndInt32 m_dynamicsLoopCount;
181  ndUnsigned8 m_isResting;
182 
183  friend class ndWorld;
184  friend class ndIkSolver;
185  friend class ndSkeletonList;
186  friend class ndSkeletonQueue;
187  friend class ndDynamicsUpdate;
188  friend class ndDynamicsUpdateSoa;
189  friend class ndDynamicsUpdateAvx2;
190  friend class ndDynamicsUpdateCuda;
191 };
192 
193 inline ndSkeletonContainer::ndNode* ndSkeletonContainer::GetRoot() const
194 {
195  return m_skeleton;
196 }
197 
198 #endif
199 
200 
ndSpinLock
Simple spin lock for synchronizing threads for very short period of time.
Definition: ndUtils.h:212
ndSkeletonContainer
Definition: ndSkeletonContainer.h:31
ndArray< ndBodyKinematic * >
ndDynamicsUpdateCuda
Definition: ndDynamicsUpdateCuda.h:31
ndSkeletonContainer::ndForcePair
Definition: ndSkeletonContainer.h:61
ndDynamicsUpdateSoa
Definition: ndDynamicsUpdateSoa.h:70
ndJointBilateralConstraint
Definition: ndJointBilateralConstraint.h:53
ndSkeletonQueue
Definition: ndWorld.cpp:49
ndRightHandSide
Definition: ndConstraint.h:205
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndSpatialVector
Definition: ndSpatialVector.h:31
ndDynamicsUpdateAvx2
Definition: ndDynamicsUpdateAvx2.h:31
ndList
Definition: ndList.h:33
ndSkeletonContainer::ndNodePair
Definition: ndSkeletonContainer.h:52
ndJacobian
Definition: ndConstraint.h:99
ndSkeletonContainer::ndNode
Definition: ndSkeletonContainer.h:84
ndSkeletonContainer::ndOrdinal
Definition: ndSkeletonContainer.h:38
ndConstraint
Definition: ndConstraint.h:229
ndSkeletonContainer::ndBodyJointMatrixDataPair
Definition: ndSkeletonContainer.h:77
ndSkeletonContainer::ndNodeList
Definition: ndSkeletonContainer.h:117
ndDynamicsUpdate
Definition: ndDynamicsUpdate.h:45
ndSpatialMatrix
Definition: ndSpatialMatrix.h:31
ndLeftHandSide
Definition: ndConstraint.h:197
ndSkeletonContainer::ndMatriData
Definition: ndSkeletonContainer.h:69
ndIkSolver
Definition: ndIkSolver.h:32
ndWorld
Definition: ndWorld.h:47
ndSkeletonList
Definition: ndSkeletonList.h:31