dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.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 "btContinuousConvexCollision.h"
  17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  18 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
  19 #include "LinearMath/btTransformUtil.h"
  20 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  21
  22 #include "btGjkPairDetector.h"
  23 #include "btPointCollector.h"
  24 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
  25
  26 btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
  27         : m_simplexSolver(simplexSolver),
  28           m_penetrationDepthSolver(penetrationDepthSolver),
  29           m_convexA(convexA),
  30           m_convexB1(convexB),
  31           m_planeShape(0)
  32 {
  33 }
  34
  35 btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane)
  36         : m_simplexSolver(0),
  37           m_penetrationDepthSolver(0),
  38           m_convexA(convexA),
  39           m_convexB1(0),
  40           m_planeShape(plane)
  41 {
  42 }
  43
  44 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
  45 /// You don't want your game ever to lock-up.
  46 #define MAX_ITERATIONS 64
  47
  48 void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector)
  49 {
  50         if (m_convexB1)
  51         {
  52                 m_simplexSolver->reset();
  53                 btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver);
  54                 btGjkPairDetector::ClosestPointInput input;
  55                 input.m_transformA = transA;
  56                 input.m_transformB = transB;
  57                 gjk.getClosestPoints(input, pointCollector, 0);
  58         }
  59         else
  60         {
  61                 //convex versus plane
  62                 const btConvexShape* convexShape = m_convexA;
  63                 const btStaticPlaneShape* planeShape = m_planeShape;
  64
  65                 const btVector3& planeNormal = planeShape->getPlaneNormal();
  66                 const btScalar& planeConstant = planeShape->getPlaneConstant();
  67
  68                 btTransform convexWorldTransform = transA;
  69                 btTransform convexInPlaneTrans;
  70                 convexInPlaneTrans = transB.inverse() * convexWorldTransform;
  71                 btTransform planeInConvex;
  72                 planeInConvex = convexWorldTransform.inverse() * transB;
  73
  74                 btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
  75
  76                 btVector3 vtxInPlane = convexInPlaneTrans(vtx);
  77                 btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
  78
  79                 btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
  80                 btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
  81                 btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
  82
  83                 pointCollector.addContactPoint(
  84                         normalOnSurfaceB,
  85                         vtxInPlaneWorld,
  86                         distance);
  87         }
  88 }
  89
  90 bool btContinuousConvexCollision::calcTimeOfImpact(
  91         const btTransform& fromA,
  92         const btTransform& toA,
  93         const btTransform& fromB,
  94         const btTransform& toB,
  95         CastResult& result)
  96 {
  97         /// compute linear and angular velocity for this interval, to interpolate
  98         btVector3 linVelA, angVelA, linVelB, angVelB;
  99         btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA);
 100         btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB);
 101
 102         btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
 103         btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f;
 104
 105         btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
 106         btVector3 relLinVel = (linVelB - linVelA);
 107
 108         btScalar relLinVelocLength = (linVelB - linVelA).length();
 109
 110         if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f)
 111                 return false;
 112
 113         btScalar lambda = btScalar(0.);
 114
 115         btVector3 n;
 116         n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 117         bool hasResult = false;
 118         btVector3 c;
 119
 120         btScalar lastLambda = lambda;
 121         //btScalar epsilon = btScalar(0.001);
 122
 123         int numIter = 0;
 124         //first solution, using GJK
 125
 126         btScalar radius = 0.001f;
 127         //      result.drawCoordSystem(sphereTr);
 128
 129         btPointCollector pointCollector1;
 130
 131         {
 132                 computeClosestPoints(fromA, fromB, pointCollector1);
 133
 134                 hasResult = pointCollector1.m_hasResult;
 135                 c = pointCollector1.m_pointInWorld;
 136         }
 137
 138         if (hasResult)
 139         {
 140                 btScalar dist;
 141                 dist = pointCollector1.m_distance + result.m_allowedPenetration;
 142                 n = pointCollector1.m_normalOnBInWorld;
 143                 btScalar projectedLinearVelocity = relLinVel.dot(n);
 144                 if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
 145                         return false;
 146
 147                 //not close enough
 148                 while (dist > radius)
 149                 {
 150                         if (result.m_debugDrawer)
 151                         {
 152                                 result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1));
 153                         }
 154                         btScalar dLambda = btScalar(0.);
 155
 156                         projectedLinearVelocity = relLinVel.dot(n);
 157
 158                         //don't report time of impact for motion away from the contact normal (or causes minor penetration)
 159                         if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
 160                                 return false;
 161
 162                         dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
 163
 164                         lambda += dLambda;
 165
 166                         if (lambda > btScalar(1.) || lambda < btScalar(0.))
 167                                 return false;
 168
 169                         //todo: next check with relative epsilon
 170                         if (lambda <= lastLambda)
 171                         {
 172                                 return false;
 173                                 //n.setValue(0,0,0);
 174                                 //break;
 175                         }
 176                         lastLambda = lambda;
 177
 178                         //interpolate to next lambda
 179                         btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
 180
 181                         btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA);
 182                         btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB);
 183                         relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
 184
 185                         if (result.m_debugDrawer)
 186                         {
 187                                 result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0));
 188                         }
 189
 190                         result.DebugDraw(lambda);
 191
 192                         btPointCollector pointCollector;
 193                         computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector);
 194
 195                         if (pointCollector.m_hasResult)
 196                         {
 197                                 dist = pointCollector.m_distance + result.m_allowedPenetration;
 198                                 c = pointCollector.m_pointInWorld;
 199                                 n = pointCollector.m_normalOnBInWorld;
 200                         }
 201                         else
 202                         {
 203                                 result.reportFailure(-1, numIter);
 204                                 return false;
 205                         }
 206
 207                         numIter++;
 208                         if (numIter > MAX_ITERATIONS)
 209                         {
 210                                 result.reportFailure(-2, numIter);
 211                                 return false;
 212                         }
 213                 }
 214
 215                 result.m_fraction = lambda;
 216                 result.m_normal = n;
 217                 result.m_hitPoint = c;
 218                 return true;
 219         }
 220
 221         return false;
 222 }