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