obsolete_gjk/vmath_gjk2d_simple.d

   1 /* Gilbert-Johnson-Keerthi intersection algorithm with Expanding Polytope Algorithm
   2  * coded by Ketmar // Invisible Vector <ketmar@ketmar.no-ip.org>
   3  * Understanding is not required. Only obedience.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, version 3 of the License ONLY.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 module iv.vmath_gjk2d_simple;
  18
  19 import iv.vmath;
  20
  21 version = gjk_warnings;
  22
  23 // ////////////////////////////////////////////////////////////////////////// //
  24 /* GJK object should support:
  25  *   Vec2 position
  26  *   Vec2 support (Vec2 dir) -- in world space
  27  *
  28  * for polyhedra:
  29  *
  30  * vec2 position () const { return centroid; }
  31  *
  32  * // dumb O(n) support function, just brute force check all points
  33  * vec2 support() (in auto ref vec2 dir) const {
  34  *   VT furthestPoint = verts[0];
  35  *   auto maxDot = furthestPoint.dot(dir);
  36  *   foreach (const ref v; verts[1..$]) {
  37  *     auto d = v.dot(dir);
  38  *     if (d > maxDot) {
  39  *       maxDot = d;
  40  *       furthestPoint = v;
  41  *     }
  42  *   }
  43  *   return furthestPoint;
  44  * }
  45  */
  46 public template IsGoodGJKObject(T, VT) if ((is(T == struct) || is(T == class)) && IsVectorDim!(VT, 2)) {
  47   enum IsGoodGJKObject = is(typeof((inout int=0) {
  48     const o = T.init;
  49     VT v = o.position;
  50     v = o.support(VT(0, 0));
  51   }));
  52 }
  53
  54
  55 // ////////////////////////////////////////////////////////////////////////// //
  56 /// check if two convex shapes are colliding. can optionally return separating vector in `sepmove`.
  57 public bool gjk(CT, VT) (in auto ref CT coll1, in auto ref CT coll2, VT* sepmove=null) if (IsGoodGJKObject!(CT, VT)) {
  58   VT sdir = coll2.position-coll1.position; // initial search direction
  59   if (sdir.isZero) sdir = VT(1, 0); // use arbitrary normal if initial direction is zero
  60
  61   VT[3] spx = void; // simplex; [0] is most recently added, [2] is oldest
  62
  63   spx.ptr[2] = getSupportPoint(coll1, coll2, sdir);
  64   if (spx.ptr[2].dot(sdir) <= 0) return false; // past the origin
  65
  66   sdir = -sdir; // change search direction
  67
  68   int spxidx = 1;
  69   for (;;) {
  70     spx.ptr[spxidx] = getSupportPoint(coll1, coll2, sdir);
  71     if (spx.ptr[spxidx].dot(sdir) <= 0) return false; // past the origin
  72     if (checkSimplex(sdir, spx[spxidx..$])) {
  73       if (sepmove !is null) *sepmove = EPA(coll1, coll2, spx[spxidx..$]);
  74       return true;
  75     }
  76     spxidx = 0;
  77   }
  78
  79   return false;
  80 }
  81
  82
  83 // ////////////////////////////////////////////////////////////////////////// //
  84 /// return distance between two convex shapes, and separation normal
  85 /// negative distance means that shapes are overlapping, and zero distance means touching (and ops are invalid)
  86 public auto gjkdist(CT, VT) (in auto ref CT coll1, in auto ref CT coll2, VT* op0=null, VT* op1=null, VT* sepnorm=null) if (IsGoodGJKObject!(CT, VT)) {
  87   static VT segClosestToOrigin() (in auto ref VT segp0, in auto ref VT segp1) {
  88     immutable oseg = segp1-segp0;
  89     immutable ab2 = oseg.dot(oseg);
  90     immutable apab = (-segp0).dot(oseg);
  91     if (ab2 <= EPSILON!(VT.Float)) return segp0;
  92     VT.Float t = apab/ab2;
  93     if (t < 0) t = 0; else if (t > 1) t = 1;
  94     return segp0+oseg*t;
  95   }
  96
  97   enum GetSupport(string smpx) =
  98     smpx~"p0 = coll1.support(d);\n"~
  99     smpx~"p1 = coll2.support(-d);\n"~
 100     smpx~" = "~smpx~"p0-"~smpx~"p1;";
 101
 102   if (sepnorm !is null) *sepnorm = VT(0, 0);
 103   if (op0 !is null) *op0 = VT(0, 0);
 104   if (op1 !is null) *op1 = VT(0, 0);
 105
 106   VT a, b, c; // simplex
 107   VT ap0, bp0, cp0; // simplex support points, needed for closest points calculation
 108   VT ap1, bp1, cp1; // simplex support points, needed for closest points calculation
 109   // position is centroid, use that fact
 110   auto d = coll2.position-coll1.position;
 111   // check for a zero direction vector
 112   if (d.isZero) return cast(VT.Float)-1; // centroids are the same, not separated
 113   //getSupport(a, ap0, ap1, d);
 114   mixin(GetSupport!"a");
 115   d = -d;
 116   mixin(GetSupport!"b");
 117   d = segClosestToOrigin(b, a);
 118   foreach (immutable iter; 0..100) {
 119     if (d.lengthSquared <= EPSILON!(VT.Float)) return cast(VT.Float)-1; // if the closest point is the origin, not separated
 120     d = -d;
 121     mixin(GetSupport!"c");
 122     // is simplex triangle contains origin?
 123     immutable sa = a.cross(b);
 124     if (sa*b.cross(c) > 0 && sa*c.cross(a) > 0) return cast(VT.Float)-1; // yes, not separated
 125     if (c.dot(d)-a.dot(d) < EPSILON!(VT.Float)*EPSILON!(VT.Float)) break; // new point is not far enough, we found her!
 126     auto p0 = segClosestToOrigin(a, c);
 127     auto p1 = segClosestToOrigin(c, b);
 128     immutable p0sqlen = p0.lengthSquared;
 129     immutable p1sqlen = p1.lengthSquared;
 130     if (p0sqlen <= EPSILON!(VT.Float) || p1sqlen <= EPSILON!(VT.Float)) {
 131       // origin is very close, but not exactly on edge; assume zero distance (special case)
 132       if (sepnorm !is null) *sepnorm = d.normalized;
 133       return cast(VT.Float)0;
 134     }
 135     if (p0sqlen < p1sqlen) { b = c; bp0 = cp0; bp1 = cp1; d = p0; } else { a = c; ap0 = cp0; ap1 = cp1; d = p1; }
 136   }
 137   // either out of iterations, or new point was not far enough
 138   d.normalize;
 139   auto dist = -c.dot(d);
 140   // get closest points
 141   if (op0 !is null || op1 !is null) {
 142     auto l = b-a;
 143     if (l.isZero) {
 144       if (op0 !is null) *op0 = ap0;
 145       if (op1 !is null) *op1 = ap1;
 146     } else {
 147       immutable ll = l.dot(l);
 148       immutable l2 = -l.dot(a)/ll;
 149       immutable l1 = cast(VT.Float)1-l2;
 150       if (l1 < 0) {
 151         if (op0 !is null) *op0 = bp0;
 152         if (op1 !is null) *op1 = bp1;
 153       } else if (l2 < 0) {
 154         if (op0 !is null) *op0 = ap0;
 155         if (op1 !is null) *op1 = ap1;
 156       } else {
 157         if (op0 !is null) *op0 = ap0*l1+bp0*l2;
 158         if (op1 !is null) *op1 = ap1*l1+bp1*l2;
 159       }
 160     }
 161   }
 162   if (dist < 0) { d = -d; dist = -dist; }
 163   if (sepnorm !is null) *sepnorm = d;
 164   return dist;
 165 }
 166
 167
 168 // ////////////////////////////////////////////////////////////////////////// //
 169 // return the Minkowski sum point (ok, something *like* it, but not Minkowski difference yet ;-)
 170 private VT getSupportPoint(CT, VT) (in ref CT coll1, in ref CT coll2, in ref VT sdir) {
 171   pragma(inline, true);
 172   return coll1.support(sdir)-coll2.support(-sdir);
 173 }
 174
 175
 176 // check if simplex contains origin, update sdir, and possibly update simplex
 177 private bool checkSimplex(VT) (ref VT sdir, VT[] spx) {
 178   assert(spx.length == 2 || spx.length == 3);
 179   if (spx.length == 3) {
 180     // simplex has 3 elements
 181     auto a = spx.ptr[0]; // last added point
 182     auto ao = -a; // to origin
 183     // get the edges
 184     auto ab = spx.ptr[1]-a;
 185     auto ac = spx.ptr[2]-a;
 186     // get the edge normals
 187     auto abn = ac.tripleProduct(ab, ab);
 188     auto acn = ab.tripleProduct(ac, ac);
 189     // see where the origin is at
 190     auto acloc = acn.dot(ao);
 191     if (acloc >= 0) {
 192       // remove middle element
 193       spx.ptr[1] = spx.ptr[0];
 194       sdir = acn;
 195     } else {
 196       auto abloc = abn.dot(ao);
 197       if (abloc < 0) return true; // intersection
 198       // remove last element
 199       spx.ptr[2] = spx.ptr[1];
 200       spx.ptr[1] = spx.ptr[0];
 201       sdir = abn;
 202     }
 203   } else {
 204     // simplex has 2 elements
 205     auto a = spx.ptr[0]; // last added point
 206     auto ao = -a; // to origin
 207     auto ab = spx.ptr[1]-a;
 208     sdir = ab.tripleProduct(ao, ab);
 209     if (sdir.lengthSquared <= EPSILON!(VT.Float)) sdir = sdir.rperp; // bad direction, use any normal
 210   }
 211   return false;
 212 }
 213
 214
 215 // ////////////////////////////////////////////////////////////////////////// //
 216 // Expanding Polytope Algorithm
 217 // find minimum translation vector to resolve collision
 218 // using the final simplex obtained with the GJK algorithm
 219 private VT EPA(CT, VT) (in ref CT coll1, in ref CT coll2, const(VT)[] spx...) {
 220   enum MaxIterations = 100;
 221   enum MaxFaces = MaxIterations*3;
 222
 223   static struct SxEdge {
 224     VT p0, p1;
 225     VT normal;
 226     VT.Float dist;
 227
 228   nothrow @safe @nogc:
 229     void calcNormDist (int winding) {
 230       pragma(inline, true);
 231       mixin(ImportCoreMath!(VT.Float, "fabs"));
 232       normal = p1-p0;
 233       if (winding < 0) normal = normal.perp; else normal = normal.rperp;
 234       normal.normalize();
 235       dist = fabs(p0.dot(normal));
 236     }
 237
 238     void set (in ref VT ap0, in ref VT ap1, int winding) {
 239       p0 = ap0;
 240       p1 = ap1;
 241       calcNormDist(winding);
 242     }
 243
 244     this (in ref VT ap0, in ref VT ap1, int winding) { pragma(inline, true); set(ap0, ap1, winding); }
 245   }
 246
 247   // as this cannot be called recursive, we can use thread-local static here
 248   static SxEdge[MaxFaces] faces = void;
 249   int faceCount;
 250
 251   // compute the winding
 252   int winding = 0;
 253   VT prevv = spx[$-1];
 254   foreach (const ref v; spx[]) {
 255     auto cp = prevv.cross(v);
 256     if (cp > 0) { winding = 1; break; }
 257     if (cp < 0) { winding = -1; break; }
 258     prevv = v;
 259   }
 260
 261   // build the initial edge queue
 262   prevv = spx[$-1];
 263   foreach (const ref v; spx[]) {
 264     faces.ptr[faceCount++].set(prevv, v, winding);
 265     prevv = v;
 266   }
 267
 268   void extractClosestEdge (ref SxEdge eres) {
 269     import core.stdc.string : memmove;
 270     int res = 0;
 271     auto lastDist = VT.Float.infinity;
 272     foreach (immutable idx, const ref SxEdge e; faces[0..faceCount]) {
 273       if (e.dist < lastDist) { res = cast(int)idx; lastDist = e.dist; }
 274     }
 275     eres = faces.ptr[res];
 276     if (faceCount-res > 1) memmove(faces.ptr+res, faces.ptr+res+1, (faceCount-res-1)*SxEdge.sizeof);
 277     --faceCount;
 278   }
 279
 280   SxEdge e;
 281   VT p;
 282   foreach (immutable i; 0..MaxIterations) {
 283     extractClosestEdge(e);
 284     p = getSupportPoint(coll1, coll2, e.normal);
 285     immutable proj = p.dot(e.normal);
 286     if (proj-e.dist < EPSILON!(VT.Float)/* *EPSILON!(VT.Float) */) return e.normal*proj;
 287     if (faces.length-faceCount < 2) assert(0, "out of memory in GJK-EPA");
 288     faces.ptr[faceCount++].set(e.p0, p, winding);
 289     faces.ptr[faceCount++].set(p, e.p1, winding);
 290   }
 291   version(gjk_warnings) { import core.stdc.stdio; stderr.fprintf("EPA: out of iterations!\n"); }
 292   return e.normal*p.dot(e.normal);
 293 }