Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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
00132
00133
00134 if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
00135 {
00136 btVector3 v0,v1;
00137 btPlaneSpace1(planeNormal,v0,v1);
00138
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
00173 return btScalar(1.);
00174 }