Newton Dynamics  4.00
ndDynamicsUpdate.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_WORLD_DYNAMICS_UPDATE_H__
23 #define __ND_WORLD_DYNAMICS_UPDATE_H__
24 
25 #include "ndNewtonStdafx.h"
26 
27 #define D_MAX_BODY_RADIX_BIT 9
28 
29 // the solver is a RK order 4, but instead of weighting the intermediate derivative by the usual 1/6, 1/3, 1/3, 1/6 coefficients
30 // I am using 1/4, 1/4, 1/4, 1/4.
31 // This is correct.The weighting coefficients of any RK method comes from an arbitrary criteria
32 // solving for a set of linear equation on the coefficients.
33 // The standard coefficients just happen to lead to an accurate result because they are optimal
34 // with respect to a second order solution.For differential equations of higher order, then is not
35 // clear if the traditional weigh factors are any better than any other set of weighting factors.
36 // A different set of coefficient generates results that are not much different than the optimal set,
37 // but it allows for simpler calculation of the intermediate derivatives and also for less intermediate memory.
38 // For more detail on the derivation of the Runge Kutta coefficients you can go to:
39 // http://pathfinder.scar.utoronto.ca/~dyer/csca57/book_P/node51.html
40 
41 class ndWorld;
42 
43 D_MSV_NEWTON_ALIGN_32
45 {
46  public:
48  {
49  public:
50  ndInt32 m_body;
51  ndInt32 m_joint;
52  };
53 
55  {
56  public:
57  ndBodyKinematic* m_body;
58  ndBodyKinematic* m_root;
59  };
60 
61  class ndIsland
62  {
63  public:
64  ndIsland(ndBodyKinematic* const root)
65  :m_start(0)
66  , m_count(0)
67  , m_root(root)
68  {
69  }
70 
71  ndInt32 m_start;
72  ndInt32 m_count;
73  ndBodyKinematic* m_root;
74  };
75 
76  public:
77  ndDynamicsUpdate(ndWorld* const world);
78  virtual ~ndDynamicsUpdate();
79 
80  virtual const char* GetStringId() const;
81  ndInt32 GetUnconstrainedBodyCount() const;
82  void ClearBuffer(void* const buffer, ndInt32 sizeInByte) const;
83  void ClearJacobianBuffer(ndInt32 count, ndJacobian* const dst) const;
84 
85  ndVector GetVelocTol() const;
86  ndFloat32 GetTimestepRK() const;
87  ndArray<ndIsland>& GetIslands();
88  ndArray<ndJacobian>& GetInternalForces();
89  ndArray<ndLeftHandSide>& GetLeftHandSide();
90  ndArray<ndInt32>& GetJointForceIndexBuffer();
91  ndArray<ndRightHandSide>& GetRightHandSide();
92  ndArray<ndJacobian>& GetTempInternalForces();
93  ndArray<ndBodyKinematic*>& GetBodyIslandOrder();
94  ndArray<ndJointBodyPairIndex>& GetJointBodyPairIndexBuffer();
95 
96  private:
97  void SortJoints();
98  void SortIslands();
99  void BuildIsland();
100  void InitWeights();
101  void InitBodyArray();
102  void InitSkeletons();
103  void CalculateForces();
104  void IntegrateBodies();
105  void UpdateSkeletons();
106  void InitJacobianMatrix();
107  void UpdateForceFeedback();
108  void CalculateJointsForce();
109  void IntegrateBodiesVelocity();
110  void CalculateJointsAcceleration();
111  void IntegrateUnconstrainedBodies();
112 
113  void DetermineSleepStates();
114  void GetJacobianDerivatives(ndConstraint* const joint);
115 
116  protected:
117  void Clear();
118  virtual void Update();
119  void SortJointsScan();
120  void SortBodyJointScan();
121  ndBodyKinematic* FindRootAndSplit(ndBodyKinematic* const body);
122 
123  ndVector m_velocTol;
124  ndArray<ndIsland> m_islands;
125  ndArray<ndInt32> m_jointForcesIndex;
126  ndArray<ndJacobian> m_internalForces;
127  ndArray<ndLeftHandSide> m_leftHandSide;
128  ndArray<ndRightHandSide> m_rightHandSide;
129  ndArray<ndJacobian> m_tempInternalForces;
130  ndArray<ndBodyKinematic*> m_bodyIslandOrder;
131  ndArray<ndJointBodyPairIndex> m_jointBodyPairIndexBuffer;
132 
133  ndWorld* m_world;
134  ndFloat32 m_timestep;
135  ndFloat32 m_invTimestep;
136  ndFloat32 m_firstPassCoef;
137  ndFloat32 m_invStepRK;
138  ndFloat32 m_timestepRK;
139  ndFloat32 m_invTimestepRK;
140  ndUnsigned32 m_solverPasses;
141  ndInt32 m_activeJointCount;
142  ndInt32 m_unConstrainedBodyCount;
143 
144  friend class ndWorld;
145  friend class ndSkeletonContainer;
146 } D_GCC_NEWTON_ALIGN_32;
147 
148 inline ndVector ndDynamicsUpdate::GetVelocTol() const
149 {
150  return m_velocTol;
151 }
152 
153 inline ndFloat32 ndDynamicsUpdate::GetTimestepRK() const
154 {
155  return m_timestepRK;
156 }
157 
158 inline ndArray<ndDynamicsUpdate::ndIsland>& ndDynamicsUpdate::GetIslands()
159 {
160  return m_islands;
161 }
162 
163 inline ndArray<ndJacobian>& ndDynamicsUpdate::GetInternalForces()
164 {
165  return m_internalForces;
166 }
167 
168 inline ndArray<ndJacobian>& ndDynamicsUpdate::GetTempInternalForces()
169 {
170  return m_tempInternalForces;
171 }
172 
173 inline ndArray<ndLeftHandSide>& ndDynamicsUpdate::GetLeftHandSide()
174 {
175  return m_leftHandSide;
176 }
177 
178 inline ndArray<ndRightHandSide>& ndDynamicsUpdate::GetRightHandSide()
179 {
180  return m_rightHandSide;
181 }
182 
183 inline ndArray<ndBodyKinematic*>& ndDynamicsUpdate::GetBodyIslandOrder()
184 {
185  return m_bodyIslandOrder;
186 }
187 
188 inline ndInt32 ndDynamicsUpdate::GetUnconstrainedBodyCount() const
189 {
190  return m_unConstrainedBodyCount;
191 }
192 
193 inline ndArray<ndDynamicsUpdate::ndJointBodyPairIndex>& ndDynamicsUpdate::GetJointBodyPairIndexBuffer()
194 {
195  return m_jointBodyPairIndexBuffer;
196 }
197 
198 inline ndArray<ndInt32>& ndDynamicsUpdate::GetJointForceIndexBuffer()
199 {
200  return m_jointForcesIndex;
201 }
202 
203 inline void ndDynamicsUpdate::ClearJacobianBuffer(ndInt32 count, ndJacobian* const buffer) const
204 {
205  const ndVector zero(ndVector::m_zero);
206  ndVector* const dst = &buffer[0].m_linear;
207  for (ndInt32 i = 0; i < count; ++i)
208  {
209  dst[i * 2 + 0] = zero;
210  dst[i * 2 + 1] = zero;
211  }
212 }
213 
214 inline void ndDynamicsUpdate::ClearBuffer(void* const buffer, ndInt32 sizeInByte) const
215 {
216  D_TRACKTIME();
217  ndInt32 sizeInJacobian = ndInt32 (sizeInByte / sizeof(ndJacobian));
218  ClearJacobianBuffer(sizeInJacobian, (ndJacobian*)buffer);
219  char* const ptr = (char*)buffer;
220  for (ndInt32 i = ndInt32 (sizeInJacobian * sizeof(ndJacobian)); i < sizeInByte; ++i)
221  {
222  ptr[i] = 0;
223  }
224 }
225 
226 inline ndBodyKinematic* ndDynamicsUpdate::FindRootAndSplit(ndBodyKinematic* const body)
227 {
228  ndBodyKinematic* node = body;
229  while (node->m_islandParent != node)
230  {
231  ndBodyKinematic* const prev = node;
232  node = node->m_islandParent;
233  prev->m_islandParent = node->m_islandParent;
234  }
235  return node;
236 }
237 
238 #endif
239 
ndClassAlloc
Base class for providing memory allocation for all other engine classes.
Definition: ndClassAlloc.h:30
ndSkeletonContainer
Definition: ndSkeletonContainer.h:31
ndArray
Generic template vector.
Definition: ndArray.h:42
ndDynamicsUpdate::ndJointBodyPairIndex
Definition: ndDynamicsUpdate.h:48
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndDynamicsUpdate::ndBodyIndexPair
Definition: ndDynamicsUpdate.h:55
ndJacobian
Definition: ndConstraint.h:99
ndConstraint
Definition: ndConstraint.h:229
ndDynamicsUpdate
Definition: ndDynamicsUpdate.h:45
ndDynamicsUpdate::ndIsland
Definition: ndDynamicsUpdate.h:62
ndVector
Definition: ndVectorArmNeon.h:41
ndWorld
Definition: ndWorld.h:47