dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.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 "btMinkowskiPenetrationDepthSolver.h"
  17 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  18 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  19 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
  21
  22 #define NUM_UNITSPHERE_POINTS 42
  23
  24 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
  25                                                                                                          const btConvexShape* convexA, const btConvexShape* convexB,
  26                                                                                                          const btTransform& transA, const btTransform& transB,
  27                                                                                                          btVector3& v, btVector3& pa, btVector3& pb,
  28                                                                                                          class btIDebugDraw* debugDraw)
  29 {
  30         (void)v;
  31
  32         bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
  33
  34         struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
  35         {
  36                 btIntermediateResult() : m_hasResult(false)
  37                 {
  38                 }
  39
  40                 btVector3 m_normalOnBInWorld;
  41                 btVector3 m_pointInWorld;
  42                 btScalar m_depth;
  43                 bool m_hasResult;
  44
  45                 virtual void setShapeIdentifiersA(int partId0, int index0)
  46                 {
  47                         (void)partId0;
  48                         (void)index0;
  49                 }
  50                 virtual void setShapeIdentifiersB(int partId1, int index1)
  51                 {
  52                         (void)partId1;
  53                         (void)index1;
  54                 }
  55                 void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
  56                 {
  57                         m_normalOnBInWorld = normalOnBInWorld;
  58                         m_pointInWorld = pointInWorld;
  59                         m_depth = depth;
  60                         m_hasResult = true;
  61                 }
  62         };
  63
  64         //just take fixed number of orientation, and sample the penetration depth in that direction
  65         btScalar minProj = btScalar(BT_LARGE_FLOAT);
  66         btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
  67         btVector3 minA, minB;
  68         btVector3 separatingAxisInA, separatingAxisInB;
  69         btVector3 pInA, qInB, pWorld, qWorld, w;
  70
  71 #ifndef __SPU__
  72 #define USE_BATCHED_SUPPORT 1
  73 #endif
  74 #ifdef USE_BATCHED_SUPPORT
  75
  76         btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
  77         btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
  78         btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
  79         btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
  80         int i;
  81
  82         int numSampleDirections = NUM_UNITSPHERE_POINTS;
  83
  84         for (i = 0; i < numSampleDirections; i++)
  85         {
  86                 btVector3 norm = getPenetrationDirections()[i];
  87                 separatingAxisInABatch[i] = (-norm) * transA.getBasis();
  88                 separatingAxisInBBatch[i] = norm * transB.getBasis();
  89         }
  90
  91         {
  92                 int numPDA = convexA->getNumPreferredPenetrationDirections();
  93                 if (numPDA)
  94                 {
  95                         for (int i = 0; i < numPDA; i++)
  96                         {
  97                                 btVector3 norm;
  98                                 convexA->getPreferredPenetrationDirection(i, norm);
  99                                 norm = transA.getBasis() * norm;
 100                                 getPenetrationDirections()[numSampleDirections] = norm;
 101                                 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 102                                 separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 103                                 numSampleDirections++;
 104                         }
 105                 }
 106         }
 107
 108         {
 109                 int numPDB = convexB->getNumPreferredPenetrationDirections();
 110                 if (numPDB)
 111                 {
 112                         for (int i = 0; i < numPDB; i++)
 113                         {
 114                                 btVector3 norm;
 115                                 convexB->getPreferredPenetrationDirection(i, norm);
 116                                 norm = transB.getBasis() * norm;
 117                                 getPenetrationDirections()[numSampleDirections] = norm;
 118                                 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 119                                 separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 120                                 numSampleDirections++;
 121                         }
 122                 }
 123         }
 124
 125         convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections);
 126         convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
 127
 128         for (i = 0; i < numSampleDirections; i++)
 129         {
 130                 btVector3 norm = getPenetrationDirections()[i];
 131                 if (check2d)
 132                 {
 133                         norm[2] = 0.f;
 134                 }
 135                 if (norm.length2() > 0.01)
 136                 {
 137                         separatingAxisInA = separatingAxisInABatch[i];
 138                         separatingAxisInB = separatingAxisInBBatch[i];
 139
 140                         pInA = supportVerticesABatch[i];
 141                         qInB = supportVerticesBBatch[i];
 142
 143                         pWorld = transA(pInA);
 144                         qWorld = transB(qInB);
 145                         if (check2d)
 146                         {
 147                                 pWorld[2] = 0.f;
 148                                 qWorld[2] = 0.f;
 149                         }
 150
 151                         w = qWorld - pWorld;
 152                         btScalar delta = norm.dot(w);
 153                         //find smallest delta
 154                         if (delta < minProj)
 155                         {
 156                                 minProj = delta;
 157                                 minNorm = norm;
 158                                 minA = pWorld;
 159                                 minB = qWorld;
 160                         }
 161                 }
 162         }
 163 #else
 164
 165         int numSampleDirections = NUM_UNITSPHERE_POINTS;
 166
 167 #ifndef __SPU__
 168         {
 169                 int numPDA = convexA->getNumPreferredPenetrationDirections();
 170                 if (numPDA)
 171                 {
 172                         for (int i = 0; i < numPDA; i++)
 173                         {
 174                                 btVector3 norm;
 175                                 convexA->getPreferredPenetrationDirection(i, norm);
 176                                 norm = transA.getBasis() * norm;
 177                                 getPenetrationDirections()[numSampleDirections] = norm;
 178                                 numSampleDirections++;
 179                         }
 180                 }
 181         }
 182
 183         {
 184                 int numPDB = convexB->getNumPreferredPenetrationDirections();
 185                 if (numPDB)
 186                 {
 187                         for (int i = 0; i < numPDB; i++)
 188                         {
 189                                 btVector3 norm;
 190                                 convexB->getPreferredPenetrationDirection(i, norm);
 191                                 norm = transB.getBasis() * norm;
 192                                 getPenetrationDirections()[numSampleDirections] = norm;
 193                                 numSampleDirections++;
 194                         }
 195                 }
 196         }
 197 #endif  // __SPU__
 198
 199         for (int i = 0; i < numSampleDirections; i++)
 200         {
 201                 const btVector3& norm = getPenetrationDirections()[i];
 202                 separatingAxisInA = (-norm) * transA.getBasis();
 203                 separatingAxisInB = norm * transB.getBasis();
 204                 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
 205                 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 206                 pWorld = transA(pInA);
 207                 qWorld = transB(qInB);
 208                 w = qWorld - pWorld;
 209                 btScalar delta = norm.dot(w);
 210                 //find smallest delta
 211                 if (delta < minProj)
 212                 {
 213                         minProj = delta;
 214                         minNorm = norm;
 215                         minA = pWorld;
 216                         minB = qWorld;
 217                 }
 218         }
 219 #endif  //USE_BATCHED_SUPPORT
 220
 221         //add the margins
 222
 223         minA += minNorm * convexA->getMarginNonVirtual();
 224         minB -= minNorm * convexB->getMarginNonVirtual();
 225         //no penetration
 226         if (minProj < btScalar(0.))
 227                 return false;
 228
 229         btScalar extraSeparation = 0.5f;  ///scale dependent
 230         minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 231
 232 //#define DEBUG_DRAW 1
 233 #ifdef DEBUG_DRAW
 234         if (debugDraw)
 235         {
 236                 btVector3 color(0, 1, 0);
 237                 debugDraw->drawLine(minA, minB, color);
 238                 color = btVector3(1, 1, 1);
 239                 btVector3 vec = minB - minA;
 240                 btScalar prj2 = minNorm.dot(vec);
 241                 debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
 242         }
 243 #endif  //DEBUG_DRAW
 244
 245         btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0);
 246
 247         btScalar offsetDist = minProj;
 248         btVector3 offset = minNorm * offsetDist;
 249
 250         btGjkPairDetector::ClosestPointInput input;
 251
 252         btVector3 newOrg = transA.getOrigin() + offset;
 253
 254         btTransform displacedTrans = transA;
 255         displacedTrans.setOrigin(newOrg);
 256
 257         input.m_transformA = displacedTrans;
 258         input.m_transformB = transB;
 259         input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);  //minProj;
 260
 261         btIntermediateResult res;
 262         gjkdet.setCachedSeparatingAxis(-minNorm);
 263         gjkdet.getClosestPoints(input, res, debugDraw);
 264
 265         btScalar correctedMinNorm = minProj - res.m_depth;
 266
 267         //the penetration depth is over-estimated, relax it
 268         btScalar penetration_relaxation = btScalar(1.);
 269         minNorm *= penetration_relaxation;
 270
 271         if (res.m_hasResult)
 272         {
 273                 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 274                 pb = res.m_pointInWorld;
 275                 v = minNorm;
 276
 277 #ifdef DEBUG_DRAW
 278                 if (debugDraw)
 279                 {
 280                         btVector3 color(1, 0, 0);
 281                         debugDraw->drawLine(pa, pb, color);
 282                 }
 283 #endif  //DEBUG_DRAW
 284         }
 285         return res.m_hasResult;
 286 }
 287
 288 btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
 289 {
 290         static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
 291                 {
 292                         btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)),
 293                         btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)),
 294                         btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)),
 295                         btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)),
 296                         btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)),
 297                         btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)),
 298                         btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)),
 299                         btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)),
 300                         btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)),
 301                         btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)),
 302                         btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)),
 303                         btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)),
 304                         btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)),
 305                         btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)),
 306                         btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)),
 307                         btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)),
 308                         btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)),
 309                         btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)),
 310                         btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)),
 311                         btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)),
 312                         btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)),
 313                         btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)),
 314                         btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)),
 315                         btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)),
 316                         btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)),
 317                         btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)),
 318                         btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)),
 319                         btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)),
 320                         btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)),
 321                         btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)),
 322                         btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)),
 323                         btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)),
 324                         btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)),
 325                         btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)),
 326                         btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)),
 327                         btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)),
 328                         btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)),
 329                         btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)),
 330                         btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)),
 331                         btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)),
 332                         btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)),
 333                         btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))};
 334
 335         return sPenetrationDirections;
 336 }