Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Featherstone / btMultiBodyConstraint.cpp
1 #include "btMultiBodyConstraint.h"
2 #include "BulletDynamics/Dynamics/btRigidBody.h"
3 #include "btMultiBodyPoint2Point.h"                             //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
4
5
6
7 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
8         :m_bodyA(bodyA),
9         m_bodyB(bodyB),
10         m_linkA(linkA),
11         m_linkB(linkB),
12         m_numRows(numRows),
13         m_jacSizeA(0),
14         m_jacSizeBoth(0),
15         m_isUnilateral(isUnilateral),
16         m_numDofsFinalized(-1),
17         m_maxAppliedImpulse(100)
18 {
19
20 }
21
22 void btMultiBodyConstraint::updateJacobianSizes()
23 {
24     if(m_bodyA)
25         {
26                 m_jacSizeA = (6 + m_bodyA->getNumDofs());
27         }
28
29         if(m_bodyB)
30         {
31                 m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
32         }
33         else
34                 m_jacSizeBoth = m_jacSizeA;
35 }
36
37 void btMultiBodyConstraint::allocateJacobiansMultiDof()
38 {
39         updateJacobianSizes();
40
41         m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
42         m_data.resize((2 + m_jacSizeBoth) * m_numRows);
43 }
44
45 btMultiBodyConstraint::~btMultiBodyConstraint()
46 {
47 }
48
49 void    btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
50 {
51         for (int i = 0; i < ndof; ++i)
52                 data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
53 }
54
55 btScalar btMultiBodyConstraint::fillMultiBodyConstraint(        btMultiBodySolverConstraint& solverConstraint,
56                                                                                                                         btMultiBodyJacobianData& data,
57                                                                                                                         btScalar* jacOrgA, btScalar* jacOrgB,
58                                                                                                                         const btVector3& contactNormalOnB,
59                                                                                                                         const btVector3& posAworld, const btVector3& posBworld,
60                                                                                                                         btScalar posError,
61                                                                                                                         const btContactSolverInfo& infoGlobal,
62                                                                                                                         btScalar lowerLimit, btScalar upperLimit,
63                                                                                                                         btScalar relaxation,
64                                                                                                                         bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
65 {
66
67
68         solverConstraint.m_multiBodyA = m_bodyA;
69         solverConstraint.m_multiBodyB = m_bodyB;
70         solverConstraint.m_linkA = m_linkA;
71         solverConstraint.m_linkB = m_linkB;
72
73         btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
74         btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
75
76         btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
77         btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
78
79         btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
80         btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
81
82         btVector3 rel_pos1, rel_pos2;                           //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
83         if (bodyA)
84                 rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
85         if (bodyB)
86                 rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
87
88         if (multiBodyA)
89         {
90                 if (solverConstraint.m_linkA<0)
91                 {
92                         rel_pos1 = posAworld - multiBodyA->getBasePos();
93                 } else
94                 {
95                         rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
96                 }
97
98                 const int ndofA  = multiBodyA->getNumDofs() + 6;
99
100                 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
101
102                 if (solverConstraint.m_deltaVelAindex <0)
103                 {
104                         solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
105                         multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
106                         data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
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, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
126                 }
127
128                 //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)
129                 //resize..
130                 data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);               //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
131                 btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
132                 btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
133                 //determine..
134                 multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
135
136                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
137                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
138                 solverConstraint.m_contactNormal1 = contactNormalOnB;
139         }
140         else //if(rb0)
141         {
142                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
143                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
144                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
145                 solverConstraint.m_contactNormal1 = contactNormalOnB;
146         }
147
148         if (multiBodyB)
149         {
150                 if (solverConstraint.m_linkB<0)
151                 {
152                         rel_pos2 = posBworld - multiBodyB->getBasePos();
153                 } else
154                 {
155                         rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
156                 }
157
158                 const int ndofB  = multiBodyB->getNumDofs() + 6;
159
160                 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
161                 if (solverConstraint.m_deltaVelBindex <0)
162                 {
163                         solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
164                         multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
165                         data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
166                 }
167
168                 //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
169                 //resize..
170                 solverConstraint.m_jacBindex = data.m_jacobians.size();
171                 data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
172                 //copy/determine..
173                 if(jacOrgB)
174                 {
175                         for (int i=0;i<ndofB;i++)
176                                 data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
177                 }
178                 else
179                 {
180                         multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
181                 }
182
183                 //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)
184                 //resize..
185                 data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
186                 btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
187                 btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
188                 //determine..
189                 multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
190
191                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
192                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
193                 solverConstraint.m_contactNormal2 = -contactNormalOnB;
194
195         }
196         else //if(rb1)
197         {
198                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
199                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
200                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
201                 solverConstraint.m_contactNormal2 = -contactNormalOnB;
202         }
203         {
204
205                 btVector3 vec;
206                 btScalar denom0 = 0.f;
207                 btScalar denom1 = 0.f;
208                 btScalar* jacB = 0;
209                 btScalar* jacA = 0;
210                 btScalar* deltaVelA = 0;
211                 btScalar* deltaVelB = 0;
212                 int ndofA  = 0;
213                 //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
214                 if (multiBodyA)
215                 {
216                         ndofA = multiBodyA->getNumDofs() + 6;
217                         jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
218                         deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
219                         for (int i = 0; i < ndofA; ++i)
220                         {
221                                 btScalar j = jacA[i] ;
222                                 btScalar l = deltaVelA[i];
223                                 denom0 += j*l;
224                         }
225                 }
226                 else if(rb0)
227                 {
228                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
229                         denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
230                 }
231                 //
232                 if (multiBodyB)
233                 {
234                         const int ndofB = multiBodyB->getNumDofs() + 6;
235                         jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
236                         deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
237                         for (int i = 0; i < ndofB; ++i)
238                         {
239                                 btScalar j = jacB[i] ;
240                                 btScalar l = deltaVelB[i];
241                                 denom1 += j*l;
242                         }
243
244                 }
245                 else if(rb1)
246                 {
247                         vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
248                         denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
249                 }
250
251                 //
252                 btScalar d = denom0+denom1;
253                 if (d>SIMD_EPSILON)
254                 {
255                         solverConstraint.m_jacDiagABInv = relaxation/(d);
256                 }
257                 else
258                 {
259                 //disable the constraint row to handle singularity/redundant constraint
260                         solverConstraint.m_jacDiagABInv  = 0.f;
261                 }
262         }
263
264
265         //compute rhs and remaining solverConstraint fields
266         btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
267
268         btScalar rel_vel = 0.f;
269         int ndofA  = 0;
270         int ndofB  = 0;
271         {
272                 btVector3 vel1,vel2;
273                 if (multiBodyA)
274                 {
275                         ndofA = multiBodyA->getNumDofs() + 6;
276                         btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
277                         for (int i = 0; i < ndofA ; ++i)
278                                 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
279                 }
280                 else if(rb0)
281                 {
282                         rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
283                 }
284                 if (multiBodyB)
285                 {
286                         ndofB = multiBodyB->getNumDofs() + 6;
287                         btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
288                         for (int i = 0; i < ndofB ; ++i)
289                                 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
290
291                 }
292                 else if(rb1)
293                 {
294                         rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
295                 }
296
297                 solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
298         }
299
300
301         ///warm starting (or zero if disabled)
302         /*
303         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
304         {
305                 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
306
307                 if (solverConstraint.m_appliedImpulse)
308                 {
309                         if (multiBodyA)
310                         {
311                                 btScalar impulse = solverConstraint.m_appliedImpulse;
312                                 btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
313                                 multiBodyA->applyDeltaVee(deltaV,impulse);
314                                 applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
315                         } else
316                         {
317                                 if (rb0)
318                                         bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
319                         }
320                         if (multiBodyB)
321                         {
322                                 btScalar impulse = solverConstraint.m_appliedImpulse;
323                                 btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
324                                 multiBodyB->applyDeltaVee(deltaV,impulse);
325                                 applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
326                         } else
327                         {
328                                 if (rb1)
329                                         bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
330                         }
331                 }
332         } else
333         */
334
335         solverConstraint.m_appliedImpulse = 0.f;
336         solverConstraint.m_appliedPushImpulse = 0.f;
337
338         {
339
340                 btScalar positionalError = 0.f;
341                 btScalar        velocityError = desiredVelocity - rel_vel;// * damping;
342
343
344                 btScalar erp = infoGlobal.m_erp2;
345                 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
346                 {
347                         erp = infoGlobal.m_erp;
348                 }
349
350                 positionalError = -penetration * erp/infoGlobal.m_timeStep;
351
352                 btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
353                 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
354
355                 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
356                 {
357                         //combine position and velocity into rhs
358                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
359                         solverConstraint.m_rhsPenetration = 0.f;
360
361                 } else
362                 {
363                         //split position and velocity into rhs and m_rhsPenetration
364                         solverConstraint.m_rhs = velocityImpulse;
365                         solverConstraint.m_rhsPenetration = penetrationImpulse;
366                 }
367
368                 solverConstraint.m_cfm = 0.f;
369                 solverConstraint.m_lowerLimit = lowerLimit;
370                 solverConstraint.m_upperLimit = upperLimit;
371         }
372
373         return rel_vel;
374
375 }