vmath_tests/gjk_test_stephen.d

   1 import arsd.simpledisplay;
   2 import iv.vfs.io;
   3 import iv.vmath;
   4 import iv.vmath_gjk_stephen;
   5
   6 version = want_points;
   7 //version = use_seed;
   8
   9 version(use_seed) enum UseSeed = true; else enum UseSeed = false;
  10
  11
  12 // ////////////////////////////////////////////////////////////////////////// //
  13 alias GJK = GJKImpl!vec2;
  14
  15
  16 // ////////////////////////////////////////////////////////////////////////// //
  17 final class Body2D(VT) if (IsVectorDim!(VT, 2)) {
  18   VT[] verts; // vertices
  19   VT[] norms; // normals
  20
  21   // GJK interface
  22   int vertCount () const nothrow @nogc { pragma(inline, true); return cast(int)verts.length; }
  23   VT vert (int idx) const nothrow @nogc { pragma(inline, true); return verts[idx]; }
  24   int ringCount () const nothrow @nogc { pragma(inline, true); return cast(int)(verts.length+1); }
  25   int ring (int idx) const nothrow @nogc { pragma(inline, true); return (idx < verts.length ? idx : -1); }
  26
  27   void setVerts (const(VT)[] aaverts, VT.Float arot=0) {
  28     // no hulls with less than 3 vertices (ensure actual polygon)
  29     if (aaverts.length < 3) throw new Exception("degenerate body");
  30
  31     // rotate vertices
  32     VT[] averts;
  33     averts.reserve(aaverts.length);
  34     auto rmat = Mat3!VT.Rotate(arot);
  35     foreach (const ref v; aaverts) averts ~= rmat*v;
  36
  37     // find the right most point on the hull
  38     int rightMost = 0;
  39     VT.Float highestXCoord = averts[0].x;
  40     foreach (immutable i; 1..averts.length) {
  41       VT.Float x = averts[i].x;
  42       if (x > highestXCoord) {
  43         highestXCoord = x;
  44         rightMost = cast(int)i;
  45       } else if (x == highestXCoord) {
  46         // if matching x then take farthest negative y
  47         if (averts[i].y < averts[rightMost].y) rightMost = cast(int)i;
  48       }
  49     }
  50
  51     auto hull = new int[](averts.length);
  52     int outCount = 0;
  53     int indexHull = rightMost;
  54     int vcount = 0;
  55
  56     for (;;) {
  57       hull[outCount] = indexHull;
  58
  59       // search for next index that wraps around the hull by computing cross products to
  60       // find the most counter-clockwise vertex in the set, given the previos hull index
  61       int nextHullIndex = 0;
  62       foreach (immutable i; 1..averts.length) {
  63         // skip if same coordinate as we need three unique points in the set to perform a cross product
  64         if (nextHullIndex == indexHull) {
  65           nextHullIndex = i;
  66           continue;
  67         }
  68         // cross every set of three unique vertices
  69         // record each counter clockwise third vertex and add to the output hull
  70         // See : http://www.oocities.org/pcgpe/math2d.html
  71         auto e1 = averts[nextHullIndex]-averts[hull[outCount]];
  72         auto e2 = averts[i]-averts[hull[outCount]];
  73         auto c = e1.cross(e2);
  74         if (c < 0.0f) nextHullIndex = i;
  75
  76         // cross product is zero then e vectors are on same line
  77         // therefore want to record vertex farthest along that line
  78         if (c == 0.0f && e2.lengthSquared > e1.lengthSquared) nextHullIndex = i;
  79       }
  80
  81       ++outCount;
  82       indexHull = nextHullIndex;
  83
  84       // conclude algorithm upon wrap-around
  85       if (nextHullIndex == rightMost) {
  86         vcount = outCount;
  87         break;
  88       }
  89     }
  90     if (vcount < 3) throw new Exception("degenerate body");
  91
  92     // copy vertices into shape's vertices
  93     verts.reserve(vcount);
  94     foreach (immutable i; 0..vcount) verts ~= averts[hull[i]];
  95     if (!isConvex()) throw new Exception("non-convex body");
  96
  97     // compute face normals
  98     norms.reserve(verts.length);
  99     foreach (immutable i1; 0..verts.length) {
 100       immutable i2 = (i1+1)%verts.length;
 101       auto face = verts[i2]-verts[i1];
 102       // ensure no zero-length edges, because that's bad
 103       assert(face.lengthSquared > EPSILON!(VT.Float)*EPSILON!(VT.Float));
 104       // calculate normal with 2D cross product between vector and scalar
 105       norms ~= VT(face.y, -face.x).normalized;
 106     }
 107     assert(isConvex);
 108   }
 109
 110   bool isConvex () const {
 111     static int sign() (VT.Float v) { pragma(inline, true); return (v < 0 ? -1 : v > 0 ? 1 : 0); }
 112     if (verts.length < 3) return false;
 113     if (verts.length == 3) return true; // nothing to check here
 114     int dir;
 115     foreach (immutable idx, const ref v; verts) {
 116       immutable v1 = VT(verts[(idx+1)%verts.length])-v;
 117       immutable v2 = VT(verts[(idx+2)%verts.length]);
 118       int d = sign(v2.x*v1.y-v2.y*v1.x+v1.x*v.y-v1.y*v.x);
 119       if (d == 0) return false;
 120       if (idx) {
 121         if (dir != d) return false;
 122       } else {
 123         dir = d;
 124       }
 125     }
 126     return true;
 127   }
 128
 129   void moveBy() (in auto ref VT delta) {
 130     foreach (ref v; verts) v += delta;
 131   }
 132
 133   void moveBy() (VT.Float dx, VT.Float dy, VT.Float dz=0) { moveBy(VT(dx, dy, dz)); }
 134
 135   bool inside() (in auto ref VT p) const {
 136     static int sign() (VT.Float v) { pragma(inline, true); return (v < 0 ? -1 : v > 0 ? 1 : 0); }
 137     if (verts.length < 3) return false;
 138     int side = 0;
 139     foreach (immutable idx, const ref v; verts) {
 140       immutable as = verts[(idx+1)%verts.length]-v;
 141       immutable ap = p-v;
 142       int d = sign(as.cross(ap));
 143       if (d != 0) {
 144         if (side == 0) side = d; else if (side != d) return false;
 145       }
 146     }
 147     return true;
 148   }
 149
 150   bool inside() (VT.Float x, VT.Float y) const { return inside(VT(x, y)); }
 151 }
 152
 153
 154 // ////////////////////////////////////////////////////////////////////////// //
 155 auto generateBody () {
 156   import std.random;
 157   vec2[] vtx;
 158   foreach (immutable _; 0..uniform!"[]"(10, 50)) vtx ~= vec2(uniform!"[]"(-50, 50), uniform!"[]"(-50, 50));
 159   auto flesh = new Body2D!vec2();
 160   flesh.setVerts(vtx);
 161   return flesh;
 162 }
 163
 164
 165 static assert(IsGoodGJKObject!(Body2D!vec2, vec2));
 166
 167
 168 // ////////////////////////////////////////////////////////////////////////// //
 169 void main () {
 170   auto flesh0 = generateBody();
 171   auto flesh1 = generateBody();
 172
 173   flesh0.moveBy(350, 450);
 174   flesh1.moveBy(250, 350);
 175
 176   auto sdwin = new SimpleWindow(1024, 768, "GJK Test");
 177
 178   int fhigh = -1;
 179   bool checkCollision = true;
 180   GJK gjk;
 181
 182   void repaint () {
 183     auto pt = sdwin.draw();
 184     pt.fillColor = Color.black;
 185     pt.outlineColor = Color.black;
 186     pt.drawRectangle(Point(0, 0), sdwin.width, sdwin.height);
 187     pt.outlineColor = Color.white;
 188
 189     void drawVL(VT) (in auto ref VT v0, in auto ref VT v1) if (IsVectorDim!(VT, 2)) {
 190       pt.drawLine(Point(cast(int)v0.x, cast(int)v0.y), Point(cast(int)v1.x, cast(int)v1.y));
 191     }
 192
 193     void drawBody(BT) (BT flesh) if (is(BT == Body2D!VT, VT)) {
 194       foreach (immutable int idx; 0..cast(int)flesh.verts.length) {
 195         immutable v0 = flesh.verts[idx];
 196         immutable v1 = flesh.verts[(idx+1)%cast(int)flesh.verts.length];
 197         drawVL(v0, v1);
 198       }
 199     }
 200
 201     void drawPoint(VT) (in auto ref VT v) if (IsVector!VT) {
 202       immutable v0 = v-2;
 203       immutable v1 = v+2;
 204       pt.drawEllipse(Point(cast(int)v0.x, cast(int)v0.y), Point(cast(int)v1.x, cast(int)v1.y));
 205     }
 206
 207     bool collided = false;
 208
 209     if (checkCollision) {
 210       import std.math : sqrt;
 211       version(want_points) {
 212         GJK.Vec wpt1, wpt2;
 213         auto res = gjk.distance(flesh0, flesh1, &wpt1, &wpt2, UseSeed);
 214         writeln("res=", res, "; wpt1=", wpt1, "; wpt2=", wpt2, "; d2=", wpt1.distanceSquared(wpt2), "; dist=", sqrt(res));
 215       } else {
 216         auto res = gjk.distance(flesh0, flesh1, UseSeed);
 217         writeln("res=", res, "; dist=", sqrt(res));
 218       }
 219       writeln("  disp: ", gjk.simplex.disp);
 220       if (res < GJK.EPS) collided = true;
 221       pt.outlineColor = Color.red;
 222       version(want_points) {
 223         drawPoint(wpt1);
 224         drawPoint(wpt2);
 225       }
 226     }
 227
 228     pt.outlineColor = (fhigh == 0 ? Color.green : collided ? Color.red : Color.white);
 229     drawBody(flesh0);
 230     pt.outlineColor = (fhigh == 1 ? Color.green : collided ? Color.red : Color.white);
 231     drawBody(flesh1);
 232
 233     version(want_points) {
 234     } else {
 235       pt.outlineColor = Color.yellow;
 236       drawPoint(gjk.extractPoint(0));
 237       drawPoint(gjk.extractPoint(1));
 238     }
 239   }
 240
 241   repaint();
 242   sdwin.eventLoop(0,
 243     delegate (KeyEvent event) {
 244       if (!event.pressed) return;
 245       if (event == "C-Q") { sdwin.close(); return; }
 246     },
 247     delegate (MouseEvent event) {
 248       int oldhi = fhigh;
 249       if (event.type == MouseEventType.buttonPressed && event.button == MouseButton.left) {
 250              if (flesh0.inside(event.x, event.y)) fhigh = 0;
 251         else if (flesh1.inside(event.x, event.y)) fhigh = 1;
 252         else fhigh = -1;
 253         if (oldhi != fhigh) {
 254           checkCollision = (fhigh == -1);
 255           repaint();
 256         }
 257         return;
 258       }
 259       if (fhigh != -1 && event.type == MouseEventType.motion && (event.modifierState&ModifierState.leftButtonDown) != 0) {
 260              if (fhigh == 0) flesh0.moveBy(event.dx, event.dy);
 261         else if (fhigh == 1) flesh1.moveBy(event.dx, event.dy);
 262         checkCollision = (fhigh == -1);
 263         repaint();
 264         return;
 265       }
 266       if (event.type == MouseEventType.buttonReleased && event.button == MouseButton.left) {
 267         if (fhigh != -1) {
 268           fhigh = -1;
 269           checkCollision = true;
 270           repaint();
 271         }
 272       }
 273     },
 274   );
 275 }