btConvexPlaneCollisionAlgorithm.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 "btConvexPlaneCollisionAlgorithm.h"
00017 
00018 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00019 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00020 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00021 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
00022 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00023 
00024 //#include <stdio.h>
00025 
00026 btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
00027 : btCollisionAlgorithm(ci),
00028 m_ownManifold(false),
00029 m_manifoldPtr(mf),
00030 m_isSwapped(isSwapped),
00031 m_numPerturbationIterations(numPerturbationIterations),
00032 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
00033 {
00034         const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap;
00035         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap;
00036 
00037         if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()))
00038         {
00039                 m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject());
00040                 m_ownManifold = true;
00041         }
00042 }
00043 
00044 
00045 btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
00046 {
00047         if (m_ownManifold)
00048         {
00049                 if (m_manifoldPtr)
00050                         m_dispatcher->releaseManifold(m_manifoldPtr);
00051         }
00052 }
00053 
00054 void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00055 {
00056     const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00057         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
00058 
00059         btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
00060         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
00061 
00062     bool hasCollision = false;
00063         const btVector3& planeNormal = planeShape->getPlaneNormal();
00064         const btScalar& planeConstant = planeShape->getPlaneConstant();
00065         
00066         btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
00067         btTransform convexInPlaneTrans;
00068         convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
00069         //now perturbe the convex-world transform
00070         convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
00071         btTransform planeInConvex;
00072         planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
00073         
00074         btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
00075 
00076         btVector3 vtxInPlane = convexInPlaneTrans(vtx);
00077         btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
00078 
00079         btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
00080         btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
00081 
00082         hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
00083         resultOut->setPersistentManifold(m_manifoldPtr);
00084         if (hasCollision)
00085         {
00087                 btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
00088                 btVector3 pOnB = vtxInPlaneWorld;
00089                 resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
00090         }
00091 }
00092 
00093 
00094 void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00095 {
00096         (void)dispatchInfo;
00097         if (!m_manifoldPtr)
00098                 return;
00099 
00100         const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00101         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
00102 
00103         btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
00104         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
00105 
00106         bool hasCollision = false;
00107         const btVector3& planeNormal = planeShape->getPlaneNormal();
00108         const btScalar& planeConstant = planeShape->getPlaneConstant();
00109         btTransform planeInConvex;
00110         planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
00111         btTransform convexInPlaneTrans;
00112         convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
00113 
00114         btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
00115         btVector3 vtxInPlane = convexInPlaneTrans(vtx);
00116         btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
00117 
00118         btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
00119         btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
00120 
00121         hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
00122         resultOut->setPersistentManifold(m_manifoldPtr);
00123         if (hasCollision)
00124         {
00126                 btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
00127                 btVector3 pOnB = vtxInPlaneWorld;
00128                 resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
00129         }
00130 
00131         //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
00132         //they keep on rolling forever because of the additional off-center contact points
00133         //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
00134         if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
00135         {
00136                 btVector3 v0,v1;
00137                 btPlaneSpace1(planeNormal,v0,v1);
00138                 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
00139 
00140                 const btScalar angleLimit = 0.125f * SIMD_PI;
00141                 btScalar perturbeAngle;
00142                 btScalar radius = convexShape->getAngularMotionDisc();
00143                 perturbeAngle = gContactBreakingThreshold / radius;
00144                 if ( perturbeAngle > angleLimit ) 
00145                                 perturbeAngle = angleLimit;
00146 
00147                 btQuaternion perturbeRot(v0,perturbeAngle);
00148                 for (int i=0;i<m_numPerturbationIterations;i++)
00149                 {
00150                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
00151                         btQuaternion rotq(planeNormal,iterationAngle);
00152                         collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
00153                 }
00154         }
00155 
00156         if (m_ownManifold)
00157         {
00158                 if (m_manifoldPtr->getNumContacts())
00159                 {
00160                         resultOut->refreshContactPoints();
00161                 }
00162         }
00163 }
00164 
00165 btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00166 {
00167         (void)resultOut;
00168         (void)dispatchInfo;
00169         (void)col0;
00170         (void)col1;
00171 
00172         //not yet
00173         return btScalar(1.);
00174 }