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 "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00022 //#include <stdio.h>
00023 
00024 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
00025 : btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
00026 m_ownManifold(false),
00027 m_manifoldPtr(mf),
00028 m_isSwapped(isSwapped)
00029 {
00030         const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap;
00031         const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap;
00032         
00033         if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()))
00034         {
00035                 m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject());
00036                 m_ownManifold = true;
00037         }
00038 }
00039 
00040 
00041 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
00042 {
00043         if (m_ownManifold)
00044         {
00045                 if (m_manifoldPtr)
00046                         m_dispatcher->releaseManifold(m_manifoldPtr);
00047         }
00048 }
00049 
00050 
00051 
00052 void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
00053 {
00054         (void)dispatchInfo;
00055         (void)resultOut;
00056         if (!m_manifoldPtr)
00057                 return;
00058 
00059         const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00060         const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00061 
00062         btVector3 pOnBox;
00063 
00064         btVector3 normalOnSurfaceB;
00065         btScalar penetrationDepth;
00066         btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
00067         const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
00068         btScalar radius = sphere0->getRadius();
00069         btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
00070 
00071         resultOut->setPersistentManifold(m_manifoldPtr);
00072 
00073         if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
00074         {
00076                 resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
00077         }
00078 
00079         if (m_ownManifold)
00080         {
00081                 if (m_manifoldPtr->getNumContacts())
00082                 {
00083                         resultOut->refreshContactPoints();
00084                 }
00085         }
00086 
00087 }
00088 
00089 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00090 {
00091         (void)resultOut;
00092         (void)dispatchInfo;
00093         (void)col0;
00094         (void)col1;
00095 
00096         //not yet
00097         return btScalar(1.);
00098 }
00099 
00100 
00101 bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
00102 {
00103         const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
00104         btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
00105         btScalar boxMargin = boxShape->getMargin();
00106         penetrationDepth = 1.0f;
00107 
00108         // convert the sphere position to the box's local space
00109         btTransform const &m44T = boxObjWrap->getWorldTransform();
00110         btVector3 sphereRelPos = m44T.invXform(sphereCenter);
00111 
00112         // Determine the closest point to the sphere center in the box
00113         btVector3 closestPoint = sphereRelPos;
00114         closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
00115         closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
00116         closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
00117         closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
00118         closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
00119         closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
00120         
00121         btScalar intersectionDist = fRadius + boxMargin;
00122         btScalar contactDist = intersectionDist + maxContactDistance;
00123         normal = sphereRelPos - closestPoint;
00124 
00125         //if there is no penetration, we are done
00126         btScalar dist2 = normal.length2();
00127         if (dist2 > contactDist * contactDist)
00128         {
00129                 return false;
00130         }
00131 
00132         btScalar distance;
00133 
00134         //special case if the sphere center is inside the box
00135         if (dist2 <= SIMD_EPSILON)
00136         {
00137                 distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
00138         }
00139         else //compute the penetration details
00140         {
00141                 distance = normal.length();
00142                 normal /= distance;
00143         }
00144 
00145         pointOnBox = closestPoint + normal * boxMargin;
00146 //      v3PointOnSphere = sphereRelPos - (normal * fRadius);    
00147         penetrationDepth = distance - intersectionDist;
00148 
00149         // transform back in world space
00150         btVector3 tmp = m44T(pointOnBox);
00151         pointOnBox = tmp;
00152 //      tmp = m44T(v3PointOnSphere);
00153 //      v3PointOnSphere = tmp;
00154         tmp = m44T.getBasis() * normal;
00155         normal = tmp;
00156 
00157         return true;
00158 }
00159 
00160 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) 
00161 {
00162         //project the center of the sphere on the closest face of the box
00163         btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
00164         btScalar minDist = faceDist;
00165         closestPoint.setX( boxHalfExtent.getX() );
00166         normal.setValue(btScalar(1.0f),  btScalar(0.0f),  btScalar(0.0f));
00167 
00168         faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
00169         if (faceDist < minDist)
00170         {
00171                 minDist = faceDist;
00172                 closestPoint = sphereRelPos;
00173                 closestPoint.setX( -boxHalfExtent.getX() );
00174                 normal.setValue(btScalar(-1.0f),  btScalar(0.0f),  btScalar(0.0f));
00175         }
00176 
00177         faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
00178         if (faceDist < minDist)
00179         {
00180                 minDist = faceDist;
00181                 closestPoint = sphereRelPos;
00182                 closestPoint.setY( boxHalfExtent.getY() );
00183                 normal.setValue(btScalar(0.0f),  btScalar(1.0f),  btScalar(0.0f));
00184         }
00185 
00186         faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
00187         if (faceDist < minDist)
00188         {
00189                 minDist = faceDist;
00190                 closestPoint = sphereRelPos;
00191                 closestPoint.setY( -boxHalfExtent.getY() );
00192                 normal.setValue(btScalar(0.0f),  btScalar(-1.0f),  btScalar(0.0f));
00193         }
00194 
00195         faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
00196         if (faceDist < minDist)
00197         {
00198                 minDist = faceDist;
00199                 closestPoint = sphereRelPos;
00200                 closestPoint.setZ( boxHalfExtent.getZ() );
00201                 normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(1.0f));
00202         }
00203 
00204         faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
00205         if (faceDist < minDist)
00206         {
00207                 minDist = faceDist;
00208                 closestPoint = sphereRelPos;
00209                 closestPoint.setZ( -boxHalfExtent.getZ() );
00210                 normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(-1.0f));
00211         }
00212 
00213         return minDist;
00214 }