1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 17 #include "btMultiBodyConstraintSolver.h" 18 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" 19 #include "btMultiBodyLinkCollider.h" 20 21 #include "BulletDynamics/ConstraintSolver/btSolverBody.h" 22 #include "btMultiBodyConstraint.h" 23 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" 24 25 #include "LinearMath/btQuickprof.h" 26 27 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 28 { 29 btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); 30 31 //solve featherstone non-contact constraints 32 33 //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); 34 for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) 35 { 36 btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; 37 //if (iteration < constraint.m_overrideNumSolverIterations) 38 //resolveSingleConstraintRowGenericMultiBody(constraint); 39 resolveSingleConstraintRowGeneric(constraint); 40 if(constraint.m_multiBodyA) 41 constraint.m_multiBodyA->setPosUpdated(false); 42 if(constraint.m_multiBodyB) 43 constraint.m_multiBodyB->setPosUpdated(false); 44 } 45 46 //solve featherstone normal contact 47 for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) 48 { 49 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; 50 if (iteration < infoGlobal.m_numIterations) 51 resolveSingleConstraintRowGeneric(constraint); 52 53 if(constraint.m_multiBodyA) 54 constraint.m_multiBodyA->setPosUpdated(false); 55 if(constraint.m_multiBodyB) 56 constraint.m_multiBodyB->setPosUpdated(false); 57 } 58 59 //solve featherstone frictional contact 60 61 for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) 62 { 63 if (iteration < infoGlobal.m_numIterations) 64 { 65 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; 66 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; 67 //adjust friction limits here 68 if (totalImpulse>btScalar(0)) 69 { 70 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); 71 frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; 72 resolveSingleConstraintRowGeneric(frictionConstraint); 73 74 if(frictionConstraint.m_multiBodyA) 75 frictionConstraint.m_multiBodyA->setPosUpdated(false); 76 if(frictionConstraint.m_multiBodyB) 77 frictionConstraint.m_multiBodyB->setPosUpdated(false); 78 } 79 } 80 } 81 return val; 82 } 83 84 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 85 { 86 m_multiBodyNonContactConstraints.resize(0); 87 m_multiBodyNormalContactConstraints.resize(0); 88 m_multiBodyFrictionContactConstraints.resize(0); 89 m_data.m_jacobians.resize(0); 90 m_data.m_deltaVelocitiesUnitImpulse.resize(0); 91 m_data.m_deltaVelocities.resize(0); 92 93 for (int i=0;i<numBodies;i++) 94 { 95 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); 96 if (fcA) 97 { 98 fcA->m_multiBody->setCompanionId(-1); 99 } 100 } 101 102 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); 103 104 return val; 105 } 106 107 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) 108 { 109 for (int i = 0; i < ndof; ++i) 110 m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; 111 } 112 113 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) 114 { 115 116 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 117 btScalar deltaVelADotn=0; 118 btScalar deltaVelBDotn=0; 119 btSolverBody* bodyA = 0; 120 btSolverBody* bodyB = 0; 121 int ndofA=0; 122 int ndofB=0; 123 124 if (c.m_multiBodyA) 125 { 126 ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; 127 for (int i = 0; i < ndofA; ++i) 128 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; 129 } else if(c.m_solverBodyIdA >= 0) 130 { 131 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; 132 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); 133 } 134 135 if (c.m_multiBodyB) 136 { 137 ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; 138 for (int i = 0; i < ndofB; ++i) 139 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; 140 } else if(c.m_solverBodyIdB >= 0) 141 { 142 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; 143 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); 144 } 145 146 147 deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom 148 deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; 149 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 150 151 if (sum < c.m_lowerLimit) 152 { 153 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; 154 c.m_appliedImpulse = c.m_lowerLimit; 155 } 156 else if (sum > c.m_upperLimit) 157 { 158 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; 159 c.m_appliedImpulse = c.m_upperLimit; 160 } 161 else 162 { 163 c.m_appliedImpulse = sum; 164 } 165 166 if (c.m_multiBodyA) 167 { 168 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); 169 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 170 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations 171 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity 172 if(c.m_multiBodyA->isMultiDof()) 173 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); 174 else 175 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); 176 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 177 } else if(c.m_solverBodyIdA >= 0) 178 { 179 bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 180 181 } 182 if (c.m_multiBodyB) 183 { 184 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); 185 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 186 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations 187 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity 188 if(c.m_multiBodyB->isMultiDof()) 189 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); 190 else 191 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); 192 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 193 } else if(c.m_solverBodyIdB >= 0) 194 { 195 bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 196 } 197 198 } 199 200 201 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) 202 { 203 204 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 205 btScalar deltaVelADotn=0; 206 btScalar deltaVelBDotn=0; 207 int ndofA=0; 208 int ndofB=0; 209 210 if (c.m_multiBodyA) 211 { 212 ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; 213 for (int i = 0; i < ndofA; ++i) 214 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; 215 } 216 217 if (c.m_multiBodyB) 218 { 219 ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; 220 for (int i = 0; i < ndofB; ++i) 221 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; 222 } 223 224 225 deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom 226 deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; 227 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 228 229 if (sum < c.m_lowerLimit) 230 { 231 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; 232 c.m_appliedImpulse = c.m_lowerLimit; 233 } 234 else if (sum > c.m_upperLimit) 235 { 236 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; 237 c.m_appliedImpulse = c.m_upperLimit; 238 } 239 else 240 { 241 c.m_appliedImpulse = sum; 242 } 243 244 if (c.m_multiBodyA) 245 { 246 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); 247 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); 248 } 249 if (c.m_multiBodyB) 250 { 251 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); 252 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); 253 } 254 } 255 256 257 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 258 const btVector3& contactNormal, 259 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, 260 btScalar& relaxation, 261 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) 262 { 263 264 BT_PROFILE("setupMultiBodyContactConstraint"); 265 btVector3 rel_pos1; 266 btVector3 rel_pos2; 267 268 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; 269 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; 270 271 const btVector3& pos1 = cp.getPositionWorldOnA(); 272 const btVector3& pos2 = cp.getPositionWorldOnB(); 273 274 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; 275 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; 276 277 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; 278 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; 279 280 if (bodyA) 281 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 282 if (bodyB) 283 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); 284 285 relaxation = 1.f; 286 287 288 289 290 if (multiBodyA) 291 { 292 if (solverConstraint.m_linkA<0) 293 { 294 rel_pos1 = pos1 - multiBodyA->getBasePos(); 295 } else 296 { 297 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); 298 } 299 const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; 300 301 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); 302 303 if (solverConstraint.m_deltaVelAindex <0) 304 { 305 solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); 306 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); 307 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); 308 } else 309 { 310 btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); 311 } 312 313 solverConstraint.m_jacAindex = m_data.m_jacobians.size(); 314 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); 315 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); 316 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); 317 318 btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; 319 if(multiBodyA->isMultiDof()) 320 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); 321 else 322 multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); 323 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; 324 if(multiBodyA->isMultiDof()) 325 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); 326 else 327 multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); 328 329 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); 330 solverConstraint.m_relpos1CrossNormal = torqueAxis0; 331 solverConstraint.m_contactNormal1 = contactNormal; 332 } else 333 { 334 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); 335 solverConstraint.m_relpos1CrossNormal = torqueAxis0; 336 solverConstraint.m_contactNormal1 = contactNormal; 337 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); 338 } 339 340 341 342 if (multiBodyB) 343 { 344 if (solverConstraint.m_linkB<0) 345 { 346 rel_pos2 = pos2 - multiBodyB->getBasePos(); 347 } else 348 { 349 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); 350 } 351 352 const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; 353 354 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); 355 if (solverConstraint.m_deltaVelBindex <0) 356 { 357 solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); 358 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); 359 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); 360 } 361 362 solverConstraint.m_jacBindex = m_data.m_jacobians.size(); 363 364 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); 365 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); 366 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); 367 368 if(multiBodyB->isMultiDof()) 369 multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); 370 else 371 multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); 372 if(multiBodyB->isMultiDof()) 373 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); 374 else 375 multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); 376 377 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); 378 solverConstraint.m_relpos2CrossNormal = -torqueAxis1; 379 solverConstraint.m_contactNormal2 = -contactNormal; 380 381 } else 382 { 383 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); 384 solverConstraint.m_relpos2CrossNormal = -torqueAxis1; 385 solverConstraint.m_contactNormal2 = -contactNormal; 386 387 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); 388 } 389 390 { 391 392 btVector3 vec; 393 btScalar denom0 = 0.f; 394 btScalar denom1 = 0.f; 395 btScalar* jacB = 0; 396 btScalar* jacA = 0; 397 btScalar* lambdaA =0; 398 btScalar* lambdaB =0; 399 int ndofA = 0; 400 if (multiBodyA) 401 { 402 ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; 403 jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; 404 lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; 405 for (int i = 0; i < ndofA; ++i) 406 { 407 btScalar j = jacA[i] ; 408 btScalar l =lambdaA[i]; 409 denom0 += j*l; 410 } 411 } else 412 { 413 if (rb0) 414 { 415 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); 416 denom0 = rb0->getInvMass() + contactNormal.dot(vec); 417 } 418 } 419 if (multiBodyB) 420 { 421 const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; 422 jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; 423 lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; 424 for (int i = 0; i < ndofB; ++i) 425 { 426 btScalar j = jacB[i] ; 427 btScalar l =lambdaB[i]; 428 denom1 += j*l; 429 } 430 431 } else 432 { 433 if (rb1) 434 { 435 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); 436 denom1 = rb1->getInvMass() + contactNormal.dot(vec); 437 } 438 } 439 440 441 442 btScalar d = denom0+denom1; 443 if (d>SIMD_EPSILON) 444 { 445 solverConstraint.m_jacDiagABInv = relaxation/(d); 446 } else 447 { 448 //disable the constraint row to handle singularity/redundant constraint 449 solverConstraint.m_jacDiagABInv = 0.f; 450 } 451 452 } 453 454 455 //compute rhs and remaining solverConstraint fields 456 457 458 459 btScalar restitution = 0.f; 460 btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; 461 462 btScalar rel_vel = 0.f; 463 int ndofA = 0; 464 int ndofB = 0; 465 { 466 467 btVector3 vel1,vel2; 468 if (multiBodyA) 469 { 470 ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; 471 btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; 472 for (int i = 0; i < ndofA ; ++i) 473 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; 474 } else 475 { 476 if (rb0) 477 { 478 rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); 479 } 480 } 481 if (multiBodyB) 482 { 483 ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; 484 btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; 485 for (int i = 0; i < ndofB ; ++i) 486 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; 487 488 } else 489 { 490 if (rb1) 491 { 492 rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); 493 } 494 } 495 496 solverConstraint.m_friction = cp.m_combinedFriction; 497 498 if(!isFriction) 499 { 500 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); 501 if (restitution <= btScalar(0.)) 502 { 503 restitution = 0.f; 504 } 505 } 506 } 507 508 509 ///warm starting (or zero if disabled) 510 //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) 511 if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 512 { 513 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; 514 515 if (solverConstraint.m_appliedImpulse) 516 { 517 if (multiBodyA) 518 { 519 btScalar impulse = solverConstraint.m_appliedImpulse; 520 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; 521 if(multiBodyA->isMultiDof()) 522 multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); 523 else 524 multiBodyA->applyDeltaVee(deltaV,impulse); 525 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); 526 } else 527 { 528 if (rb0) 529 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); 530 } 531 if (multiBodyB) 532 { 533 btScalar impulse = solverConstraint.m_appliedImpulse; 534 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; 535 if(multiBodyB->isMultiDof()) 536 multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); 537 else 538 multiBodyB->applyDeltaVee(deltaV,impulse); 539 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); 540 } else 541 { 542 if (rb1) 543 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); 544 } 545 } 546 } else 547 { 548 solverConstraint.m_appliedImpulse = 0.f; 549 } 550 551 solverConstraint.m_appliedPushImpulse = 0.f; 552 553 { 554 555 btScalar positionalError = 0.f; 556 btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction 557 558 btScalar erp = infoGlobal.m_erp2; 559 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 560 { 561 erp = infoGlobal.m_erp; 562 } 563 564 if (penetration>0) 565 { 566 positionalError = 0; 567 velocityError -= penetration / infoGlobal.m_timeStep; 568 569 } else 570 { 571 positionalError = -penetration * erp/infoGlobal.m_timeStep; 572 } 573 574 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 575 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 576 577 if(!isFriction) 578 { 579 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 580 { 581 //combine position and velocity into rhs 582 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 583 solverConstraint.m_rhsPenetration = 0.f; 584 585 } else 586 { 587 //split position and velocity into rhs and m_rhsPenetration 588 solverConstraint.m_rhs = velocityImpulse; 589 solverConstraint.m_rhsPenetration = penetrationImpulse; 590 } 591 592 solverConstraint.m_lowerLimit = 0; 593 solverConstraint.m_upperLimit = 1e10f; 594 } 595 else 596 { 597 solverConstraint.m_rhs = velocityImpulse; 598 solverConstraint.m_rhsPenetration = 0.f; 599 solverConstraint.m_lowerLimit = -solverConstraint.m_friction; 600 solverConstraint.m_upperLimit = solverConstraint.m_friction; 601 } 602 603 solverConstraint.m_cfm = 0.f; //why not use cfmSlip? 604 } 605 606 } 607 608 609 610 611 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) 612 { 613 BT_PROFILE("addMultiBodyFrictionConstraint"); 614 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); 615 solverConstraint.m_orgConstraint = 0; 616 solverConstraint.m_orgDofIndex = -1; 617 618 solverConstraint.m_frictionIndex = frictionIndex; 619 bool isFriction = true; 620 621 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); 622 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); 623 624 btMultiBody* mbA = fcA? fcA->m_multiBody : 0; 625 btMultiBody* mbB = fcB? fcB->m_multiBody : 0; 626 627 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); 628 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); 629 630 solverConstraint.m_solverBodyIdA = solverBodyIdA; 631 solverConstraint.m_solverBodyIdB = solverBodyIdB; 632 solverConstraint.m_multiBodyA = mbA; 633 if (mbA) 634 solverConstraint.m_linkA = fcA->m_link; 635 636 solverConstraint.m_multiBodyB = mbB; 637 if (mbB) 638 solverConstraint.m_linkB = fcB->m_link; 639 640 solverConstraint.m_originalContactPoint = &cp; 641 642 setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); 643 return solverConstraint; 644 } 645 646 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) 647 { 648 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); 649 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); 650 651 btMultiBody* mbA = fcA? fcA->m_multiBody : 0; 652 btMultiBody* mbB = fcB? fcB->m_multiBody : 0; 653 654 btCollisionObject* colObj0=0,*colObj1=0; 655 656 colObj0 = (btCollisionObject*)manifold->getBody0(); 657 colObj1 = (btCollisionObject*)manifold->getBody1(); 658 659 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); 660 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); 661 662 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; 663 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; 664 665 666 ///avoid collision response between two static objects 667 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) 668 // return; 669 670 671 672 for (int j=0;j<manifold->getNumContacts();j++) 673 { 674 675 btManifoldPoint& cp = manifold->getContactPoint(j); 676 677 if (cp.getDistance() <= manifold->getContactProcessingThreshold()) 678 { 679 680 btScalar relaxation; 681 682 int frictionIndex = m_multiBodyNormalContactConstraints.size(); 683 684 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); 685 686 // btRigidBody* rb0 = btRigidBody::upcast(colObj0); 687 // btRigidBody* rb1 = btRigidBody::upcast(colObj1); 688 solverConstraint.m_orgConstraint = 0; 689 solverConstraint.m_orgDofIndex = -1; 690 solverConstraint.m_solverBodyIdA = solverBodyIdA; 691 solverConstraint.m_solverBodyIdB = solverBodyIdB; 692 solverConstraint.m_multiBodyA = mbA; 693 if (mbA) 694 solverConstraint.m_linkA = fcA->m_link; 695 696 solverConstraint.m_multiBodyB = mbB; 697 if (mbB) 698 solverConstraint.m_linkB = fcB->m_link; 699 700 solverConstraint.m_originalContactPoint = &cp; 701 702 bool isFriction = false; 703 setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); 704 705 // const btVector3& pos1 = cp.getPositionWorldOnA(); 706 // const btVector3& pos2 = cp.getPositionWorldOnB(); 707 708 /////setup the friction constraints 709 #define ENABLE_FRICTION 710 #ifdef ENABLE_FRICTION 711 solverConstraint.m_frictionIndex = frictionIndex; 712 #if ROLLING_FRICTION 713 int rollingFriction=1; 714 btVector3 angVelA(0,0,0),angVelB(0,0,0); 715 if (rb0) 716 angVelA = rb0->getAngularVelocity(); 717 if (rb1) 718 angVelB = rb1->getAngularVelocity(); 719 btVector3 relAngVel = angVelB-angVelA; 720 721 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) 722 { 723 //only a single rollingFriction per manifold 724 rollingFriction--; 725 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) 726 { 727 relAngVel.normalize(); 728 applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 729 applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 730 if (relAngVel.length()>0.001) 731 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 732 733 } else 734 { 735 addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 736 btVector3 axis0,axis1; 737 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); 738 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 739 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 740 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 741 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 742 if (axis0.length()>0.001) 743 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 744 if (axis1.length()>0.001) 745 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 746 747 } 748 } 749 #endif //ROLLING_FRICTION 750 751 ///Bullet has several options to set the friction directions 752 ///By default, each contact has only a single friction direction that is recomputed automatically very frame 753 ///based on the relative linear velocity. 754 ///If the relative velocity it zero, it will automatically compute a friction direction. 755 756 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. 757 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. 758 /// 759 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. 760 /// 761 ///The user can manually override the friction directions for certain contacts using a contact callback, 762 ///and set the cp.m_lateralFrictionInitialized to true 763 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) 764 ///this will give a conveyor belt effect 765 /// 766 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) 767 {/* 768 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; 769 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); 770 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) 771 { 772 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); 773 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 774 { 775 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); 776 cp.m_lateralFrictionDir2.normalize();//?? 777 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 778 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 779 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 780 781 } 782 783 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 784 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 785 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 786 787 } else 788 */ 789 { 790 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); 791 792 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 793 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 794 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); 795 796 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 797 { 798 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 799 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 800 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); 801 } 802 803 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) 804 { 805 cp.m_lateralFrictionInitialized = true; 806 } 807 } 808 809 } else 810 { 811 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); 812 813 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 814 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); 815 816 //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); 817 //todo: 818 solverConstraint.m_appliedImpulse = 0.f; 819 solverConstraint.m_appliedPushImpulse = 0.f; 820 } 821 822 823 #endif //ENABLE_FRICTION 824 825 } 826 } 827 } 828 829 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) 830 { 831 //btPersistentManifold* manifold = 0; 832 833 for (int i=0;i<numManifolds;i++) 834 { 835 btPersistentManifold* manifold= manifoldPtr[i]; 836 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); 837 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); 838 if (!fcA && !fcB) 839 { 840 //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case 841 convertContact(manifold,infoGlobal); 842 } else 843 { 844 convertMultiBodyContact(manifold,infoGlobal); 845 } 846 } 847 848 //also convert the multibody constraints, if any 849 850 851 for (int i=0;i<m_tmpNumMultiBodyConstraints;i++) 852 { 853 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i]; 854 m_data.m_solverBodyPool = &m_tmpSolverBodyPool; 855 m_data.m_fixedBodyId = m_fixedBodyId; 856 857 c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); 858 } 859 860 } 861 862 863 864 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) 865 { 866 return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); 867 } 868 869 #if 0 870 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse) 871 { 872 if (appliedImpulse!=0 && mb->internalNeedsJointFeedback()) 873 { 874 //todo: get rid of those temporary memory allocations for the joint feedback 875 btAlignedObjectArray<btScalar> forceVector; 876 int numDofsPlusBase = 6+mb->getNumDofs(); 877 forceVector.resize(numDofsPlusBase); 878 for (int i=0;i<numDofsPlusBase;i++) 879 { 880 forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse; 881 } 882 btAlignedObjectArray<btScalar> output; 883 output.resize(numDofsPlusBase); 884 bool applyJointFeedback = true; 885 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback); 886 } 887 } 888 #endif 889 890 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) 891 { 892 #if 1 893 894 //bod->addBaseForce(m_gravity * bod->getBaseMass()); 895 //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); 896 897 if (c.m_orgConstraint) 898 { 899 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); 900 } 901 902 903 if (c.m_multiBodyA) 904 { 905 906 if(c.m_multiBodyA->isMultiDof()) 907 { 908 c.m_multiBodyA->setCompanionId(-1); 909 btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); 910 btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); 911 if (c.m_linkA<0) 912 { 913 c.m_multiBodyA->addBaseConstraintForce(force); 914 c.m_multiBodyA->addBaseConstraintTorque(torque); 915 } else 916 { 917 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); 918 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); 919 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); 920 } 921 } 922 } 923 924 if (c.m_multiBodyB) 925 { 926 if(c.m_multiBodyB->isMultiDof()) 927 { 928 { 929 c.m_multiBodyB->setCompanionId(-1); 930 btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); 931 btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); 932 if (c.m_linkB<0) 933 { 934 c.m_multiBodyB->addBaseConstraintForce(force); 935 c.m_multiBodyB->addBaseConstraintTorque(torque); 936 } else 937 { 938 { 939 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); 940 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); 941 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); 942 } 943 944 } 945 } 946 } 947 } 948 #endif 949 950 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 951 952 if (c.m_multiBodyA) 953 { 954 955 if(c.m_multiBodyA->isMultiDof()) 956 { 957 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); 958 } 959 else 960 { 961 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); 962 } 963 } 964 965 if (c.m_multiBodyB) 966 { 967 if(c.m_multiBodyB->isMultiDof()) 968 { 969 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); 970 } 971 else 972 { 973 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); 974 } 975 } 976 #endif 977 978 979 980 } 981 982 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) 983 { 984 BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); 985 int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); 986 987 988 //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 989 //or as applied force, so we can measure the joint reaction forces easier 990 for (int i=0;i<numPoolConstraints;i++) 991 { 992 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i]; 993 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); 994 995 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); 996 997 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 998 { 999 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); 1000 } 1001 } 1002 1003 1004 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++) 1005 { 1006 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; 1007 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); 1008 } 1009 1010 1011 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 1012 { 1013 BT_PROFILE("warm starting write back"); 1014 for (int j=0;j<numPoolConstraints;j++) 1015 { 1016 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; 1017 btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint; 1018 btAssert(pt); 1019 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; 1020 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; 1021 1022 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); 1023 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1024 { 1025 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; 1026 } 1027 //do a callback here? 1028 } 1029 } 1030 #if 0 1031 //multibody joint feedback 1032 { 1033 BT_PROFILE("multi body joint feedback"); 1034 for (int j=0;j<numPoolConstraints;j++) 1035 { 1036 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; 1037 1038 //apply the joint feedback into all links of the btMultiBody 1039 //todo: double-check the signs of the applied impulse 1040 1041 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof()) 1042 { 1043 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); 1044 } 1045 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) 1046 { 1047 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); 1048 } 1049 #if 0 1050 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof()) 1051 { 1052 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], 1053 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex, 1054 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA, 1055 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); 1056 1057 } 1058 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof()) 1059 { 1060 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], 1061 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex, 1062 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB, 1063 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); 1064 } 1065 1066 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1067 { 1068 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof()) 1069 { 1070 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], 1071 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex, 1072 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA, 1073 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); 1074 } 1075 1076 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof()) 1077 { 1078 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], 1079 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex, 1080 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB, 1081 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); 1082 } 1083 } 1084 #endif 1085 } 1086 1087 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++) 1088 { 1089 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; 1090 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof()) 1091 { 1092 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); 1093 } 1094 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) 1095 { 1096 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); 1097 } 1098 } 1099 } 1100 1101 numPoolConstraints = m_multiBodyNonContactConstraints.size(); 1102 1103 #if 0 1104 //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint 1105 for (int i=0;i<numPoolConstraints;i++) 1106 { 1107 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i]; 1108 1109 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint; 1110 btJointFeedback* fb = constr->getJointFeedback(); 1111 if (fb) 1112 { 1113 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; 1114 fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; 1115 fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; 1116 fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ 1117 1118 } 1119 1120 constr->internalSetAppliedImpulse(c.m_appliedImpulse); 1121 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) 1122 { 1123 constr->setEnabled(false); 1124 } 1125 1126 } 1127 #endif 1128 #endif 1129 1130 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); 1131 } 1132 1133 1134 void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) 1135 { 1136 //printf("solveMultiBodyGroup start\n"); 1137 m_tmpMultiBodyConstraints = multiBodyConstraints; 1138 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; 1139 1140 btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); 1141 1142 m_tmpMultiBodyConstraints = 0; 1143 m_tmpNumMultiBodyConstraints = 0; 1144 1145 1146 } 1147
