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