Newton Dynamics  4.00
ndConstraint.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_CONSTRAINT_H__
23 #define __ND_CONSTRAINT_H__
24 
25 #include "ndCollisionStdafx.h"
26 
27 #define D_MAX_BOUND D_LCP_MAX_VALUE
28 #define D_MIN_BOUND (-D_LCP_MAX_VALUE)
29 #define D_INDEPENDENT_ROW -1
30 #define D_CONSTRAINT_MAX_ROWS (3 * 16)
31 
32 class ndBody;
33 class ndContact;
34 class ndConstraint;
35 class ndLeftHandSide;
36 class ndRightHandSide;
37 class ndBodyKinematic;
39 
40 D_MSV_NEWTON_ALIGN_32
42 {
43  public:
45  {
46  m_debugScale = ndFloat32(1.0f);
47  }
48 
49  virtual ~ndConstraintDebugCallback()
50  {
51  }
52 
53  virtual void DrawPoint(const ndVector& point, const ndVector& color, ndFloat32 thickness = ndFloat32 (8.0f)) = 0;
54  virtual void DrawLine(const ndVector& p0, const ndVector& p1, const ndVector& color, ndFloat32 thickness = ndFloat32(1.0f)) = 0;
55 
56  virtual void SetScale(ndFloat32 scale)
57  {
58  m_debugScale = scale;
59  }
60 
61  virtual ndFloat32 GetScale() const
62  {
63  return m_debugScale;
64  }
65 
66  virtual void DrawFrame(const ndMatrix& matrix, ndFloat32 intensity = ndFloat32 (1.0f))
67  {
68  ndVector x(matrix.m_posit + matrix.RotateVector(ndVector(m_debugScale, ndFloat32(0.0f), ndFloat32(0.0f), ndFloat32(0.0f))));
69  DrawLine(matrix.m_posit, x, ndVector(intensity, ndFloat32(0.0f), ndFloat32(0.0f), ndFloat32(1.0f)));
70 
71  ndVector y(matrix.m_posit + matrix.RotateVector(ndVector(ndFloat32(0.0f), m_debugScale, ndFloat32(0.0f), ndFloat32(0.0f))));
72  DrawLine(matrix.m_posit, y, ndVector(ndFloat32(0.0f), intensity, ndFloat32(0.0f), ndFloat32(1.0f)));
73 
74  ndVector z(matrix.m_posit + matrix.RotateVector(ndVector(ndFloat32(0.0f), ndFloat32(0.0f), m_debugScale, ndFloat32(0.0f))));
75  DrawLine(matrix.m_posit, z, ndVector(ndFloat32(0.0f), ndFloat32(0.0f), intensity, ndFloat32(1.0f)));
76  }
77 
78  virtual void DrawArrow(const ndMatrix& matrix, const ndVector& color, ndFloat32 length)
79  {
80  ndVector p1(matrix.m_posit + matrix.RotateVector(ndVector(m_debugScale * length, ndFloat32(0.0f), ndFloat32(0.0f), ndFloat32(0.0f))));
81  DrawLine(matrix.m_posit, p1, color);
82  }
83 
84  ndFloat32 m_debugScale;
85 } D_GCC_NEWTON_ALIGN_32;
86 
87 D_MSV_NEWTON_ALIGN_32
89 {
90  public:
91  ndVector m_r0;
92  ndVector m_r1;
93  ndVector m_posit0;
94  ndVector m_posit1;
95 } D_GCC_NEWTON_ALIGN_32;
96 
97 D_MSV_NEWTON_ALIGN_32
99 {
100  public:
101  ndVector m_linear;
102  ndVector m_angular;
103 } D_GCC_NEWTON_ALIGN_32;
104 
105 D_MSV_NEWTON_ALIGN_32
107 {
108  public:
109  ndJacobian m_jacobianM0;
110  ndJacobian m_jacobianM1;
111 } D_GCC_NEWTON_ALIGN_32;
112 
114 {
115  public:
116  void Clear()
117  {
118  m_force = ndFloat32(ndFloat32(0.0f));
119  m_impact = ndFloat32(ndFloat32(0.0f));
120  for (ndInt32 i = 0; i < ndInt32(sizeof(m_initialGuess) / sizeof(m_initialGuess[0])); ++i)
121  {
122  m_initialGuess[i] = ndFloat32(ndFloat32(0.0f));
123  }
124  }
125 
126  void Push(ndFloat32 val)
127  {
128  for (ndInt32 i = 1; i < ndInt32(sizeof(m_initialGuess) / sizeof(m_initialGuess[0])); ++i)
129  {
130  m_initialGuess[i - 1] = m_initialGuess[i];
131  }
132  m_initialGuess[sizeof(m_initialGuess) / sizeof(m_initialGuess[0]) - 1] = val;
133  }
134 
135  ndFloat32 GetInitialGuess() const
136  {
137  //return 100.0f;
138  ndFloat32 smallest = ndFloat32(1.0e15f);
139  ndFloat32 value = ndFloat32(ndFloat32(0.0f));
140  for (ndInt32 i = 0; i < ndInt32(sizeof(m_initialGuess) / sizeof(m_initialGuess[0])); ++i)
141  {
142  ndFloat32 mag = ndAbs(m_initialGuess[i]);
143  if (mag < smallest)
144  {
145  smallest = mag;
146  value = m_initialGuess[i];
147  }
148  }
149  return value;
150  }
151 
152  ndFloat32 m_force;
153  ndFloat32 m_impact;
154  ndFloat32 m_initialGuess[4];
155 };
156 
158 {
159  public:
160  ndInt32 m_rowsCount;
161  ndFloat32 m_timestep;
162  ndFloat32 m_invTimestep;
163  ndFloat32 m_firstPassCoefFlag;
164  ndRightHandSide* m_rightHandSide;
165  const ndLeftHandSide* m_leftHandSide;
166 };
167 
169 {
170  public:
171  ndForceImpactPair* m_jointForce;
172  ndFloat32 m_low;
173  ndFloat32 m_upper;
174  ndInt32 m_normalIndex;
175 };
176 
177 D_MSV_NEWTON_ALIGN_32
179 {
180  public:
181  ndJacobianPair m_jacobian[D_CONSTRAINT_MAX_ROWS];
182  ndBilateralBounds m_forceBounds[D_CONSTRAINT_MAX_ROWS];
183  ndFloat32 m_jointAccel[D_CONSTRAINT_MAX_ROWS];
184  ndFloat32 m_restitution[D_CONSTRAINT_MAX_ROWS];
185  ndFloat32 m_penetration[D_CONSTRAINT_MAX_ROWS];
186  ndFloat32 m_diagonalRegularizer[D_CONSTRAINT_MAX_ROWS];
187  ndFloat32 m_penetrationStiffness[D_CONSTRAINT_MAX_ROWS];
188  ndFloat32 m_zeroRowAcceleration[D_CONSTRAINT_MAX_ROWS];
189  ndInt32 m_flags[D_CONSTRAINT_MAX_ROWS];
190  ndFloat32 m_timestep;
191  ndFloat32 m_invTimestep;
192  ndInt32 m_rowsCount;
193 } D_GCC_NEWTON_ALIGN_32;
194 
195 D_MSV_NEWTON_ALIGN_32
197 {
198  public:
199  ndJacobianPair m_Jt;
200  ndJacobianPair m_JMinv;
201 } D_GCC_NEWTON_ALIGN_32;
202 
203 D_MSV_NEWTON_ALIGN_32
205 {
206  public:
207  ndFloat32 m_force;
208  ndFloat32 m_diagDamp;
209  ndFloat32 m_invJinvMJt;
210  ndFloat32 m_coordenateAccel;
211 
212  ndFloat32 m_lowerBoundFrictionCoefficent;
213  ndFloat32 m_upperBoundFrictionCoefficent;
214  ndFloat32 m_deltaAccel;
215  ndFloat32 m_restitution;
216 
217  ndFloat32 m_maxImpact;
218  ndFloat32 m_penetration;
219  ndFloat32 m_diagonalRegularizer;
220  ndFloat32 m_penetrationStiffness;
221 
222  ndForceImpactPair* m_jointFeebackForce;
223  ndInt32 m_normalForceIndex;
224  ndInt32 m_normalForceIndexFlat;
225 } D_GCC_NEWTON_ALIGN_32;
226 
227 D_MSV_NEWTON_ALIGN_32
228 class ndConstraint: public ndContainersFreeListAlloc<ndConstraint>
229 {
230  public:
231  // add some reflexion to the classes
232  D_CLASS_REFLECTION(ndConstraint);
233 
234  virtual ~ndConstraint();
235 
236  virtual ndContact* GetAsContact();
237  virtual ndJointBilateralConstraint* GetAsBilateral();
238 
239  bool IsActive() const;
240  void SetActive(bool state);
241  virtual bool IsBilateral() const;
242 
243  virtual ndUnsigned32 GetRowsCount() const = 0;
244  virtual ndBodyKinematic* GetBody0() const;
245  virtual ndBodyKinematic* GetBody1() const;
246  virtual void JacobianDerivative(ndConstraintDescritor& desc) = 0;
247  virtual void JointAccelerations(ndJointAccelerationDecriptor* const desc) = 0;
248 
249  virtual void DebugJoint(ndConstraintDebugCallback&) const;
250  void InitPointParam(ndPointParam& param, const ndVector& p0Global, const ndVector& p1Global) const;
251 
252  ndInt32 m_rowCount;
253  ndInt32 m_rowStart;
254  ndUnsigned8 m_active;
255  ndUnsigned8 m_fence0;
256  ndUnsigned8 m_fence1;
257  ndUnsigned8 m_resting; // this should be identical to m_fence0, should be removed.
258  ndUnsigned8 m_isInSkeletonLoop;
259 
260  protected:
261  ndConstraint();
262 } D_GCC_NEWTON_ALIGN_32 ;
263 
264 inline ndConstraint::~ndConstraint()
265 {
266 }
267 
268 inline ndContact* ndConstraint::GetAsContact()
269 {
270  return nullptr;
271 }
272 
273 inline ndJointBilateralConstraint* ndConstraint::GetAsBilateral()
274 {
275  return nullptr;
276 }
277 
278 inline bool ndConstraint::IsActive() const
279 {
280  return m_active ? true : false;
281 }
282 
283 inline void ndConstraint::SetActive(bool state)
284 {
285  m_active = ndUnsigned8(state ? 1 : 0);
286 }
287 
288 inline bool ndConstraint::IsBilateral() const
289 {
290  return false;
291 }
292 
293 inline ndBodyKinematic* ndConstraint::GetBody0() const
294 {
295  return nullptr;
296 }
297 
298 inline ndBodyKinematic* ndConstraint::GetBody1() const
299 {
300  return nullptr;
301 }
302 
303 inline void ndConstraint::DebugJoint(ndConstraintDebugCallback&) const
304 {
305 }
306 
307 #endif
308 
ndClassAlloc
Base class for providing memory allocation for all other engine classes.
Definition: ndClassAlloc.h:30
ndBilateralBounds
Definition: ndConstraint.h:169
ndJacobianPair
Definition: ndConstraint.h:107
ndBody
Definition: ndBody.h:43
ndContact
Definition: ndContact.h:97
ndConstraintDescritor
Definition: ndConstraint.h:179
ndJointBilateralConstraint
Definition: ndJointBilateralConstraint.h:53
ndRightHandSide
Definition: ndConstraint.h:205
ndForceImpactPair
Definition: ndConstraint.h:114
ndConstraintDebugCallback
Definition: ndConstraint.h:42
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndMatrix
Definition: ndMatrix.h:42
ndJacobian
Definition: ndConstraint.h:99
ndConstraint
Definition: ndConstraint.h:229
ndContainersFreeListAlloc
Definition: ndContainersAlloc.h:60
ndJointAccelerationDecriptor
Definition: ndConstraint.h:158
ndLeftHandSide
Definition: ndConstraint.h:197
ndVector
Definition: ndVectorArmNeon.h:41
ndPointParam
Definition: ndConstraint.h:89