dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.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 "btSphereBoxCollisionAlgorithm.h"
  17 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  18 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  19 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  20 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  22 //#include <stdio.h>
  23
  24 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
  25         : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
  26           m_ownManifold(false),
  27           m_manifoldPtr(mf),
  28           m_isSwapped(isSwapped)
  29 {
  30         const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
  31         const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
  32
  33         if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()))
  34         {
  35                 m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject());
  36                 m_ownManifold = true;
  37         }
  38 }
  39
  40 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
  41 {
  42         if (m_ownManifold)
  43         {
  44                 if (m_manifoldPtr)
  45                         m_dispatcher->releaseManifold(m_manifoldPtr);
  46         }
  47 }
  48
  49 void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  50 {
  51         (void)dispatchInfo;
  52         (void)resultOut;
  53         if (!m_manifoldPtr)
  54                 return;
  55
  56         const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
  57         const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
  58
  59         btVector3 pOnBox;
  60
  61         btVector3 normalOnSurfaceB;
  62         btScalar penetrationDepth;
  63         btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
  64         const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
  65         btScalar radius = sphere0->getRadius();
  66         btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
  67
  68         resultOut->setPersistentManifold(m_manifoldPtr);
  69
  70         if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
  71         {
  72                 /// report a contact. internally this will be kept persistent, and contact reduction is done
  73                 resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
  74         }
  75
  76         if (m_ownManifold)
  77         {
  78                 if (m_manifoldPtr->getNumContacts())
  79                 {
  80                         resultOut->refreshContactPoints();
  81                 }
  82         }
  83 }
  84
  85 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  86 {
  87         (void)resultOut;
  88         (void)dispatchInfo;
  89         (void)col0;
  90         (void)col1;
  91
  92         //not yet
  93         return btScalar(1.);
  94 }
  95
  96 bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance)
  97 {
  98         const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape();
  99         btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
 100         btScalar boxMargin = boxShape->getMargin();
 101         penetrationDepth = 1.0f;
 102
 103         // convert the sphere position to the box's local space
 104         btTransform const& m44T = boxObjWrap->getWorldTransform();
 105         btVector3 sphereRelPos = m44T.invXform(sphereCenter);
 106
 107         // Determine the closest point to the sphere center in the box
 108         btVector3 closestPoint = sphereRelPos;
 109         closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX()));
 110         closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX()));
 111         closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY()));
 112         closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY()));
 113         closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ()));
 114         closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ()));
 115
 116         btScalar intersectionDist = fRadius + boxMargin;
 117         btScalar contactDist = intersectionDist + maxContactDistance;
 118         normal = sphereRelPos - closestPoint;
 119
 120         //if there is no penetration, we are done
 121         btScalar dist2 = normal.length2();
 122         if (dist2 > contactDist * contactDist)
 123         {
 124                 return false;
 125         }
 126
 127         btScalar distance;
 128
 129         //special case if the sphere center is inside the box
 130         if (dist2 <= SIMD_EPSILON)
 131         {
 132                 distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
 133         }
 134         else  //compute the penetration details
 135         {
 136                 distance = normal.length();
 137                 normal /= distance;
 138         }
 139
 140         pointOnBox = closestPoint + normal * boxMargin;
 141         //      v3PointOnSphere = sphereRelPos - (normal * fRadius);
 142         penetrationDepth = distance - intersectionDist;
 143
 144         // transform back in world space
 145         btVector3 tmp = m44T(pointOnBox);
 146         pointOnBox = tmp;
 147         //      tmp = m44T(v3PointOnSphere);
 148         //      v3PointOnSphere = tmp;
 149         tmp = m44T.getBasis() * normal;
 150         normal = tmp;
 151
 152         return true;
 153 }
 154
 155 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal)
 156 {
 157         //project the center of the sphere on the closest face of the box
 158         btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
 159         btScalar minDist = faceDist;
 160         closestPoint.setX(boxHalfExtent.getX());
 161         normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
 162
 163         faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
 164         if (faceDist < minDist)
 165         {
 166                 minDist = faceDist;
 167                 closestPoint = sphereRelPos;
 168                 closestPoint.setX(-boxHalfExtent.getX());
 169                 normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
 170         }
 171
 172         faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
 173         if (faceDist < minDist)
 174         {
 175                 minDist = faceDist;
 176                 closestPoint = sphereRelPos;
 177                 closestPoint.setY(boxHalfExtent.getY());
 178                 normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
 179         }
 180
 181         faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
 182         if (faceDist < minDist)
 183         {
 184                 minDist = faceDist;
 185                 closestPoint = sphereRelPos;
 186                 closestPoint.setY(-boxHalfExtent.getY());
 187                 normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
 188         }
 189
 190         faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
 191         if (faceDist < minDist)
 192         {
 193                 minDist = faceDist;
 194                 closestPoint = sphereRelPos;
 195                 closestPoint.setZ(boxHalfExtent.getZ());
 196                 normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
 197         }
 198
 199         faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
 200         if (faceDist < minDist)
 201         {
 202                 minDist = faceDist;
 203                 closestPoint = sphereRelPos;
 204                 closestPoint.setZ(-boxHalfExtent.getZ());
 205                 normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
 206         }
 207
 208         return minDist;
 209 }