git.maemo.org Git - blok/blob - Box2D/Source/Collision/b2CollidePoly.cpp

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * 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
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2Collision.h"
  20 #include "Shapes/b2PolygonShape.h"
  21
  22 struct ClipVertex
  23 {
  24         b2Vec2 v;
  25         b2ContactID id;
  26 };
  27
  28 static int32 ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
  29                                           const b2Vec2& normal, float32 offset)
  30 {
  31         // Start with no output points
  32         int32 numOut = 0;
  33
  34         // Calculate the distance of end points to the line
  35         float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
  36         float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
  37
  38         // If the points are behind the plane
  39         if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
  40         if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
  41
  42         // If the points are on different sides of the plane
  43         if (distance0 * distance1 < 0.0f)
  44         {
  45                 // Find intersection point of edge and plane
  46                 float32 interp = distance0 / (distance0 - distance1);
  47                 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
  48                 if (distance0 > 0.0f)
  49                 {
  50                         vOut[numOut].id = vIn[0].id;
  51                 }
  52                 else
  53                 {
  54                         vOut[numOut].id = vIn[1].id;
  55                 }
  56                 ++numOut;
  57         }
  58
  59         return numOut;
  60 }
  61
  62 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
  63 static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
  64                                                           const b2PolygonShape* poly2, const b2XForm& xf2)
  65 {
  66         int32 count1 = poly1->GetVertexCount();
  67         const b2Vec2* vertices1 = poly1->GetVertices();
  68         const b2Vec2* normals1 = poly1->GetNormals();
  69
  70         int32 count2 = poly2->GetVertexCount();
  71         const b2Vec2* vertices2 = poly2->GetVertices();
  72
  73         b2Assert(0 <= edge1 && edge1 < count1);
  74
  75         // Convert normal from poly1's frame into poly2's frame.
  76         b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
  77         b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
  78
  79         // Find support vertex on poly2 for -normal.
  80         int32 index = 0;
  81         float32 minDot = B2_FLT_MAX;
  82
  83         for (int32 i = 0; i < count2; ++i)
  84         {
  85                 float32 dot = b2Dot(vertices2[i], normal1);
  86                 if (dot < minDot)
  87                 {
  88                         minDot = dot;
  89                         index = i;
  90                 }
  91         }
  92
  93         b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
  94         b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
  95         float32 separation = b2Dot(v2 - v1, normal1World);
  96         return separation;
  97 }
  98
  99 // Find the max separation between poly1 and poly2 using edge normals from poly1.
 100 static float32 FindMaxSeparation(int32* edgeIndex,
 101                                                                  const b2PolygonShape* poly1, const b2XForm& xf1,
 102                                                                  const b2PolygonShape* poly2, const b2XForm& xf2)
 103 {
 104         int32 count1 = poly1->GetVertexCount();
 105         const b2Vec2* normals1 = poly1->GetNormals();
 106
 107         // Vector pointing from the centroid of poly1 to the centroid of poly2.
 108         b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
 109         b2Vec2 dLocal1 = b2MulT(xf1.R, d);
 110
 111         // Find edge normal on poly1 that has the largest projection onto d.
 112         int32 edge = 0;
 113         float32 maxDot = -B2_FLT_MAX;
 114         for (int32 i = 0; i < count1; ++i)
 115         {
 116                 float32 dot = b2Dot(normals1[i], dLocal1);
 117                 if (dot > maxDot)
 118                 {
 119                         maxDot = dot;
 120                         edge = i;
 121                 }
 122         }
 123
 124         // Get the separation for the edge normal.
 125         float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 126         if (s > 0.0f)
 127         {
 128                 return s;
 129         }
 130
 131         // Check the separation for the previous edge normal.
 132         int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
 133         float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
 134         if (sPrev > 0.0f)
 135         {
 136                 return sPrev;
 137         }
 138
 139         // Check the separation for the next edge normal.
 140         int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
 141         float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
 142         if (sNext > 0.0f)
 143         {
 144                 return sNext;
 145         }
 146
 147         // Find the best edge and the search direction.
 148         int32 bestEdge;
 149         float32 bestSeparation;
 150         int32 increment;
 151         if (sPrev > s && sPrev > sNext)
 152         {
 153                 increment = -1;
 154                 bestEdge = prevEdge;
 155                 bestSeparation = sPrev;
 156         }
 157         else if (sNext > s)
 158         {
 159                 increment = 1;
 160                 bestEdge = nextEdge;
 161                 bestSeparation = sNext;
 162         }
 163         else
 164         {
 165                 *edgeIndex = edge;
 166                 return s;
 167         }
 168
 169         // Perform a local search for the best edge normal.
 170         for ( ; ; )
 171         {
 172                 if (increment == -1)
 173                         edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
 174                 else
 175                         edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
 176
 177                 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 178                 if (s > 0.0f)
 179                 {
 180                         return s;
 181                 }
 182
 183                 if (s > bestSeparation)
 184                 {
 185                         bestEdge = edge;
 186                         bestSeparation = s;
 187                 }
 188                 else
 189                 {
 190                         break;
 191                 }
 192         }
 193
 194         *edgeIndex = bestEdge;
 195         return bestSeparation;
 196 }
 197
 198 static void FindIncidentEdge(ClipVertex c[2],
 199                                                          const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
 200                                                          const b2PolygonShape* poly2, const b2XForm& xf2)
 201 {
 202         int32 count1 = poly1->GetVertexCount();
 203         const b2Vec2* normals1 = poly1->GetNormals();
 204
 205         int32 count2 = poly2->GetVertexCount();
 206         const b2Vec2* vertices2 = poly2->GetVertices();
 207         const b2Vec2* normals2 = poly2->GetNormals();
 208
 209         b2Assert(0 <= edge1 && edge1 < count1);
 210
 211         // Get the normal of the reference edge in poly2's frame.
 212         b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
 213
 214         // Find the incident edge on poly2.
 215         int32 index = 0;
 216         float32 minDot = B2_FLT_MAX;
 217         for (int32 i = 0; i < count2; ++i)
 218         {
 219                 float32 dot = b2Dot(normal1, normals2[i]);
 220                 if (dot < minDot)
 221                 {
 222                         minDot = dot;
 223                         index = i;
 224                 }
 225         }
 226
 227         // Build the clip vertices for the incident edge.
 228         int32 i1 = index;
 229         int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
 230
 231         c[0].v = b2Mul(xf2, vertices2[i1]);
 232         c[0].id.features.referenceEdge = (uint8)edge1;
 233         c[0].id.features.incidentEdge = (uint8)i1;
 234         c[0].id.features.incidentVertex = 0;
 235
 236         c[1].v = b2Mul(xf2, vertices2[i2]);
 237         c[1].id.features.referenceEdge = (uint8)edge1;
 238         c[1].id.features.incidentEdge = (uint8)i2;
 239         c[1].id.features.incidentVertex = 1;
 240 }
 241
 242 // Find edge normal of max separation on A - return if separating axis is found
 243 // Find edge normal of max separation on B - return if separation axis is found
 244 // Choose reference edge as min(minA, minB)
 245 // Find incident edge
 246 // Clip
 247
 248 // The normal points from 1 to 2
 249 void b2CollidePolygons(b2Manifold* manifold,
 250                                           const b2PolygonShape* polyA, const b2XForm& xfA,
 251                                           const b2PolygonShape* polyB, const b2XForm& xfB)
 252 {
 253         manifold->pointCount = 0;
 254
 255         int32 edgeA = 0;
 256         float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
 257         if (separationA > 0.0f)
 258                 return;
 259
 260         int32 edgeB = 0;
 261         float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
 262         if (separationB > 0.0f)
 263                 return;
 264
 265         const b2PolygonShape* poly1;    // reference poly
 266         const b2PolygonShape* poly2;    // incident poly
 267         b2XForm xf1, xf2;
 268         int32 edge1;            // reference edge
 269         uint8 flip;
 270         const float32 k_relativeTol = 0.98f;
 271         const float32 k_absoluteTol = 0.001f;
 272
 273         // TODO_ERIN use "radius" of poly for absolute tolerance.
 274         if (separationB > k_relativeTol * separationA + k_absoluteTol)
 275         {
 276                 poly1 = polyB;
 277                 poly2 = polyA;
 278                 xf1 = xfB;
 279                 xf2 = xfA;
 280                 edge1 = edgeB;
 281                 flip = 1;
 282         }
 283         else
 284         {
 285                 poly1 = polyA;
 286                 poly2 = polyB;
 287                 xf1 = xfA;
 288                 xf2 = xfB;
 289                 edge1 = edgeA;
 290                 flip = 0;
 291         }
 292
 293         ClipVertex incidentEdge[2];
 294         FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
 295
 296         int32 count1 = poly1->GetVertexCount();
 297         const b2Vec2* vertices1 = poly1->GetVertices();
 298
 299         b2Vec2 v11 = vertices1[edge1];
 300         b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
 301
 302         b2Vec2 dv = v12 - v11;
 303         b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
 304         sideNormal.Normalize();
 305         b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
 306
 307         v11 = b2Mul(xf1, v11);
 308         v12 = b2Mul(xf1, v12);
 309
 310         float32 frontOffset = b2Dot(frontNormal, v11);
 311         float32 sideOffset1 = -b2Dot(sideNormal, v11);
 312         float32 sideOffset2 = b2Dot(sideNormal, v12);
 313
 314         // Clip incident edge against extruded edge1 side edges.
 315         ClipVertex clipPoints1[2];
 316         ClipVertex clipPoints2[2];
 317         int np;
 318
 319         // Clip to box side 1
 320         np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
 321
 322         if (np < 2)
 323                 return;
 324
 325         // Clip to negative box side 1
 326         np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
 327
 328         if (np < 2)
 329                 return;
 330
 331         // Now clipPoints2 contains the clipped points.
 332         manifold->normal = flip ? -frontNormal : frontNormal;
 333
 334         int32 pointCount = 0;
 335         for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
 336         {
 337                 float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
 338
 339                 if (separation <= 0.0f)
 340                 {
 341                         b2ManifoldPoint* cp = manifold->points + pointCount;
 342                         cp->separation = separation;
 343                         cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
 344                         cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
 345                         cp->id = clipPoints2[i].id;
 346                         cp->id.features.flip = flip;
 347                         ++pointCount;
 348                 }
 349         }
 350
 351         manifold->pointCount = pointCount;
 352 }