Back to index

supertuxkart  0.5+dfsg1
btContactConstraint.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btContactConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btVector3.h"
00020 #include "btJacobianEntry.h"
00021 #include "btContactSolverInfo.h"
00022 #include "LinearMath/btMinMax.h"
00023 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
00024 
00025 #define ASSERT2 assert
00026 
00027 #define USE_INTERNAL_APPLY_IMPULSE 1
00028 
00029 
00030 //bilateral constraint between two dynamic objects
00031 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
00032                       btRigidBody& body2, const btVector3& pos2,
00033                       btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
00034 {
00035        (void)timeStep;
00036        (void)distance;
00037 
00038 
00039        btScalar normalLenSqr = normal.length2();
00040        ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
00041        if (normalLenSqr > btScalar(1.1))
00042        {
00043               impulse = btScalar(0.);
00044               return;
00045        }
00046        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
00047        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
00048        //this jacobian entry could be re-used for all iterations
00049        
00050        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00051        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00052        btVector3 vel = vel1 - vel2;
00053        
00054 
00055          btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
00056               body2.getCenterOfMassTransform().getBasis().transpose(),
00057               rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
00058               body2.getInvInertiaDiagLocal(),body2.getInvMass());
00059 
00060        btScalar jacDiagAB = jac.getDiagonal();
00061        btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
00062        
00063          btScalar rel_vel = jac.getRelativeVelocity(
00064               body1.getLinearVelocity(),
00065               body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
00066               body2.getLinearVelocity(),
00067               body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
00068        btScalar a;
00069        a=jacDiagABInv;
00070 
00071 
00072        rel_vel = normal.dot(vel);
00073        
00074        //todo: move this into proper structure
00075        btScalar contactDamping = btScalar(0.2);
00076 
00077 #ifdef ONLY_USE_LINEAR_MASS
00078        btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
00079        impulse = - contactDamping * rel_vel * massTerm;
00080 #else  
00081        btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
00082        impulse = velocityImpulse;
00083 #endif
00084 }
00085 
00086 
00087 
00088 //response  between two dynamic objects with friction
00089 btScalar resolveSingleCollision(
00090        btRigidBody& body1,
00091        btRigidBody& body2,
00092        btManifoldPoint& contactPoint,
00093        const btContactSolverInfo& solverInfo)
00094 {
00095 
00096        const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
00097        const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
00098        const btVector3& normal = contactPoint.m_normalWorldOnB;
00099 
00100        //constant over all iterations
00101        btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
00102        btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
00103        
00104        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00105        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00106        btVector3 vel = vel1 - vel2;
00107        btScalar rel_vel;
00108        rel_vel = normal.dot(vel);
00109        
00110        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
00111 
00112        // btScalar damping = solverInfo.m_damping ;
00113        btScalar Kerp = solverInfo.m_erp;
00114        btScalar Kcor = Kerp *Kfps;
00115 
00116        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
00117        assert(cpd);
00118        btScalar distance = cpd->m_penetration;
00119        btScalar positionalError = Kcor *-distance;
00120        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
00121 
00122        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
00123 
00124        btScalar      velocityImpulse = velocityError * cpd->m_jacDiagABInv;
00125 
00126        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
00127        
00128        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
00129        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
00130        btScalar sum = oldNormalImpulse + normalImpulse;
00131        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
00132 
00133        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
00134 
00135 #ifdef USE_INTERNAL_APPLY_IMPULSE
00136        if (body1.getInvMass())
00137        {
00138               body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
00139        }
00140        if (body2.getInvMass())
00141        {
00142               body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
00143        }
00144 #else //USE_INTERNAL_APPLY_IMPULSE
00145        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
00146        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
00147 #endif //USE_INTERNAL_APPLY_IMPULSE
00148 
00149        return normalImpulse;
00150 }
00151 
00152 
00153 btScalar resolveSingleFriction(
00154        btRigidBody& body1,
00155        btRigidBody& body2,
00156        btManifoldPoint& contactPoint,
00157        const btContactSolverInfo& solverInfo)
00158 {
00159 
00160        (void)solverInfo;
00161 
00162        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
00163        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
00164 
00165        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
00166        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
00167        
00168        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
00169        assert(cpd);
00170 
00171        btScalar combinedFriction = cpd->m_friction;
00172        
00173        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
00174        
00175        if (cpd->m_appliedImpulse>btScalar(0.))
00176        //friction
00177        {
00178               //apply friction in the 2 tangential directions
00179               
00180               // 1st tangent
00181               btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00182               btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00183               btVector3 vel = vel1 - vel2;
00184        
00185               btScalar j1,j2;
00186 
00187               {
00188                             
00189                      btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
00190 
00191                      // calculate j that moves us to zero relative velocity
00192                      j1 = -vrel * cpd->m_jacDiagABInvTangent0;
00193                      btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
00194                      cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
00195                      btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
00196                      btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
00197                      j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
00198 
00199               }
00200               {
00201                      // 2nd tangent
00202 
00203                      btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
00204                      
00205                      // calculate j that moves us to zero relative velocity
00206                      j2 = -vrel * cpd->m_jacDiagABInvTangent1;
00207                      btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
00208                      cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
00209                      btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
00210                      btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
00211                      j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
00212               }
00213 
00214 #ifdef USE_INTERNAL_APPLY_IMPULSE
00215        if (body1.getInvMass())
00216        {
00217               body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
00218               body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
00219        }
00220        if (body2.getInvMass())
00221        {
00222               body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
00223               body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);  
00224        }
00225 #else //USE_INTERNAL_APPLY_IMPULSE
00226        body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
00227        body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
00228 #endif //USE_INTERNAL_APPLY_IMPULSE
00229 
00230 
00231        } 
00232        return cpd->m_appliedImpulse;
00233 }
00234 
00235 
00236 btScalar resolveSingleFrictionOriginal(
00237        btRigidBody& body1,
00238        btRigidBody& body2,
00239        btManifoldPoint& contactPoint,
00240        const btContactSolverInfo& solverInfo)
00241 {
00242 
00243        (void)solverInfo;
00244 
00245        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
00246        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
00247 
00248        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
00249        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
00250        
00251        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
00252        assert(cpd);
00253 
00254        btScalar combinedFriction = cpd->m_friction;
00255        
00256        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
00257        //if (contactPoint.m_appliedImpulse>btScalar(0.))
00258        //friction
00259        {
00260               //apply friction in the 2 tangential directions
00261               
00262               {
00263                      // 1st tangent
00264                      btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00265                      btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00266                      btVector3 vel = vel1 - vel2;
00267                      
00268                      btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
00269 
00270                      // calculate j that moves us to zero relative velocity
00271                      btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
00272                      btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
00273                      btSetMin(total, limit);
00274                      btSetMax(total, -limit);
00275                      j = total - cpd->m_accumulatedTangentImpulse0;
00276                      cpd->m_accumulatedTangentImpulse0 = total;
00277                      body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
00278                      body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
00279               }
00280 
00281                             
00282               {
00283                      // 2nd tangent
00284                      btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00285                      btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00286                      btVector3 vel = vel1 - vel2;
00287 
00288                      btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
00289                      
00290                      // calculate j that moves us to zero relative velocity
00291                      btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
00292                      btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
00293                      btSetMin(total, limit);
00294                      btSetMax(total, -limit);
00295                      j = total - cpd->m_accumulatedTangentImpulse1;
00296                      cpd->m_accumulatedTangentImpulse1 = total;
00297                      body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
00298                      body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
00299               }
00300        } 
00301        return cpd->m_appliedImpulse;
00302 }
00303 
00304 
00305 //velocity + friction
00306 //response  between two dynamic objects with friction
00307 btScalar resolveSingleCollisionCombined(
00308        btRigidBody& body1,
00309        btRigidBody& body2,
00310        btManifoldPoint& contactPoint,
00311        const btContactSolverInfo& solverInfo)
00312 {
00313 
00314        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
00315        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
00316        const btVector3& normal = contactPoint.m_normalWorldOnB;
00317 
00318        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
00319        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
00320        
00321        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00322        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00323        btVector3 vel = vel1 - vel2;
00324        btScalar rel_vel;
00325        rel_vel = normal.dot(vel);
00326        
00327        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
00328 
00329        //btScalar damping = solverInfo.m_damping ;
00330        btScalar Kerp = solverInfo.m_erp;
00331        btScalar Kcor = Kerp *Kfps;
00332 
00333        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
00334        assert(cpd);
00335        btScalar distance = cpd->m_penetration;
00336        btScalar positionalError = Kcor *-distance;
00337        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
00338 
00339        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
00340 
00341        btScalar      velocityImpulse = velocityError * cpd->m_jacDiagABInv;
00342 
00343        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
00344        
00345        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
00346        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
00347        btScalar sum = oldNormalImpulse + normalImpulse;
00348        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
00349 
00350        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
00351 
00352 
00353 #ifdef USE_INTERNAL_APPLY_IMPULSE
00354        if (body1.getInvMass())
00355        {
00356               body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
00357        }
00358        if (body2.getInvMass())
00359        {
00360               body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
00361        }
00362 #else //USE_INTERNAL_APPLY_IMPULSE
00363        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
00364        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
00365 #endif //USE_INTERNAL_APPLY_IMPULSE
00366 
00367        {
00368               //friction
00369               btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00370               btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00371               btVector3 vel = vel1 - vel2;
00372          
00373               rel_vel = normal.dot(vel);
00374 
00375 
00376               btVector3 lat_vel = vel - normal * rel_vel;
00377               btScalar lat_rel_vel = lat_vel.length();
00378 
00379               btScalar combinedFriction = cpd->m_friction;
00380 
00381               if (cpd->m_appliedImpulse > 0)
00382               if (lat_rel_vel > SIMD_EPSILON)
00383               {
00384                      lat_vel /= lat_rel_vel;
00385                      btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
00386                      btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
00387                      btScalar friction_impulse = lat_rel_vel /
00388                             (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
00389                      btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
00390 
00391                      btSetMin(friction_impulse, normal_impulse);
00392                      btSetMax(friction_impulse, -normal_impulse);
00393                      body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
00394                      body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
00395               }
00396        }
00397 
00398 
00399 
00400        return normalImpulse;
00401 }
00402 
00403 btScalar resolveSingleFrictionEmpty(
00404        btRigidBody& body1,
00405        btRigidBody& body2,
00406        btManifoldPoint& contactPoint,
00407        const btContactSolverInfo& solverInfo)
00408 {
00409        (void)contactPoint;
00410        (void)body1;
00411        (void)body2;
00412        (void)solverInfo;
00413 
00414 
00415        return btScalar(0.);
00416 };
00417