22 #ifndef __ND_CONSTRAINT_H__
23 #define __ND_CONSTRAINT_H__
25 #include "ndCollisionStdafx.h"
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)
46 m_debugScale = ndFloat32(1.0f);
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;
56 virtual void SetScale(ndFloat32 scale)
61 virtual ndFloat32 GetScale()
const
66 virtual void DrawFrame(
const ndMatrix& matrix, ndFloat32 intensity = ndFloat32 (1.0f))
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)));
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)));
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)));
78 virtual void DrawArrow(
const ndMatrix& matrix,
const ndVector& color, ndFloat32 length)
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);
84 ndFloat32 m_debugScale;
85 } D_GCC_NEWTON_ALIGN_32;
95 } D_GCC_NEWTON_ALIGN_32;
103 } D_GCC_NEWTON_ALIGN_32;
105 D_MSV_NEWTON_ALIGN_32
111 } D_GCC_NEWTON_ALIGN_32;
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)
122 m_initialGuess[i] = ndFloat32(ndFloat32(0.0f));
126 void Push(ndFloat32 val)
128 for (ndInt32 i = 1; i < ndInt32(
sizeof(m_initialGuess) /
sizeof(m_initialGuess[0])); ++i)
130 m_initialGuess[i - 1] = m_initialGuess[i];
132 m_initialGuess[
sizeof(m_initialGuess) /
sizeof(m_initialGuess[0]) - 1] = val;
135 ndFloat32 GetInitialGuess()
const
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)
142 ndFloat32 mag = ndAbs(m_initialGuess[i]);
146 value = m_initialGuess[i];
154 ndFloat32 m_initialGuess[4];
161 ndFloat32 m_timestep;
162 ndFloat32 m_invTimestep;
163 ndFloat32 m_firstPassCoefFlag;
174 ndInt32 m_normalIndex;
177 D_MSV_NEWTON_ALIGN_32
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;
193 } D_GCC_NEWTON_ALIGN_32;
195 D_MSV_NEWTON_ALIGN_32
201 } D_GCC_NEWTON_ALIGN_32;
203 D_MSV_NEWTON_ALIGN_32
208 ndFloat32 m_diagDamp;
209 ndFloat32 m_invJinvMJt;
210 ndFloat32 m_coordenateAccel;
212 ndFloat32 m_lowerBoundFrictionCoefficent;
213 ndFloat32 m_upperBoundFrictionCoefficent;
214 ndFloat32 m_deltaAccel;
215 ndFloat32 m_restitution;
217 ndFloat32 m_maxImpact;
218 ndFloat32 m_penetration;
219 ndFloat32 m_diagonalRegularizer;
220 ndFloat32 m_penetrationStiffness;
223 ndInt32 m_normalForceIndex;
224 ndInt32 m_normalForceIndexFlat;
225 } D_GCC_NEWTON_ALIGN_32;
227 D_MSV_NEWTON_ALIGN_32
239 bool IsActive()
const;
240 void SetActive(
bool state);
241 virtual bool IsBilateral()
const;
243 virtual ndUnsigned32 GetRowsCount()
const = 0;
254 ndUnsigned8 m_active;
255 ndUnsigned8 m_fence0;
256 ndUnsigned8 m_fence1;
257 ndUnsigned8 m_resting;
258 ndUnsigned8 m_isInSkeletonLoop;
262 } D_GCC_NEWTON_ALIGN_32 ;
264 inline ndConstraint::~ndConstraint()
268 inline ndContact* ndConstraint::GetAsContact()
278 inline bool ndConstraint::IsActive()
const
280 return m_active ? true :
false;
283 inline void ndConstraint::SetActive(
bool state)
285 m_active = ndUnsigned8(state ? 1 : 0);
288 inline bool ndConstraint::IsBilateral()
const