dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btConvexPlaneCollisionAlgorithm.h"
  17
  18 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  19 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  21 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
  22 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  23
  24 //#include <stdio.h>
  25
  26 btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
  27         : btCollisionAlgorithm(ci),
  28           m_ownManifold(false),
  29           m_manifoldPtr(mf),
  30           m_isSwapped(isSwapped),
  31           m_numPerturbationIterations(numPerturbationIterations),
  32           m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
  33 {
  34         const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
  35         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
  36
  37         if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()))
  38         {
  39                 m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject());
  40                 m_ownManifold = true;
  41         }
  42 }
  43
  44 btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
  45 {
  46         if (m_ownManifold)
  47         {
  48                 if (m_manifoldPtr)
  49                         m_dispatcher->releaseManifold(m_manifoldPtr);
  50         }
  51 }
  52
  53 void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  54 {
  55         const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
  56         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
  57
  58         btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
  59         btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
  60
  61         bool hasCollision = false;
  62         const btVector3& planeNormal = planeShape->getPlaneNormal();
  63         const btScalar& planeConstant = planeShape->getPlaneConstant();
  64
  65         btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
  66         btTransform convexInPlaneTrans;
  67         convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
  68         //now perturbe the convex-world transform
  69         convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot);
  70         btTransform planeInConvex;
  71         planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
  72
  73         btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
  74
  75         btVector3 vtxInPlane = convexInPlaneTrans(vtx);
  76         btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
  77
  78         btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
  79         btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
  80
  81         hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
  82         resultOut->setPersistentManifold(m_manifoldPtr);
  83         if (hasCollision)
  84         {
  85                 /// report a contact. internally this will be kept persistent, and contact reduction is done
  86                 btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
  87                 btVector3 pOnB = vtxInPlaneWorld;
  88                 resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
  89         }
  90 }
  91
  92 void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  93 {
  94         (void)dispatchInfo;
  95         if (!m_manifoldPtr)
  96                 return;
  97
  98         const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
  99         const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 100
 101         btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
 102         btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
 103
 104         bool hasCollision = false;
 105         const btVector3& planeNormal = planeShape->getPlaneNormal();
 106         const btScalar& planeConstant = planeShape->getPlaneConstant();
 107         btTransform planeInConvex;
 108         planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
 109         btTransform convexInPlaneTrans;
 110         convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
 111
 112         btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
 113         btVector3 vtxInPlane = convexInPlaneTrans(vtx);
 114         btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
 115
 116         btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 117         btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
 118
 119         hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 120         resultOut->setPersistentManifold(m_manifoldPtr);
 121         if (hasCollision)
 122         {
 123                 /// report a contact. internally this will be kept persistent, and contact reduction is done
 124                 btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
 125                 btVector3 pOnB = vtxInPlaneWorld;
 126                 resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
 127         }
 128
 129         //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
 130         //they keep on rolling forever because of the additional off-center contact points
 131         //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
 132         if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
 133         {
 134                 btVector3 v0, v1;
 135                 btPlaneSpace1(planeNormal, v0, v1);
 136                 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
 137
 138                 const btScalar angleLimit = 0.125f * SIMD_PI;
 139                 btScalar perturbeAngle;
 140                 btScalar radius = convexShape->getAngularMotionDisc();
 141                 perturbeAngle = gContactBreakingThreshold / radius;
 142                 if (perturbeAngle > angleLimit)
 143                         perturbeAngle = angleLimit;
 144
 145                 btQuaternion perturbeRot(v0, perturbeAngle);
 146                 for (int i = 0; i < m_numPerturbationIterations; i++)
 147                 {
 148                         btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
 149                         btQuaternion rotq(planeNormal, iterationAngle);
 150                         collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut);
 151                 }
 152         }
 153
 154         if (m_ownManifold)
 155         {
 156                 if (m_manifoldPtr->getNumContacts())
 157                 {
 158                         resultOut->refreshContactPoints();
 159                 }
 160         }
 161 }
 162
 163 btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 164 {
 165         (void)resultOut;
 166         (void)dispatchInfo;
 167         (void)col0;
 168         (void)col1;
 169
 170         //not yet
 171         return btScalar(1.);
 172 }