vmath_tests/gjk_test_simple3d.d

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