b2dlite/collide.d

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   3 *
   4 * Permission to use, copy, modify, distribute and sell this software
   5 * and its documentation for any purpose is hereby granted without fee,
   6 * provided that the above copyright notice appear in all copies.
   7 * Erin Catto makes no representations about the suitability
   8 * of this software for any purpose.
   9 * It is provided "as is" without express or implied warranty.
  10 */
  11 module b2dlite.collide;
  12
  13 import b2dlite.arbiter;
  14 import b2dlite.bbody;
  15 import b2dlite.mathutils;
  16
  17
  18 // Box vertex and edge numbering:
  19 //
  20 //        ^ y
  21 //        |
  22 //        e1
  23 //   v2 ------ v1
  24 //    |        |
  25 // e2 |        | e4  --> x
  26 //    |        |
  27 //   v3 ------ v4
  28 //        e3
  29
  30
  31 enum Axis {
  32   FACE_A_X,
  33   FACE_A_Y,
  34   FACE_B_X,
  35   FACE_B_Y,
  36 }
  37
  38
  39 enum EdgeNumbers {
  40   NO_EDGE = 0,
  41   EDGE1,
  42   EDGE2,
  43   EDGE3,
  44   EDGE4,
  45 }
  46
  47
  48 struct ClipVertex {
  49   Vec2 v;
  50   FeaturePair fp;
  51 }
  52
  53
  54 private void Flip (ref FeaturePair fp) {
  55   Swap(fp.e.inEdge1, fp.e.inEdge2);
  56   Swap(fp.e.outEdge1, fp.e.outEdge2);
  57 }
  58
  59
  60 private int ClipSegmentToLine() (ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, in auto ref Vec2 normal, float offset, char clipEdge) {
  61   // start with no output points
  62   int numOut = 0;
  63   // calculate the distance of end points to the line
  64   float distance0 = Dot(normal, vIn[0].v)-offset;
  65   float distance1 = Dot(normal, vIn[1].v)-offset;
  66   // if the points are behind the plane
  67   if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
  68   if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
  69   // if the points are on different sides of the plane
  70   if (distance0*distance1 < 0.0f) {
  71     // find intersection point of edge and plane
  72     float interp = distance0/(distance0-distance1);
  73     vOut[numOut].v = vIn[0].v+interp*(vIn[1].v-vIn[0].v);
  74     if (distance0 > 0.0f) {
  75       vOut[numOut].fp = vIn[0].fp;
  76       vOut[numOut].fp.e.inEdge1 = clipEdge;
  77       vOut[numOut].fp.e.inEdge2 = EdgeNumbers.NO_EDGE;
  78     } else {
  79       vOut[numOut].fp = vIn[1].fp;
  80       vOut[numOut].fp.e.outEdge1 = clipEdge;
  81       vOut[numOut].fp.e.outEdge2 = EdgeNumbers.NO_EDGE;
  82     }
  83     ++numOut;
  84   }
  85   return numOut;
  86 }
  87
  88
  89 private void ComputeIncidentEdge() (ClipVertex[/*2*/] c, in auto ref Vec2 h, in auto ref Vec2 pos, in auto ref Mat22 Rot, in auto ref Vec2 normal) {
  90   // the normal is from the reference box; convert it to the incident boxe's frame and flip sign
  91   Mat22 RotT = Rot.Transpose();
  92   Vec2 n = -(RotT*normal);
  93   Vec2 nAbs = Abs(n);
  94   if (nAbs.x > nAbs.y) {
  95     if (Sign(n.x) > 0.0f) {
  96       c[0].v.Set(h.x, -h.y);
  97       c[0].fp.e.inEdge2 = EdgeNumbers.EDGE3;
  98       c[0].fp.e.outEdge2 = EdgeNumbers.EDGE4;
  99
 100       c[1].v.Set(h.x, h.y);
 101       c[1].fp.e.inEdge2 = EdgeNumbers.EDGE4;
 102       c[1].fp.e.outEdge2 = EdgeNumbers.EDGE1;
 103     } else {
 104       c[0].v.Set(-h.x, h.y);
 105       c[0].fp.e.inEdge2 = EdgeNumbers.EDGE1;
 106       c[0].fp.e.outEdge2 = EdgeNumbers.EDGE2;
 107
 108       c[1].v.Set(-h.x, -h.y);
 109       c[1].fp.e.inEdge2 = EdgeNumbers.EDGE2;
 110       c[1].fp.e.outEdge2 = EdgeNumbers.EDGE3;
 111     }
 112   } else {
 113     if (Sign(n.y) > 0.0f) {
 114       c[0].v.Set(h.x, h.y);
 115       c[0].fp.e.inEdge2 = EdgeNumbers.EDGE4;
 116       c[0].fp.e.outEdge2 = EdgeNumbers.EDGE1;
 117
 118       c[1].v.Set(-h.x, h.y);
 119       c[1].fp.e.inEdge2 = EdgeNumbers.EDGE1;
 120       c[1].fp.e.outEdge2 = EdgeNumbers.EDGE2;
 121     } else {
 122       c[0].v.Set(-h.x, -h.y);
 123       c[0].fp.e.inEdge2 = EdgeNumbers.EDGE2;
 124       c[0].fp.e.outEdge2 = EdgeNumbers.EDGE3;
 125
 126       c[1].v.Set(h.x, -h.y);
 127       c[1].fp.e.inEdge2 = EdgeNumbers.EDGE3;
 128       c[1].fp.e.outEdge2 = EdgeNumbers.EDGE4;
 129     }
 130   }
 131
 132   c[0].v = pos+Rot*c[0].v;
 133   c[1].v = pos+Rot*c[1].v;
 134 }
 135
 136
 137 // the normal points from A to B
 138 int Collide (Contact* contacts, Body bodyA, Body bodyB) {
 139   // setup
 140   Vec2 hA = 0.5f*bodyA.width;
 141   Vec2 hB = 0.5f*bodyB.width;
 142
 143   Vec2 posA = bodyA.position;
 144   Vec2 posB = bodyB.position;
 145
 146   auto RotA = Mat22 (bodyA.rotation);
 147   auto RotB = Mat22 (bodyB.rotation);
 148
 149   Mat22 RotAT = RotA.Transpose();
 150   Mat22 RotBT = RotB.Transpose();
 151
 152   //k8:Vec2 a1 = RotA.col1, a2 = RotA.col2;
 153   //k8:Vec2 b1 = RotB.col1, b2 = RotB.col2;
 154
 155   Vec2 dp = posB-posA;
 156   Vec2 dA = RotAT*dp;
 157   Vec2 dB = RotBT*dp;
 158
 159   Mat22 C = RotAT*RotB;
 160   Mat22 absC = Abs(C);
 161   Mat22 absCT = absC.Transpose();
 162
 163   // Box A faces
 164   Vec2 faceA = Abs(dA)-hA-absC*hB;
 165   if (faceA.x > 0.0f || faceA.y > 0.0f) return 0;
 166
 167   // Box B faces
 168   Vec2 faceB = Abs(dB)-absCT*hA-hB;
 169   if (faceB.x > 0.0f || faceB.y > 0.0f) return 0;
 170
 171   // Find best axis
 172   Axis axis;
 173   float separation;
 174   Vec2 normal;
 175
 176   // Box A faces
 177   axis = Axis.FACE_A_X;
 178   separation = faceA.x;
 179   normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1;
 180
 181   const float relativeTol = 0.95f;
 182   const float absoluteTol = 0.01f;
 183
 184   if (faceA.y > relativeTol*separation+absoluteTol*hA.y) {
 185     axis = Axis.FACE_A_Y;
 186     separation = faceA.y;
 187     normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2;
 188   }
 189
 190   // Box B faces
 191   if (faceB.x > relativeTol*separation+absoluteTol*hB.x) {
 192     axis = Axis.FACE_B_X;
 193     separation = faceB.x;
 194     normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1;
 195   }
 196
 197   if (faceB.y > relativeTol*separation+absoluteTol*hB.y) {
 198     axis = Axis.FACE_B_Y;
 199     separation = faceB.y;
 200     normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2;
 201   }
 202
 203   // Setup clipping plane data based on the separating axis
 204   Vec2 frontNormal, sideNormal;
 205   ClipVertex[2] incidentEdge;
 206   float front, negSide, posSide;
 207   char negEdge, posEdge;
 208
 209   // Compute the clipping lines and the line segment to be clipped.
 210   switch (axis) {
 211     case Axis.FACE_A_X:
 212       frontNormal = normal;
 213       front = Dot(posA, frontNormal)+hA.x;
 214       sideNormal = RotA.col2;
 215       float side = Dot(posA, sideNormal);
 216       negSide = -side+hA.y;
 217       posSide =  side+hA.y;
 218       negEdge = EdgeNumbers.EDGE3;
 219       posEdge = EdgeNumbers.EDGE1;
 220       ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
 221       break;
 222     case Axis.FACE_A_Y:
 223       frontNormal = normal;
 224       front = Dot(posA, frontNormal)+hA.y;
 225       sideNormal = RotA.col1;
 226       float side = Dot(posA, sideNormal);
 227       negSide = -side+hA.x;
 228       posSide =  side+hA.x;
 229       negEdge = EdgeNumbers.EDGE2;
 230       posEdge = EdgeNumbers.EDGE4;
 231       ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
 232       break;
 233     case Axis.FACE_B_X:
 234       frontNormal = -normal;
 235       front = Dot(posB, frontNormal)+hB.x;
 236       sideNormal = RotB.col2;
 237       float side = Dot(posB, sideNormal);
 238       negSide = -side+hB.y;
 239       posSide =  side+hB.y;
 240       negEdge = EdgeNumbers.EDGE3;
 241       posEdge = EdgeNumbers.EDGE1;
 242       ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
 243       break;
 244     case Axis.FACE_B_Y:
 245       frontNormal = -normal;
 246       front = Dot(posB, frontNormal)+hB.y;
 247       sideNormal = RotB.col1;
 248       float side = Dot(posB, sideNormal);
 249       negSide = -side+hB.x;
 250       posSide =  side+hB.x;
 251       negEdge = EdgeNumbers.EDGE2;
 252       posEdge = EdgeNumbers.EDGE4;
 253       ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
 254       break;
 255     default: assert(0);
 256   }
 257
 258   // clip other face with 5 box planes (1 face plane, 4 edge planes)
 259   ClipVertex[2] clipPoints1;
 260   ClipVertex[2] clipPoints2;
 261   int np;
 262
 263   // clip to box side 1
 264   np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge);
 265   if (np < 2) return 0;
 266
 267   // clip to negative box side 1
 268   np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, posSide, posEdge);
 269   if (np < 2) return 0;
 270
 271   // Now clipPoints2 contains the clipping points.
 272   // Due to roundoff, it is possible that clipping removes all points.
 273
 274   int numContacts = 0;
 275   for (int i = 0; i < 2; ++i) {
 276     separation = Dot(frontNormal, clipPoints2[i].v)-front;
 277     if (separation <= 0) {
 278       contacts[numContacts].separation = separation;
 279       contacts[numContacts].normal = normal;
 280       // slide contact point onto reference face (easy to cull)
 281       contacts[numContacts].position = clipPoints2[i].v-separation*frontNormal;
 282       contacts[numContacts].feature = clipPoints2[i].fp;
 283       if (axis == Axis.FACE_B_X || axis == Axis.FACE_B_Y) Flip(contacts[numContacts].feature);
 284       ++numContacts;
 285     }
 286   }
 287
 288   return numContacts;
 289 }