Back to index

supertuxkart  0.5+dfsg1
btSphereBoxCollisionAlgorithm.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 #include "btSphereBoxCollisionAlgorithm.h"
00017 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00018 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00019 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00020 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00021 //#include <stdio.h>
00022 
00023 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
00024 : btCollisionAlgorithm(ci),
00025 m_ownManifold(false),
00026 m_manifoldPtr(mf),
00027 m_isSwapped(isSwapped)
00028 {
00029        btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
00030        btCollisionObject* boxObj = m_isSwapped? col0 : col1;
00031        
00032        if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
00033        {
00034               m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
00035               m_ownManifold = true;
00036        }
00037 }
00038 
00039 
00040 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
00041 {
00042        if (m_ownManifold)
00043        {
00044               if (m_manifoldPtr)
00045                      m_dispatcher->releaseManifold(m_manifoldPtr);
00046        }
00047 }
00048 
00049 
00050 
00051 void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00052 {
00053        (void)dispatchInfo;
00054        (void)resultOut;
00055        if (!m_manifoldPtr)
00056               return;
00057 
00058        btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
00059        btCollisionObject* boxObj = m_isSwapped? body0 : body1;
00060 
00061 
00062        btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
00063 
00064        btVector3 normalOnSurfaceB;
00065        btVector3 pOnBox,pOnSphere;
00066        btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
00067        btScalar radius = sphere0->getRadius();
00068        
00069        btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
00070 
00071        resultOut->setPersistentManifold(m_manifoldPtr);
00072 
00073        if (dist < SIMD_EPSILON)
00074        {
00075               btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
00076 
00078 
00079               resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
00080               
00081        }
00082 
00083        if (m_ownManifold)
00084        {
00085               if (m_manifoldPtr->getNumContacts())
00086               {
00087                      resultOut->refreshContactPoints();
00088               }
00089        }
00090 
00091 }
00092 
00093 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00094 {
00095        (void)resultOut;
00096        (void)dispatchInfo;
00097        (void)col0;
00098        (void)col1;
00099 
00100        //not yet
00101        return btScalar(1.);
00102 }
00103 
00104 
00105 btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
00106 {
00107 
00108        btScalar margins;
00109        btVector3 bounds[2];
00110        btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
00111        
00112        bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
00113        bounds[1] = boxShape->getHalfExtentsWithoutMargin();
00114 
00115        margins = boxShape->getMargin();//also add sphereShape margin?
00116 
00117        const btTransform&   m44T = boxObj->getWorldTransform();
00118 
00119        btVector3     boundsVec[2];
00120        btScalar      fPenetration;
00121 
00122        boundsVec[0] = bounds[0];
00123        boundsVec[1] = bounds[1];
00124 
00125        btVector3     marginsVec( margins, margins, margins );
00126 
00127        // add margins
00128        bounds[0] += marginsVec;
00129        bounds[1] -= marginsVec;
00130 
00132 
00133        btVector3     tmp, prel, n[6], normal, v3P;
00134        btScalar   fSep = btScalar(10000000.0), fSepThis;
00135 
00136        n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
00137        n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
00138        n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
00139        n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
00140        n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
00141        n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
00142 
00143        // convert  point in local space
00144        prel = m44T.invXform( sphereCenter);
00145        
00146        bool   bFound = false;
00147 
00148        v3P = prel;
00149 
00150        for (int i=0;i<6;i++)
00151        {
00152               int j = i<3? 0:1;
00153               if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
00154               {
00155                      v3P = v3P - n[i]*fSepThis;         
00156                      bFound = true;
00157               }
00158        }
00159        
00160        //
00161 
00162        if ( bFound )
00163        {
00164               bounds[0] = boundsVec[0];
00165               bounds[1] = boundsVec[1];
00166 
00167               normal = (prel - v3P).normalize();
00168               pointOnBox = v3P + normal*margins;
00169               v3PointOnSphere = prel - normal*fRadius;
00170 
00171               if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
00172               {
00173                      return btScalar(1.0);
00174               }
00175 
00176               // transform back in world space
00177               tmp = m44T( pointOnBox);
00178               pointOnBox    = tmp;
00179               tmp  = m44T( v3PointOnSphere);            
00180               v3PointOnSphere = tmp;
00181               btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
00182               
00183               //if this fails, fallback into deeper penetration case, below
00184               if (fSeps2 > SIMD_EPSILON)
00185               {
00186                      fSep = - btSqrt(fSeps2);
00187                      normal = (pointOnBox-v3PointOnSphere);
00188                      normal *= btScalar(1.)/fSep;
00189               }
00190 
00191               return fSep;
00192        }
00193 
00195        // Deep penetration case
00196 
00197        fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
00198 
00199        bounds[0] = boundsVec[0];
00200        bounds[1] = boundsVec[1];
00201 
00202        if ( fPenetration <= btScalar(0.0) )
00203               return (fPenetration-margins);
00204        else
00205               return btScalar(1.0);
00206 }
00207 
00208 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
00209 {
00210 
00211        btVector3 bounds[2];
00212 
00213        bounds[0] = aabbMin;
00214        bounds[1] = aabbMax;
00215 
00216        btVector3     p0, tmp, prel, n[6], normal;
00217        btScalar   fSep = btScalar(-10000000.0), fSepThis;
00218 
00219        // set p0 and normal to a default value to shup up GCC
00220        p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
00221        normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
00222 
00223        n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
00224        n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
00225        n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
00226        n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
00227        n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
00228        n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
00229 
00230        const btTransform&   m44T = boxObj->getWorldTransform();
00231 
00232        // convert  point in local space
00233        prel = m44T.invXform( sphereCenter);
00234 
00236 
00237        for (int i=0;i<6;i++)
00238        {
00239               int j = i<3 ? 0:1;
00240               if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )   return btScalar(1.0);
00241               if ( fSepThis > fSep )
00242               {
00243                      p0 = bounds[j];      normal = (btVector3&)n[i];
00244                      fSep = fSepThis;
00245               }
00246        }
00247 
00248        pointOnBox = prel - normal*(normal.dot((prel-p0)));
00249        v3PointOnSphere = pointOnBox + normal*fSep;
00250 
00251        // transform back in world space
00252        tmp  = m44T( pointOnBox);          
00253        pointOnBox    = tmp;
00254        tmp  = m44T( v3PointOnSphere);            v3PointOnSphere = tmp;
00255        normal = (pointOnBox-v3PointOnSphere).normalize();
00256 
00257        return fSep;
00258 
00259 }
00260