vmath_tests/gjk_test_simple2d.d

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