Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Feb 27, 2011, 7:43:24 AM (14 years ago)
Author:
rgrieder
Message:

Updated Bullet Physics Engine from v2.74 to v2.77

File:
1 edited

Legend:

Unmodified
Added
Removed
  • code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

    r5781 r7983  
    1414*/
    1515
     16///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
     17///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
     18///with reproduction case
     19//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
     20
    1621#include "btConvexConvexAlgorithm.h"
    1722
     
    2126#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
    2227#include "BulletCollision/CollisionShapes/btConvexShape.h"
     28#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
     29
     30
    2331#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
    2432#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
     
    4452
    4553
    46 
     54///////////
     55
     56
     57
     58static SIMD_FORCE_INLINE void segmentsClosestPoints(
     59        btVector3& ptsVector,
     60        btVector3& offsetA,
     61        btVector3& offsetB,
     62        btScalar& tA, btScalar& tB,
     63        const btVector3& translation,
     64        const btVector3& dirA, btScalar hlenA,
     65        const btVector3& dirB, btScalar hlenB )
     66{
     67        // compute the parameters of the closest points on each line segment
     68
     69        btScalar dirA_dot_dirB = btDot(dirA,dirB);
     70        btScalar dirA_dot_trans = btDot(dirA,translation);
     71        btScalar dirB_dot_trans = btDot(dirB,translation);
     72
     73        btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
     74
     75        if ( denom == 0.0f ) {
     76                tA = 0.0f;
     77        } else {
     78                tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
     79                if ( tA < -hlenA )
     80                        tA = -hlenA;
     81                else if ( tA > hlenA )
     82                        tA = hlenA;
     83        }
     84
     85        tB = tA * dirA_dot_dirB - dirB_dot_trans;
     86
     87        if ( tB < -hlenB ) {
     88                tB = -hlenB;
     89                tA = tB * dirA_dot_dirB + dirA_dot_trans;
     90
     91                if ( tA < -hlenA )
     92                        tA = -hlenA;
     93                else if ( tA > hlenA )
     94                        tA = hlenA;
     95        } else if ( tB > hlenB ) {
     96                tB = hlenB;
     97                tA = tB * dirA_dot_dirB + dirA_dot_trans;
     98
     99                if ( tA < -hlenA )
     100                        tA = -hlenA;
     101                else if ( tA > hlenA )
     102                        tA = hlenA;
     103        }
     104
     105        // compute the closest points relative to segment centers.
     106
     107        offsetA = dirA * tA;
     108        offsetB = dirB * tB;
     109
     110        ptsVector = translation - offsetA + offsetB;
     111}
     112
     113
     114static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
     115        btVector3& normalOnB,
     116        btVector3& pointOnB,
     117        btScalar capsuleLengthA,
     118        btScalar        capsuleRadiusA,
     119        btScalar capsuleLengthB,
     120        btScalar        capsuleRadiusB,
     121        int capsuleAxisA,
     122        int capsuleAxisB,
     123        const btTransform& transformA,
     124        const btTransform& transformB,
     125        btScalar distanceThreshold )
     126{
     127        btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
     128        btVector3 translationA = transformA.getOrigin();
     129        btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
     130        btVector3 translationB = transformB.getOrigin();
     131
     132        // translation between centers
     133
     134        btVector3 translation = translationB - translationA;
     135
     136        // compute the closest points of the capsule line segments
     137
     138        btVector3 ptsVector;           // the vector between the closest points
     139       
     140        btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
     141        btScalar tA, tB;              // parameters on line segment
     142
     143        segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
     144                                                   directionA, capsuleLengthA, directionB, capsuleLengthB );
     145
     146        btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
     147
     148        if ( distance > distanceThreshold )
     149                return distance;
     150
     151        btScalar lenSqr = ptsVector.length2();
     152        if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
     153        {
     154                //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
     155                btVector3 q;
     156                btPlaneSpace1(directionA,normalOnB,q);
     157        } else
     158        {
     159                // compute the contact normal
     160                normalOnB = ptsVector*-btRecipSqrt(lenSqr);
     161        }
     162        pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
     163
     164        return distance;
     165}
     166
     167
     168
     169
     170
     171
     172
     173//////////
    47174
    48175
     
    70197m_lowLevelOfDetail(false),
    71198#ifdef USE_SEPDISTANCE_UTIL2
    72 ,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
     199m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
    73200                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
    74201#endif
     
    112239                m_transformA(transformA),
    113240                m_transformB(transformB),
     241                m_unPerturbedTransform(unPerturbedTransform),
    114242                m_perturbA(perturbA),
    115                 m_unPerturbedTransform(unPerturbedTransform),
    116243                m_debugDrawer(debugDrawer)
    117244        {
     
    156283extern btScalar gContactBreakingThreshold;
    157284
     285
    158286//
    159287// Convex-Convex collision algorithm
     
    177305        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
    178306
     307        btVector3  normalOnB;
     308                btVector3  pointOnBWorld;
     309#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
     310        if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
     311        {
     312                btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
     313                btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
     314                btVector3 localScalingA = capsuleA->getLocalScaling();
     315                btVector3 localScalingB = capsuleB->getLocalScaling();
     316               
     317                btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
     318
     319                btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
     320                        capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
     321                        body0->getWorldTransform(),body1->getWorldTransform(),threshold);
     322
     323                if (dist<threshold)
     324                {
     325                        btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
     326                        resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
     327                }
     328                resultOut->refreshContactPoints();
     329                return;
     330        }
     331#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
     332
     333
    179334#ifdef USE_SEPDISTANCE_UTIL2
    180         m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
     335        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
     336        {
     337                m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
     338        }
     339
    181340        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
    182341#endif //USE_SEPDISTANCE_UTIL2
     
    195354        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
    196355        {
    197                 input.m_maximumDistanceSquared = 1e30f;
     356                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
    198357        } else
    199358#endif //USE_SEPDISTANCE_UTIL2
    200359        {
    201                 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
     360                if (dispatchInfo.m_convexMaxDistanceUseCPT)
     361                {
     362                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
     363                } else
     364                {
     365                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
     366                }
    202367                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
    203368        }
     
    208373
    209374        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
    210         btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
    211 
    212         //now perturbe directions to get multiple contact points
    213         btVector3 v0,v1;
    214         btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
    215         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
     375
     376       
     377
     378#ifdef USE_SEPDISTANCE_UTIL2
     379        btScalar sepDist = 0.f;
     380        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
     381        {
     382                sepDist = gjkPairDetector.getCachedSeparatingDistance();
     383                if (sepDist>SIMD_EPSILON)
     384                {
     385                        sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
     386                        //now perturbe directions to get multiple contact points
     387                       
     388                }
     389        }
     390#endif //USE_SEPDISTANCE_UTIL2
     391
    216392        //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
    217393       
    218394        //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
    219         if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
     395        if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
    220396        {
    221397               
    222398                int i;
     399                btVector3 v0,v1;
     400                btVector3 sepNormalWorldSpace;
     401       
     402                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
     403                btPlaneSpace1(sepNormalWorldSpace,v0,v1);
     404
    223405
    224406                bool perturbeA = true;
     
    250432                for ( i=0;i<m_numPerturbationIterations;i++)
    251433                {
     434                        if (v0.length2()>SIMD_EPSILON)
     435                        {
    252436                        btQuaternion perturbeRot(v0,perturbeAngle);
    253437                        btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
     
    273457                        btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
    274458                        gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
     459                        }
    275460                       
    276                        
    277461                }
    278462        }
     
    281465
    282466#ifdef USE_SEPDISTANCE_UTIL2
    283         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
     467        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
    284468        {
    285469                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
Note: See TracChangeset for help on using the changeset viewer.