Blender  V3.3
btMultiBodyConstraint.cpp
Go to the documentation of this file.
3 #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
4 
5 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
6  : m_bodyA(bodyA),
7  m_bodyB(bodyB),
8  m_linkA(linkA),
9  m_linkB(linkB),
10  m_type(type),
11  m_numRows(numRows),
12  m_jacSizeA(0),
13  m_jacSizeBoth(0),
17 {
18 }
19 
21 {
22  if (m_bodyA)
23  {
24  m_jacSizeA = (6 + m_bodyA->getNumDofs());
25  }
26 
27  if (m_bodyB)
28  {
29  m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
30  }
31  else
33 }
34 
36 {
38 
41 }
42 
44 {
45 }
46 
47 void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
48 {
49  for (int i = 0; i < ndof; ++i)
50  data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
51 }
52 
55  btScalar* jacOrgA, btScalar* jacOrgB,
56  const btVector3& constraintNormalAng,
57  const btVector3& constraintNormalLin,
58  const btVector3& posAworld, const btVector3& posBworld,
59  btScalar posError,
61  btScalar lowerLimit, btScalar upperLimit,
62  bool angConstraint,
63  btScalar relaxation,
64  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
65 {
66  solverConstraint.m_multiBodyA = m_bodyA;
67  solverConstraint.m_multiBodyB = m_bodyB;
68  solverConstraint.m_linkA = m_linkA;
69  solverConstraint.m_linkB = m_linkB;
70 
71  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
72  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
73 
74  btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
75  btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
76 
77  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
78  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
79 
80  btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
81  if (bodyA)
82  rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
83  if (bodyB)
84  rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
85 
86  if (multiBodyA)
87  {
88  if (solverConstraint.m_linkA < 0)
89  {
90  rel_pos1 = posAworld - multiBodyA->getBasePos();
91  }
92  else
93  {
94  rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
95  }
96 
97  const int ndofA = multiBodyA->getNumDofs() + 6;
98 
99  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
100 
101  if (solverConstraint.m_deltaVelAindex < 0)
102  {
103  solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
104  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
105  data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA);
106  }
107  else
108  {
109  btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
110  }
111 
112  //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
113  //resize..
114  solverConstraint.m_jacAindex = data.m_jacobians.size();
115  data.m_jacobians.resize(data.m_jacobians.size() + ndofA);
116  //copy/determine
117  if (jacOrgA)
118  {
119  for (int i = 0; i < ndofA; i++)
120  data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i];
121  }
122  else
123  {
124  btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex];
125  //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
126  multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
127  }
128 
129  //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
130  //resize..
131  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
132  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
133  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
134  //determine..
135  multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
136 
137  btVector3 torqueAxis0;
138  if (angConstraint)
139  {
140  torqueAxis0 = constraintNormalAng;
141  }
142  else
143  {
144  torqueAxis0 = rel_pos1.cross(constraintNormalLin);
145  }
146  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
147  solverConstraint.m_contactNormal1 = constraintNormalLin;
148  }
149  else //if(rb0)
150  {
151  btVector3 torqueAxis0;
152  if (angConstraint)
153  {
154  torqueAxis0 = constraintNormalAng;
155  }
156  else
157  {
158  torqueAxis0 = rel_pos1.cross(constraintNormalLin);
159  }
160  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
161  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
162  solverConstraint.m_contactNormal1 = constraintNormalLin;
163  }
164 
165  if (multiBodyB)
166  {
167  if (solverConstraint.m_linkB < 0)
168  {
169  rel_pos2 = posBworld - multiBodyB->getBasePos();
170  }
171  else
172  {
173  rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
174  }
175 
176  const int ndofB = multiBodyB->getNumDofs() + 6;
177 
178  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
179  if (solverConstraint.m_deltaVelBindex < 0)
180  {
181  solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
182  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
183  data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofB);
184  }
185 
186  //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
187  //resize..
188  solverConstraint.m_jacBindex = data.m_jacobians.size();
189  data.m_jacobians.resize(data.m_jacobians.size() + ndofB);
190  //copy/determine..
191  if (jacOrgB)
192  {
193  for (int i = 0; i < ndofB; i++)
194  data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i];
195  }
196  else
197  {
198  //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
199  multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
200  }
201 
202  //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
203  //resize..
204  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
205  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
206  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
207  //determine..
208  multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
209 
210  btVector3 torqueAxis1;
211  if (angConstraint)
212  {
213  torqueAxis1 = constraintNormalAng;
214  }
215  else
216  {
217  torqueAxis1 = rel_pos2.cross(constraintNormalLin);
218  }
219  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
220  solverConstraint.m_contactNormal2 = -constraintNormalLin;
221  }
222  else //if(rb1)
223  {
224  btVector3 torqueAxis1;
225  if (angConstraint)
226  {
227  torqueAxis1 = constraintNormalAng;
228  }
229  else
230  {
231  torqueAxis1 = rel_pos2.cross(constraintNormalLin);
232  }
233  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
234  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
235  solverConstraint.m_contactNormal2 = -constraintNormalLin;
236  }
237  {
238  btVector3 vec;
239  btScalar denom0 = 0.f;
240  btScalar denom1 = 0.f;
241  btScalar* jacB = 0;
242  btScalar* jacA = 0;
243  btScalar* deltaVelA = 0;
244  btScalar* deltaVelB = 0;
245  int ndofA = 0;
246  //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
247  if (multiBodyA)
248  {
249  ndofA = multiBodyA->getNumDofs() + 6;
250  jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
251  deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
252  for (int i = 0; i < ndofA; ++i)
253  {
254  btScalar j = jacA[i];
255  btScalar l = deltaVelA[i];
256  denom0 += j * l;
257  }
258  }
259  else if (rb0)
260  {
261  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
262  if (angConstraint)
263  {
264  denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
265  }
266  else
267  {
268  denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
269  }
270  }
271  //
272  if (multiBodyB)
273  {
274  const int ndofB = multiBodyB->getNumDofs() + 6;
275  jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
276  deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
277  for (int i = 0; i < ndofB; ++i)
278  {
279  btScalar j = jacB[i];
280  btScalar l = deltaVelB[i];
281  denom1 += j * l;
282  }
283  }
284  else if (rb1)
285  {
286  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
287  if (angConstraint)
288  {
289  denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
290  }
291  else
292  {
293  denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
294  }
295  }
296 
297  //
298  btScalar d = denom0 + denom1;
299  if (d > SIMD_EPSILON)
300  {
301  solverConstraint.m_jacDiagABInv = relaxation / (d);
302  }
303  else
304  {
305  //disable the constraint row to handle singularity/redundant constraint
306  solverConstraint.m_jacDiagABInv = 0.f;
307  }
308  }
309 
310  //compute rhs and remaining solverConstraint fields
311  btScalar penetration = isFriction ? 0 : posError;
312 
313  btScalar rel_vel = 0.f;
314  int ndofA = 0;
315  int ndofB = 0;
316  {
317  btVector3 vel1, vel2;
318  if (multiBodyA)
319  {
320  ndofA = multiBodyA->getNumDofs() + 6;
321  btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
322  for (int i = 0; i < ndofA; ++i)
323  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
324  }
325  else if (rb0)
326  {
327  rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
328  rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
329  }
330  if (multiBodyB)
331  {
332  ndofB = multiBodyB->getNumDofs() + 6;
333  btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
334  for (int i = 0; i < ndofB; ++i)
335  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
336  }
337  else if (rb1)
338  {
339  rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
340  rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
341  }
342 
343  solverConstraint.m_friction = 0.f; //cp.m_combinedFriction;
344  }
345 
346  solverConstraint.m_appliedImpulse = 0.f;
347  solverConstraint.m_appliedPushImpulse = 0.f;
348 
349  {
350  btScalar positionalError = 0.f;
351  btScalar velocityError = desiredVelocity - rel_vel; // * damping;
352 
353  btScalar erp = infoGlobal.m_erp2;
354 
355  //split impulse is not implemented yet for btMultiBody*
356  //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
357  {
358  erp = infoGlobal.m_erp;
359  }
360 
361  positionalError = -penetration * erp / infoGlobal.m_timeStep;
362 
363  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
364  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
365 
366  //split impulse is not implemented yet for btMultiBody*
367 
368  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
369  {
370  //combine position and velocity into rhs
371  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
372  solverConstraint.m_rhsPenetration = 0.f;
373  }
374  /*else
375  {
376  //split position and velocity into rhs and m_rhsPenetration
377  solverConstraint.m_rhs = velocityImpulse;
378  solverConstraint.m_rhsPenetration = penetrationImpulse;
379  }
380  */
381 
382  solverConstraint.m_cfm = 0.f;
383  solverConstraint.m_lowerLimit = lowerLimit;
384  solverConstraint.m_upperLimit = upperLimit;
385  }
386 
387  return rel_vel;
388 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum type
ATTR_WARN_UNUSED_RESULT const BMLoop * l
int m_numDofsFinalized
int m_linkB
int m_jacSizeA
void updateJacobianSizes()
int m_posOffset
bool m_isUnilateral
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
int m_type
btMultiBody * m_bodyB
bool isUnilateral() const
int m_linkA
void allocateJacobiansMultiDof()
virtual ~btMultiBodyConstraint()
int m_jacSizeBoth
int m_numRows
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar m_maxAppliedImpulse
btMultiBodyConstraint
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBody
Definition: btMultiBody.h:51
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:433
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:579
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)