Changeset 8351 for code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
- Timestamp:
- Apr 28, 2011, 7:15:14 AM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
r5781 r8351 13 13 3. This notice may not be removed or altered from any source distribution. 14 14 */ 15 16 17 #include "btContactConstraint.h" 18 #include "BulletDynamics/Dynamics/btRigidBody.h" 19 #include "LinearMath/btVector3.h" 20 #include "btJacobianEntry.h" 21 #include "btContactSolverInfo.h" 22 #include "LinearMath/btMinMax.h" 23 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" 24 25 26 27 btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB) 28 :btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB), 29 m_contactManifold(*contactManifold) 30 { 31 32 } 33 34 btContactConstraint::~btContactConstraint() 35 { 36 37 } 38 39 void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) 40 { 41 m_contactManifold = *contactManifold; 42 } 43 44 void btContactConstraint::getInfo1 (btConstraintInfo1* info) 45 { 46 47 } 48 49 void btContactConstraint::getInfo2 (btConstraintInfo2* info) 50 { 51 52 } 53 54 void btContactConstraint::buildJacobian() 55 { 56 57 } 58 59 60 15 61 16 62 … … 86 132 87 133 88 //response between two dynamic objects with friction89 btScalar resolveSingleCollision(90 btRigidBody& body1,91 btRigidBody& body2,92 btManifoldPoint& contactPoint,93 const btContactSolverInfo& solverInfo)94 {95 134 96 const btVector3& pos1_ = contactPoint.getPositionWorldOnA();97 const btVector3& pos2_ = contactPoint.getPositionWorldOnB();98 const btVector3& normal = contactPoint.m_normalWorldOnB;99 100 //constant over all iterations101 btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();102 btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();103 104 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);105 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);106 btVector3 vel = vel1 - vel2;107 btScalar rel_vel;108 rel_vel = normal.dot(vel);109 110 btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;111 112 // btScalar damping = solverInfo.m_damping ;113 btScalar Kerp = solverInfo.m_erp;114 btScalar Kcor = Kerp *Kfps;115 116 btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;117 btAssert(cpd);118 btScalar distance = cpd->m_penetration;119 btScalar positionalError = Kcor *-distance;120 btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;121 122 btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;123 124 btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;125 126 btScalar normalImpulse = penetrationImpulse+velocityImpulse;127 128 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse129 btScalar oldNormalImpulse = cpd->m_appliedImpulse;130 btScalar sum = oldNormalImpulse + normalImpulse;131 cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;132 133 normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;134 135 #ifdef USE_INTERNAL_APPLY_IMPULSE136 if (body1.getInvMass())137 {138 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);139 }140 if (body2.getInvMass())141 {142 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);143 }144 #else //USE_INTERNAL_APPLY_IMPULSE145 body1.applyImpulse(normal*(normalImpulse), rel_pos1);146 body2.applyImpulse(-normal*(normalImpulse), rel_pos2);147 #endif //USE_INTERNAL_APPLY_IMPULSE148 149 return normalImpulse;150 }151 152 153 btScalar resolveSingleFriction(154 btRigidBody& body1,155 btRigidBody& body2,156 btManifoldPoint& contactPoint,157 const btContactSolverInfo& solverInfo)158 {159 160 (void)solverInfo;161 162 const btVector3& pos1 = contactPoint.getPositionWorldOnA();163 const btVector3& pos2 = contactPoint.getPositionWorldOnB();164 165 btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();166 btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();167 168 btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;169 btAssert(cpd);170 171 btScalar combinedFriction = cpd->m_friction;172 173 btScalar limit = cpd->m_appliedImpulse * combinedFriction;174 175 if (cpd->m_appliedImpulse>btScalar(0.))176 //friction177 {178 //apply friction in the 2 tangential directions179 180 // 1st tangent181 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);182 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);183 btVector3 vel = vel1 - vel2;184 185 btScalar j1,j2;186 187 {188 189 btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);190 191 // calculate j that moves us to zero relative velocity192 j1 = -vrel * cpd->m_jacDiagABInvTangent0;193 btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;194 cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;195 btSetMin(cpd->m_accumulatedTangentImpulse0, limit);196 btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);197 j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;198 199 }200 {201 // 2nd tangent202 203 btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);204 205 // calculate j that moves us to zero relative velocity206 j2 = -vrel * cpd->m_jacDiagABInvTangent1;207 btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;208 cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;209 btSetMin(cpd->m_accumulatedTangentImpulse1, limit);210 btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);211 j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;212 }213 214 #ifdef USE_INTERNAL_APPLY_IMPULSE215 if (body1.getInvMass())216 {217 body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);218 body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);219 }220 if (body2.getInvMass())221 {222 body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);223 body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);224 }225 #else //USE_INTERNAL_APPLY_IMPULSE226 body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);227 body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);228 #endif //USE_INTERNAL_APPLY_IMPULSE229 230 231 }232 return cpd->m_appliedImpulse;233 }234 235 236 btScalar resolveSingleFrictionOriginal(237 btRigidBody& body1,238 btRigidBody& body2,239 btManifoldPoint& contactPoint,240 const btContactSolverInfo& solverInfo);241 242 btScalar resolveSingleFrictionOriginal(243 btRigidBody& body1,244 btRigidBody& body2,245 btManifoldPoint& contactPoint,246 const btContactSolverInfo& solverInfo)247 {248 249 (void)solverInfo;250 251 const btVector3& pos1 = contactPoint.getPositionWorldOnA();252 const btVector3& pos2 = contactPoint.getPositionWorldOnB();253 254 btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();255 btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();256 257 btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;258 btAssert(cpd);259 260 btScalar combinedFriction = cpd->m_friction;261 262 btScalar limit = cpd->m_appliedImpulse * combinedFriction;263 //if (contactPoint.m_appliedImpulse>btScalar(0.))264 //friction265 {266 //apply friction in the 2 tangential directions267 268 {269 // 1st tangent270 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);271 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);272 btVector3 vel = vel1 - vel2;273 274 btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);275 276 // calculate j that moves us to zero relative velocity277 btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;278 btScalar total = cpd->m_accumulatedTangentImpulse0 + j;279 btSetMin(total, limit);280 btSetMax(total, -limit);281 j = total - cpd->m_accumulatedTangentImpulse0;282 cpd->m_accumulatedTangentImpulse0 = total;283 body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);284 body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);285 }286 287 288 {289 // 2nd tangent290 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);291 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);292 btVector3 vel = vel1 - vel2;293 294 btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);295 296 // calculate j that moves us to zero relative velocity297 btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;298 btScalar total = cpd->m_accumulatedTangentImpulse1 + j;299 btSetMin(total, limit);300 btSetMax(total, -limit);301 j = total - cpd->m_accumulatedTangentImpulse1;302 cpd->m_accumulatedTangentImpulse1 = total;303 body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);304 body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);305 }306 }307 return cpd->m_appliedImpulse;308 }309 310 311 //velocity + friction312 //response between two dynamic objects with friction313 btScalar resolveSingleCollisionCombined(314 btRigidBody& body1,315 btRigidBody& body2,316 btManifoldPoint& contactPoint,317 const btContactSolverInfo& solverInfo)318 {319 320 const btVector3& pos1 = contactPoint.getPositionWorldOnA();321 const btVector3& pos2 = contactPoint.getPositionWorldOnB();322 const btVector3& normal = contactPoint.m_normalWorldOnB;323 324 btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();325 btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();326 327 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);328 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);329 btVector3 vel = vel1 - vel2;330 btScalar rel_vel;331 rel_vel = normal.dot(vel);332 333 btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;334 335 //btScalar damping = solverInfo.m_damping ;336 btScalar Kerp = solverInfo.m_erp;337 btScalar Kcor = Kerp *Kfps;338 339 btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;340 btAssert(cpd);341 btScalar distance = cpd->m_penetration;342 btScalar positionalError = Kcor *-distance;343 btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;344 345 btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;346 347 btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;348 349 btScalar normalImpulse = penetrationImpulse+velocityImpulse;350 351 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse352 btScalar oldNormalImpulse = cpd->m_appliedImpulse;353 btScalar sum = oldNormalImpulse + normalImpulse;354 cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;355 356 normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;357 358 359 #ifdef USE_INTERNAL_APPLY_IMPULSE360 if (body1.getInvMass())361 {362 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);363 }364 if (body2.getInvMass())365 {366 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);367 }368 #else //USE_INTERNAL_APPLY_IMPULSE369 body1.applyImpulse(normal*(normalImpulse), rel_pos1);370 body2.applyImpulse(-normal*(normalImpulse), rel_pos2);371 #endif //USE_INTERNAL_APPLY_IMPULSE372 373 {374 //friction375 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);376 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);377 btVector3 vel = vel1 - vel2;378 379 rel_vel = normal.dot(vel);380 381 382 btVector3 lat_vel = vel - normal * rel_vel;383 btScalar lat_rel_vel = lat_vel.length();384 385 btScalar combinedFriction = cpd->m_friction;386 387 if (cpd->m_appliedImpulse > 0)388 if (lat_rel_vel > SIMD_EPSILON)389 {390 lat_vel /= lat_rel_vel;391 btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);392 btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);393 btScalar friction_impulse = lat_rel_vel /394 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));395 btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;396 397 btSetMin(friction_impulse, normal_impulse);398 btSetMax(friction_impulse, -normal_impulse);399 body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);400 body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);401 }402 }403 404 405 406 return normalImpulse;407 }408 409 btScalar resolveSingleFrictionEmpty(410 btRigidBody& body1,411 btRigidBody& body2,412 btManifoldPoint& contactPoint,413 const btContactSolverInfo& solverInfo);414 415 btScalar resolveSingleFrictionEmpty(416 btRigidBody& body1,417 btRigidBody& body2,418 btManifoldPoint& contactPoint,419 const btContactSolverInfo& solverInfo)420 {421 (void)contactPoint;422 (void)body1;423 (void)body2;424 (void)solverInfo;425 426 427 return btScalar(0.);428 }429
Note: See TracChangeset
for help on using the changeset viewer.