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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 * The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   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 ///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library.
  17 ///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross
  18
  19 #include "btBox2dBox2dCollisionAlgorithm.h"
  20 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  21 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  22 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  23 #include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
  24 #include "BulletCollision/CollisionShapes/btBox2dShape.h"
  25 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  26
  27 #define USE_PERSISTENT_CONTACTS 1
  28
  29 btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap)
  30         : btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap),
  31           m_ownManifold(false),
  32           m_manifoldPtr(mf)
  33 {
  34         if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()))
  35         {
  36                 m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject());
  37                 m_ownManifold = true;
  38         }
  39 }
  40
  41 btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
  42 {
  43         if (m_ownManifold)
  44         {
  45                 if (m_manifoldPtr)
  46                         m_dispatcher->releaseManifold(m_manifoldPtr);
  47         }
  48 }
  49
  50 void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
  51
  52 //#include <stdio.h>
  53 void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
  54 {
  55         if (!m_manifoldPtr)
  56                 return;
  57
  58         const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
  59         const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
  60
  61         resultOut->setPersistentManifold(m_manifoldPtr);
  62
  63         b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform());
  64
  65         //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
  66         if (m_ownManifold)
  67         {
  68                 resultOut->refreshContactPoints();
  69         }
  70 }
  71
  72 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
  73 {
  74         //not yet
  75         return 1.f;
  76 }
  77
  78 struct ClipVertex
  79 {
  80         btVector3 v;
  81         int id;
  82         //b2ContactID id;
  83         //b2ContactID id;
  84 };
  85
  86 #define b2Dot(a, b) (a).dot(b)
  87 #define b2Mul(a, b) (a) * (b)
  88 #define b2MulT(a, b) (a).transpose() * (b)
  89 #define b2Cross(a, b) (a).cross(b)
  90 #define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f)
  91
  92 int b2_maxManifoldPoints = 2;
  93
  94 static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
  95                                                          const btVector3& normal, btScalar offset)
  96 {
  97         // Start with no output points
  98         int numOut = 0;
  99
 100         // Calculate the distance of end points to the line
 101         btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
 102         btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
 103
 104         // If the points are behind the plane
 105         if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
 106         if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
 107
 108         // If the points are on different sides of the plane
 109         if (distance0 * distance1 < 0.0f)
 110         {
 111                 // Find intersection point of edge and plane
 112                 btScalar interp = distance0 / (distance0 - distance1);
 113                 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
 114                 if (distance0 > 0.0f)
 115                 {
 116                         vOut[numOut].id = vIn[0].id;
 117                 }
 118                 else
 119                 {
 120                         vOut[numOut].id = vIn[1].id;
 121                 }
 122                 ++numOut;
 123         }
 124
 125         return numOut;
 126 }
 127
 128 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
 129 static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
 130                                                            const btBox2dShape* poly2, const btTransform& xf2)
 131 {
 132         const btVector3* vertices1 = poly1->getVertices();
 133         const btVector3* normals1 = poly1->getNormals();
 134
 135         int count2 = poly2->getVertexCount();
 136         const btVector3* vertices2 = poly2->getVertices();
 137
 138         btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
 139
 140         // Convert normal from poly1's frame into poly2's frame.
 141         btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
 142         btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
 143
 144         // Find support vertex on poly2 for -normal.
 145         int index = 0;
 146         btScalar minDot = BT_LARGE_FLOAT;
 147
 148         if (count2 > 0)
 149                 index = (int)normal1.minDot(vertices2, count2, minDot);
 150
 151         btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
 152         btVector3 v2 = b2Mul(xf2, vertices2[index]);
 153         btScalar separation = b2Dot(v2 - v1, normal1World);
 154         return separation;
 155 }
 156
 157 // Find the max separation between poly1 and poly2 using edge normals from poly1.
 158 static btScalar FindMaxSeparation(int* edgeIndex,
 159                                                                   const btBox2dShape* poly1, const btTransform& xf1,
 160                                                                   const btBox2dShape* poly2, const btTransform& xf2)
 161 {
 162         int count1 = poly1->getVertexCount();
 163         const btVector3* normals1 = poly1->getNormals();
 164
 165         // Vector pointing from the centroid of poly1 to the centroid of poly2.
 166         btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
 167         btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
 168
 169         // Find edge normal on poly1 that has the largest projection onto d.
 170         int edge = 0;
 171         btScalar maxDot;
 172         if (count1 > 0)
 173                 edge = (int)dLocal1.maxDot(normals1, count1, maxDot);
 174
 175         // Get the separation for the edge normal.
 176         btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 177         if (s > 0.0f)
 178         {
 179                 return s;
 180         }
 181
 182         // Check the separation for the previous edge normal.
 183         int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
 184         btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
 185         if (sPrev > 0.0f)
 186         {
 187                 return sPrev;
 188         }
 189
 190         // Check the separation for the next edge normal.
 191         int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
 192         btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
 193         if (sNext > 0.0f)
 194         {
 195                 return sNext;
 196         }
 197
 198         // Find the best edge and the search direction.
 199         int bestEdge;
 200         btScalar bestSeparation;
 201         int increment;
 202         if (sPrev > s && sPrev > sNext)
 203         {
 204                 increment = -1;
 205                 bestEdge = prevEdge;
 206                 bestSeparation = sPrev;
 207         }
 208         else if (sNext > s)
 209         {
 210                 increment = 1;
 211                 bestEdge = nextEdge;
 212                 bestSeparation = sNext;
 213         }
 214         else
 215         {
 216                 *edgeIndex = edge;
 217                 return s;
 218         }
 219
 220         // Perform a local search for the best edge normal.
 221         for (;;)
 222         {
 223                 if (increment == -1)
 224                         edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
 225                 else
 226                         edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
 227
 228                 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 229                 if (s > 0.0f)
 230                 {
 231                         return s;
 232                 }
 233
 234                 if (s > bestSeparation)
 235                 {
 236                         bestEdge = edge;
 237                         bestSeparation = s;
 238                 }
 239                 else
 240                 {
 241                         break;
 242                 }
 243         }
 244
 245         *edgeIndex = bestEdge;
 246         return bestSeparation;
 247 }
 248
 249 static void FindIncidentEdge(ClipVertex c[2],
 250                                                          const btBox2dShape* poly1, const btTransform& xf1, int edge1,
 251                                                          const btBox2dShape* poly2, const btTransform& xf2)
 252 {
 253         const btVector3* normals1 = poly1->getNormals();
 254
 255         int count2 = poly2->getVertexCount();
 256         const btVector3* vertices2 = poly2->getVertices();
 257         const btVector3* normals2 = poly2->getNormals();
 258
 259         btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
 260
 261         // Get the normal of the reference edge in poly2's frame.
 262         btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
 263
 264         // Find the incident edge on poly2.
 265         int index = 0;
 266         btScalar minDot = BT_LARGE_FLOAT;
 267         for (int i = 0; i < count2; ++i)
 268         {
 269                 btScalar dot = b2Dot(normal1, normals2[i]);
 270                 if (dot < minDot)
 271                 {
 272                         minDot = dot;
 273                         index = i;
 274                 }
 275         }
 276
 277         // Build the clip vertices for the incident edge.
 278         int i1 = index;
 279         int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
 280
 281         c[0].v = b2Mul(xf2, vertices2[i1]);
 282         //      c[0].id.features.referenceEdge = (unsigned char)edge1;
 283         //      c[0].id.features.incidentEdge = (unsigned char)i1;
 284         //      c[0].id.features.incidentVertex = 0;
 285
 286         c[1].v = b2Mul(xf2, vertices2[i2]);
 287         //      c[1].id.features.referenceEdge = (unsigned char)edge1;
 288         //      c[1].id.features.incidentEdge = (unsigned char)i2;
 289         //      c[1].id.features.incidentVertex = 1;
 290 }
 291
 292 // Find edge normal of max separation on A - return if separating axis is found
 293 // Find edge normal of max separation on B - return if separation axis is found
 294 // Choose reference edge as min(minA, minB)
 295 // Find incident edge
 296 // Clip
 297
 298 // The normal points from 1 to 2
 299 void b2CollidePolygons(btManifoldResult* manifold,
 300                                            const btBox2dShape* polyA, const btTransform& xfA,
 301                                            const btBox2dShape* polyB, const btTransform& xfB)
 302 {
 303         int edgeA = 0;
 304         btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
 305         if (separationA > 0.0f)
 306                 return;
 307
 308         int edgeB = 0;
 309         btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
 310         if (separationB > 0.0f)
 311                 return;
 312
 313         const btBox2dShape* poly1;  // reference poly
 314         const btBox2dShape* poly2;  // incident poly
 315         btTransform xf1, xf2;
 316         int edge1;  // reference edge
 317         unsigned char flip;
 318         const btScalar k_relativeTol = 0.98f;
 319         const btScalar k_absoluteTol = 0.001f;
 320
 321         // TODO_ERIN use "radius" of poly for absolute tolerance.
 322         if (separationB > k_relativeTol * separationA + k_absoluteTol)
 323         {
 324                 poly1 = polyB;
 325                 poly2 = polyA;
 326                 xf1 = xfB;
 327                 xf2 = xfA;
 328                 edge1 = edgeB;
 329                 flip = 1;
 330         }
 331         else
 332         {
 333                 poly1 = polyA;
 334                 poly2 = polyB;
 335                 xf1 = xfA;
 336                 xf2 = xfB;
 337                 edge1 = edgeA;
 338                 flip = 0;
 339         }
 340
 341         ClipVertex incidentEdge[2];
 342         FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
 343
 344         int count1 = poly1->getVertexCount();
 345         const btVector3* vertices1 = poly1->getVertices();
 346
 347         btVector3 v11 = vertices1[edge1];
 348         btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
 349
 350         //btVector3 dv = v12 - v11;
 351         btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
 352         sideNormal.normalize();
 353         btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
 354
 355         v11 = b2Mul(xf1, v11);
 356         v12 = b2Mul(xf1, v12);
 357
 358         btScalar frontOffset = b2Dot(frontNormal, v11);
 359         btScalar sideOffset1 = -b2Dot(sideNormal, v11);
 360         btScalar sideOffset2 = b2Dot(sideNormal, v12);
 361
 362         // Clip incident edge against extruded edge1 side edges.
 363         ClipVertex clipPoints1[2];
 364         clipPoints1[0].v.setValue(0, 0, 0);
 365         clipPoints1[1].v.setValue(0, 0, 0);
 366
 367         ClipVertex clipPoints2[2];
 368         clipPoints2[0].v.setValue(0, 0, 0);
 369         clipPoints2[1].v.setValue(0, 0, 0);
 370
 371         int np;
 372
 373         // Clip to box side 1
 374         np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
 375
 376         if (np < 2)
 377                 return;
 378
 379         // Clip to negative box side 1
 380         np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
 381
 382         if (np < 2)
 383         {
 384                 return;
 385         }
 386
 387         // Now clipPoints2 contains the clipped points.
 388         btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
 389
 390         int pointCount = 0;
 391         for (int i = 0; i < b2_maxManifoldPoints; ++i)
 392         {
 393                 btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
 394
 395                 if (separation <= 0.0f)
 396                 {
 397                         //b2ManifoldPoint* cp = manifold->points + pointCount;
 398                         //btScalar separation = separation;
 399                         //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
 400                         //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
 401
 402                         manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation);
 403
 404                         //                      cp->id = clipPoints2[i].id;
 405                         //                      cp->id.features.flip = flip;
 406                         ++pointCount;
 407                 }
 408         }
 409
 410         //      manifold->pointCount = pointCount;}
 411 }