b2dlite.d

   1 /*
   2  * Copyright (c) 2006-2007 Erin Catto http://box2d.org
   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  * Changes by Ketmar // Invisible Vector
  12  * basically, i replaced boxes with convex polygon bodies, and added
  13  * universal SAT solver, based on Randy Gaul code
  14  */
  15 module b2dlite;
  16 private:
  17
  18 public import iv.vmath;
  19
  20 version = b2dlite_use_bvh;
  21 version = b2dlite_bvh_many_asserts;
  22
  23
  24 // ////////////////////////////////////////////////////////////////////////// //
  25 public alias Vec2 = VecN!(2, float);
  26 public alias VFloat = Vec2.VFloat;
  27 public alias VFloatNum = Vec2.VFloatNum;
  28
  29
  30 // ////////////////////////////////////////////////////////////////////////// //
  31 public struct BodyContact {
  32   Vec2 position;
  33   Vec2 normal;
  34   VFloat separation;
  35 }
  36
  37 public void delegate (in ref BodyContact ctx) b2dlDrawContactsCB;
  38
  39
  40 // ////////////////////////////////////////////////////////////////////////// //
  41 public struct Mat22 {
  42 nothrow @safe @nogc:
  43 public:
  44   Vec2 col1, col2;
  45
  46 public:
  47   this (VFloat angle) { pragma(inline, true); set(angle); }
  48
  49   void set (VFloat angle) {
  50     pragma(inline, true);
  51     import core.stdc.math : cosf, sinf;
  52     immutable VFloat c = cosf(angle), s = sinf(angle);
  53     col1.x =  c; col1.y = s;
  54     col2.x = -s; col2.y = c;
  55   }
  56
  57 pure:
  58   this() (in auto ref Vec2 acol1, in auto ref Vec2 acol2) { pragma(inline, true); col1 = acol1; col2 = acol2; }
  59
  60   Mat22 transpose () const { pragma(inline, true); return Mat22(Vec2(col1.x, col2.x), Vec2(col1.y, col2.y)); }
  61
  62   Mat22 invert () const {
  63     pragma(inline, true);
  64     immutable VFloat a = col1.x, b = col2.x, c = col1.y, d = col2.y;
  65     Mat22 bm;
  66     VFloat det = a*d-b*c;
  67     assert(det != VFloatNum!0);
  68     det = VFloatNum!1/det;
  69     bm.col1.x = det*d;
  70     bm.col2.x = -det*b;
  71     bm.col1.y = -det*c;
  72     bm.col2.y = det*a;
  73     return bm;
  74   }
  75
  76   Vec2 opBinary(string op:"*") (in auto ref Vec2 v) const { pragma(inline, true); return Vec2(col1.x*v.x+col2.x*v.y, col1.y*v.x+col2.y*v.y); }
  77
  78   Mat22 opBinary(string op:"+") (in auto ref Mat22 bm) const { pragma(inline, true); return Mat22(col1+bm.col1, col2+bm.col2); }
  79   Mat22 opBinary(string op:"*") (in auto ref Mat22 bm) const { pragma(inline, true); return Mat22(this*bm.col1, this*bm.col2); }
  80
  81   Mat22 abs() () { pragma(inline, true); return Mat22(col1.abs, col2.abs); }
  82 }
  83
  84
  85 // ////////////////////////////////////////////////////////////////////////// //
  86 private Vec2 fcross() (VFloat s, in auto ref Vec2 a) { pragma(inline, true); return Vec2(-s*a.y, s*a.x); }
  87
  88
  89 // ////////////////////////////////////////////////////////////////////////// //
  90 // world
  91 public final class World {
  92 private:
  93   static struct ArbiterKey {
  94     BodyBase body0, body1;
  95
  96     this (BodyBase b0, BodyBase b1) { if (b0 < b1) { body0 = b0; body1 = b1; } else { body0 = b1; body1 = b0; } }
  97
  98     bool opEquals() (in auto ref ArbiterKey b) const {
  99       pragma(inline, true);
 100       return (body0 == b.body0 && body1 == b.body1);
 101     }
 102
 103     int opCmp() (in auto ref ArbiterKey b) const {
 104       if (body0 is null) {
 105         if (b.body0 !is null) return -1;
 106         if (body1 is null) return (b.body1 !is null ? -1 : 0);
 107         return body1.opCmp(b.body1);
 108       }
 109       if (int r = body0.opCmp(b.body0)) return r;
 110       if (body1 is null) return (b.body1 !is null ? -1 : 0);
 111       return body1.opCmp(b.body1);
 112     }
 113
 114     size_t toHash () {
 115       return hashu32((body0 !is null ? body0.midx : 0))^hashu32((body1 !is null ? body1.midx : 0));
 116     }
 117
 118   static:
 119     uint hashu32 (uint a) pure nothrow @safe @nogc {
 120       a -= (a<<6);
 121       a ^= (a>>17);
 122       a -= (a<<9);
 123       a ^= (a<<4);
 124       a -= (a<<3);
 125       a ^= (a<<10);
 126       a ^= (a>>15);
 127       return a;
 128     }
 129   }
 130
 131 private:
 132   version(b2dlite_use_bvh) {
 133     DynamicAABBTree bvh;
 134   }
 135
 136   // start from frame 1, to automatically make all new objects invalid even if no step was done
 137   uint frameNum = 1;
 138
 139 public:
 140   BodyBase[] bodies;
 141   Joint[] joints;
 142   Arbiter[ArbiterKey] arbiters;
 143   Vec2 gravity;
 144   int iterations;
 145
 146   // the following are world options
 147   static bool accumulateImpulses = true;
 148   static bool warmStarting = true;
 149   static bool positionCorrection = true;
 150
 151 public:
 152   this() (in auto ref Vec2 agravity, int aiterations) {
 153     gravity = agravity;
 154     iterations = aiterations;
 155     version(b2dlite_use_bvh) bvh = new DynamicAABBTree(VFloatNum!0.2); // gap
 156   }
 157
 158   void add (BodyBase bbody) {
 159     if (bbody !is null) {
 160       if (bbody.mWorld is this) return; // nothing to do, it is already here
 161       if (bbody.mWorld !is null) throw new Exception("body cannot be owned by two worlds");
 162       bbody.mWorld = this;
 163       bodies ~= bbody;
 164       version(b2dlite_use_bvh) bbody.nodeId = bvh.addObject(bbody);
 165     }
 166   }
 167
 168   void add (Joint joint) {
 169     if (joint !is null) {
 170       if (joint.mWorld is this) return; // nothing to do, it is already here
 171       if (joint.mWorld !is null) throw new Exception("joint cannot be owned by two worlds");
 172       joint.mWorld = this;
 173       joints ~= joint;
 174     }
 175   }
 176
 177   void opOpAssign(string op:"~", T) (T v) if (is(T : BodyBase) || is(T : Joint)) { add(v); }
 178
 179   void clear () {
 180     bodies = null;
 181     joints = null;
 182     arbiters.clear();
 183     version(b2dlite_use_bvh) bvh.reset();
 184   }
 185
 186   void step (VFloat dt) {
 187     if (dt <= VFloatNum!0) return;
 188     ++frameNum; // new frame (used to track arbiters)
 189     VFloat invDt = (dt > VFloatNum!0 ? VFloatNum!1/dt : VFloatNum!0);
 190     // determine overlapping bodies and update contact points
 191     broadPhase();
 192     // integrate forces
 193     foreach (BodyBase b; bodies) {
 194       if (b.invMass == VFloatNum!0) continue;
 195       b.velocity += (gravity+b.force*b.invMass)*dt;
 196       b.angularVelocity += dt*b.invI*b.torque;
 197     }
 198     // perform pre-steps
 199     foreach (ref Arbiter arb; arbiters.byValue) {
 200       if (arb.frameNum == frameNum && arb.active) arb.preStep(invDt);
 201     }
 202     foreach (Joint j; joints) j.preStep(invDt);
 203     // perform iterations
 204     foreach (immutable itnum; 0..iterations) {
 205       foreach (ref Arbiter arb; arbiters.byValue) {
 206         if (arb.frameNum == frameNum && arb.active) arb.applyImpulse();
 207       }
 208       foreach (Joint j; joints) j.applyImpulse();
 209     }
 210     // integrate velocities
 211     foreach (BodyBase b; bodies) {
 212       auto disp = b.velocity*dt;
 213       b.mPosition += b.velocity*dt;
 214       b.mRotation += b.angularVelocity*dt;
 215       b.force.set(VFloatNum!0, VFloatNum!0);
 216       b.torque = VFloatNum!0;
 217       version(b2dlite_use_bvh) bvh.updateObject(b.nodeId, disp);
 218     }
 219   }
 220
 221   void broadPhase () {
 222     version(b2dlite_use_bvh) {
 223       foreach (immutable idx, BodyBase bi; bodies) {
 224         auto aabb = bi.getAABB();
 225         bvh.reportAllShapesOverlappingWithAABB(aabb, (int nodeId) {
 226           auto bj = bvh.getNodeBody(nodeId);
 227           if (bj == bi) return;
 228           if (bi.invMass == VFloatNum!0 && bj.invMass == VFloatNum!0) return;
 229           auto ak = ArbiterKey(bi, bj);
 230           if (auto arb = ak in arbiters) {
 231             if (arb.frameNum != frameNum) arb.setup(bi, bj, frameNum);
 232           } else {
 233             arbiters[ak] = Arbiter(bi, bj, frameNum);
 234           }
 235         });
 236       }
 237     } else {
 238       // O(n^2) broad-phase
 239       foreach (immutable idx, BodyBase bi; bodies) {
 240         foreach (BodyBase bj; bodies[idx+1..$]) {
 241           if (bi.invMass == VFloatNum!0 && bj.invMass == VFloatNum!0) continue;
 242           if (auto arb = ArbiterKey(bi, bj) in arbiters) {
 243             arb.setup(bi, bj, frameNum);
 244           } else {
 245             arbiters[ArbiterKey(bi, bj)] = Arbiter(bi, bj, frameNum);
 246           }
 247         }
 248       }
 249     }
 250   }
 251
 252   void drawBVH (scope void delegate (Vec2 min, Vec2 max) dg) {
 253     version(b2dlite_use_bvh) {
 254       bvh.forEachLeaf((int nodeId, in ref AABB aabb) {
 255         dg(aabb.mMin, aabb.mMax);
 256       });
 257     }
 258   }
 259 }
 260
 261
 262 // ////////////////////////////////////////////////////////////////////////// //
 263 // body
 264 public abstract class BodyBase {
 265 private:
 266   static shared uint lastidx;
 267
 268 private:
 269   uint midx;
 270   Mat22 mRmattmp; // undefined most of the time; used only in coldet for now
 271   uint rmatFrameNum; // when rmattmp was cached last time?
 272   AABB mAABB; // cached AABB
 273   uint aabbFrameNum; // when mAABB was cached last time?
 274   int nodeId; // in bvh
 275   World mWorld;
 276
 277 public:
 278   Vec2 mPosition;
 279   VFloat mRotation;
 280
 281   Vec2 velocity;
 282   VFloat angularVelocity;
 283
 284   Vec2 force;
 285   VFloat torque;
 286
 287   VFloat friction;
 288   VFloat mass, invMass;
 289   VFloat i, invI;
 290
 291 private:
 292   void updateWorld () {
 293     version(b2dlite_use_bvh) {
 294       if (mWorld !is null) {
 295         mWorld.bvh.updateObject(nodeId, Vec2(VFloatNum!0, VFloatNum!0), true); // force reinsert
 296       }
 297     }
 298   }
 299
 300 public:
 301   this () @trusted {
 302     import core.atomic : atomicOp;
 303     midx = atomicOp!"+="(lastidx, 1);
 304
 305     mPosition.set(VFloatNum!0, VFloatNum!0);
 306     mRotation = VFloatNum!0;
 307     velocity.set(VFloatNum!0, VFloatNum!0);
 308     angularVelocity = VFloatNum!0;
 309     force.set(VFloatNum!0, VFloatNum!0);
 310     torque = VFloatNum!0;
 311     friction = VFloatNum!(0.2);
 312     mass = VFloat.max;
 313     invMass = VFloatNum!0;
 314     i = VFloat.max;
 315     invI = VFloatNum!0;
 316   }
 317
 318   @property uint id () const pure nothrow @safe @nogc { pragma(inline, true); return midx; }
 319
 320   void addForce() (in auto ref Vec2 f) pure nothrow @safe @nogc { pragma(inline, true); force += f; }
 321
 322   final ref const(Vec2) position () const pure nothrow @safe @nogc { pragma(inline, true); return mPosition; }
 323   final void position() (in auto ref Vec2 p) { pragma(inline, true); mPosition = p; updateWorld(); }
 324   final void setPosition() (VFloat ax, VFloat ay) { pragma(inline, true); mPosition.set(ax, ay); updateWorld(); }
 325
 326   final VFloat rotation () const pure nothrow @safe @nogc { pragma(inline, true); return mRotation; }
 327   final void rotation() (VFloat aangle) { pragma(inline, true); mRotation = aangle; updateWorld(); }
 328
 329   override bool opEquals (Object b) const pure nothrow @trusted @nogc {
 330     //pragma(inline, true);
 331     if (b is null) return false;
 332     if (b is this) return true;
 333     if (auto bb = cast(BodyBase)b) return (bb.midx == midx);
 334     return false;
 335   }
 336
 337   override int opCmp (Object b) const pure nothrow @trusted @nogc {
 338     //pragma(inline, true);
 339     if (b is null) return 1;
 340     if (b is this) return 0;
 341     if (auto bb = cast(BodyBase)b) return (midx < bb.midx ? -1 : midx > bb.midx ? 1 : 0);
 342     return -1;
 343   }
 344
 345   override size_t toHash () nothrow @safe @nogc {
 346     return hashu32(midx);
 347   }
 348
 349   // get (and cache) AABB
 350   final ref const(AABB) getAABB () {
 351     pragma(inline, true);
 352     if (mWorld is null || aabbFrameNum != mWorld.frameNum) {
 353       // cache rotation matrix
 354       mAABB = recalcAABB();
 355       if (mWorld !is null) aabbFrameNum = mWorld.frameNum;
 356     }
 357     return mAABB;
 358   }
 359
 360   // called to calculate new AABB for frame
 361   abstract AABB recalcAABB ();
 362
 363   // get (and cache) rotation matrix
 364   public final ref const(Mat22) rmat () nothrow @safe @nogc {
 365     pragma(inline, true);
 366     if (mWorld is null || rmatFrameNum != mWorld.frameNum) {
 367       // cache rotation matrix
 368       mRmattmp.set(mRotation);
 369       if (mWorld !is null) rmatFrameNum = mWorld.frameNum;
 370     }
 371     return mRmattmp;
 372   }
 373
 374 static:
 375   uint hashu32 (uint a) pure nothrow @safe @nogc {
 376     a -= (a<<6);
 377     a ^= (a>>17);
 378     a -= (a<<9);
 379     a ^= (a<<4);
 380     a -= (a<<3);
 381     a ^= (a<<10);
 382     a ^= (a>>15);
 383     return a;
 384   }
 385 }
 386
 387
 388 // ////////////////////////////////////////////////////////////////////////// //
 389 public final class PolyBody : BodyBase {
 390 public:
 391   Vec2[] verts; // vertices
 392   Vec2[] norms; // normals
 393
 394 protected:
 395   this () @trusted {
 396     super();
 397     computeMass(VFloatNum!1);
 398   }
 399
 400 public:
 401   this (World w, const(Vec2)[] averts, VFloat adensity, scope void delegate (typeof(this) self) dg=null) {
 402     super();
 403     set(averts, adensity);
 404     if (dg !is null) dg(this);
 405     if (w !is null) w ~= this;
 406   }
 407
 408   static PolyBody Box() (World w, in auto ref Vec2 size, VFloat density, scope void delegate (typeof(this) self, Vec2 size) dg=null) {
 409     auto res = new PolyBody();
 410     res.setBox(size);
 411     res.computeMass(density);
 412     if (dg !is null) dg(res, size);
 413     if (w !is null) w ~= res;
 414     return res;
 415   }
 416
 417   void set() (const(Vec2)[] averts, VFloat adensity) {
 418     mPosition.set(VFloatNum!0, VFloatNum!0);
 419     mRotation = VFloatNum!0;
 420     velocity.set(VFloatNum!0, VFloatNum!0);
 421     angularVelocity = VFloatNum!0;
 422     force.set(VFloatNum!0, VFloatNum!0);
 423     torque = VFloatNum!0;
 424     friction = VFloatNum!(0.2);
 425     setVerts(averts);
 426     computeMass(adensity);
 427     updateWorld();
 428   }
 429
 430   void computeMass(bool densityIsMass=false) (VFloat density) {
 431     // calculate centroid and moment of interia
 432     auto c = Vec2(0, 0); // centroid
 433     VFloat area = 0;
 434     VFloat I = 0;
 435     enum k_inv3 = VFloatNum!1/VFloatNum!3;
 436
 437     foreach (immutable i1; 0..verts.length) {
 438       // triangle vertices, third vertex implied as (0, 0)
 439       auto p1 = verts[i1];
 440       auto i2 = (i1+1)%verts.length;
 441       auto p2 = verts[i2];
 442
 443       VFloat D = p1.cross(p2);
 444       VFloat triangleArea = VFloatNum!(0.5)*D;
 445
 446       area += triangleArea;
 447
 448       // use area to weight the centroid average, not just vertex mPosition
 449       c += triangleArea*k_inv3*(p1+p2);
 450
 451       VFloat intx2 = p1.x*p1.x+p2.x*p1.x+p2.x*p2.x;
 452       VFloat inty2 = p1.y*p1.y+p2.y*p1.y+p2.y*p2.y;
 453       I += (VFloatNum!(0.25)*k_inv3*D)*(intx2+inty2);
 454     }
 455
 456     c *= VFloatNum!1/area;
 457
 458     // translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
 459     // not really necessary, but I like doing this anyway
 460     foreach (ref v; verts) v -= c;
 461
 462     if (area > 0 && density < VFloat.max) {
 463       mass = density*area;
 464       invMass = VFloatNum!1/mass;
 465       //i = mass*(size.x*size.x+size.y*size.y)/VFloatNum!12;
 466       i = I*density;
 467       invI = VFloatNum!1/i;
 468     } else {
 469       mass = VFloat.max;
 470       invMass = VFloatNum!0;
 471       i = VFloat.max;
 472       invI = VFloatNum!0;
 473     }
 474   }
 475
 476   // width and height
 477   private void setBox (Vec2 size) {
 478     size /= VFloatNum!2;
 479     verts.length = 0;
 480     verts ~= Vec2(-size.x, -size.y);
 481     verts ~= Vec2( size.x, -size.y);
 482     verts ~= Vec2( size.x,  size.y);
 483     verts ~= Vec2(-size.x,  size.y);
 484     norms.length = 0;
 485     norms ~= Vec2(VFloatNum!( 0), VFloatNum!(-1));
 486     norms ~= Vec2(VFloatNum!( 1), VFloatNum!( 0));
 487     norms ~= Vec2(VFloatNum!( 0), VFloatNum!( 1));
 488     norms ~= Vec2(VFloatNum!(-1), VFloatNum!( 0));
 489   }
 490
 491   private void setVerts (const(Vec2)[] averts) {
 492     // no hulls with less than 3 vertices (ensure actual polygon)
 493     if (averts.length < 3) throw new Exception("degenerate body");
 494
 495     // find the right most point on the hull
 496     int rightMost = 0;
 497     VFloat highestXCoord = averts[0].x;
 498     foreach (immutable i; 1..averts.length) {
 499       VFloat x = averts[i].x;
 500       if (x > highestXCoord) {
 501         highestXCoord = x;
 502         rightMost = cast(int)i;
 503       } else if (x == highestXCoord) {
 504         // if matching x then take farthest negative y
 505         if (averts[i].y < averts[rightMost].y) rightMost = cast(int)i;
 506       }
 507     }
 508
 509     auto hull = new int[](averts.length);
 510     int outCount = 0;
 511     int indexHull = rightMost;
 512     int vcount = 0;
 513
 514     for (;;) {
 515       hull[outCount] = indexHull;
 516
 517       // search for next index that wraps around the hull by computing cross products to
 518       // find the most counter-clockwise vertex in the set, given the previos hull index
 519       int nextHullIndex = 0;
 520       foreach (immutable i; 1..averts.length) {
 521         // skip if same coordinate as we need three unique points in the set to perform a cross product
 522         if (nextHullIndex == indexHull) {
 523           nextHullIndex = i;
 524           continue;
 525         }
 526         // cross every set of three unique vertices
 527         // record each counter clockwise third vertex and add to the output hull
 528         // See : http://www.oocities.org/pcgpe/math2d.html
 529         auto e1 = averts[nextHullIndex]-averts[hull[outCount]];
 530         auto e2 = averts[i]-averts[hull[outCount]];
 531         auto c = e1.cross(e2);
 532         if (c < 0.0f) nextHullIndex = i;
 533
 534         // cross product is zero then e vectors are on same line
 535         // therefore want to record vertex farthest along that line
 536         if (c == 0.0f && e2.lengthSquared > e1.lengthSquared) nextHullIndex = i;
 537       }
 538
 539       ++outCount;
 540       indexHull = nextHullIndex;
 541
 542       // conclude algorithm upon wrap-around
 543       if (nextHullIndex == rightMost) {
 544         vcount = outCount;
 545         break;
 546       }
 547     }
 548     if (vcount < 3) throw new Exception("degenerate body");
 549
 550     // copy vertices into shape's vertices
 551     foreach (immutable i; 0..vcount) verts ~= averts[hull[i]];
 552     if (!isConvex()) throw new Exception("non-convex body");
 553
 554     // compute face normals
 555     norms.reserve(verts.length);
 556     foreach (immutable i1; 0..verts.length) {
 557       immutable i2 = (i1+1)%verts.length;
 558       auto face = verts[i2]-verts[i1];
 559       // ensure no zero-length edges, because that's bad
 560       assert(face.lengthSquared > FLTEPS*FLTEPS);
 561       // calculate normal with 2D cross product between vector and scalar
 562       norms ~= Vec2(face.y, -face.x).normalized;
 563     }
 564   }
 565
 566   // the extreme point along a direction within a polygon
 567   Vec2 support() (in auto ref Vec2 dir) {
 568     VFloat bestProjection = -VFloat.max;
 569     Vec2 bestVertex;
 570     foreach (immutable i; 0..verts.length) {
 571       Vec2 v = verts[i];
 572       VFloat projection = v.dot(dir);
 573       if (projection > bestProjection) {
 574         bestVertex = v;
 575         bestProjection = projection;
 576       }
 577     }
 578     return bestVertex;
 579   }
 580
 581   bool isConvex () {
 582     static int sign() (VFloat v) { pragma(inline, true); return (v < 0 ? -1 : v > 0 ? 1 : 0); }
 583     if (verts.length < 3) return false;
 584     if (verts.length == 3) return true; // nothing to check here
 585     int dir;
 586     foreach (immutable idx, const ref v; verts) {
 587       auto v1 = Vec2(verts[(idx+1)%verts.length])-v;
 588       auto v2 = Vec2(verts[(idx+2)%verts.length]);
 589       int d = sign(v2.x*v1.y-v2.y*v1.x+v1.x*v.y-v1.y*v.x);
 590       if (d == 0) return false;
 591       if (idx) {
 592         if (dir != d) return false;
 593       } else {
 594         dir = d;
 595       }
 596     }
 597     return true;
 598   }
 599
 600   // return AABB for properly moved and rotated body
 601   override AABB recalcAABB () {
 602     AABB res;
 603     res.mMin = Vec2(float.max, float.max);
 604     res.mMax = Vec2(-float.max, -float.max);
 605     auto rmt = Mat22(mRotation);
 606     foreach (const ref vx; verts) {
 607       import std.algorithm : max, min;
 608       auto vp = mPosition+rmt*vx;
 609       res.mMin.x = min(res.mMin.x, vp.x);
 610       res.mMin.y = min(res.mMin.y, vp.y);
 611       res.mMax.x = max(res.mMax.x, vp.x);
 612       res.mMax.y = max(res.mMax.y, vp.y);
 613     }
 614     return res;
 615   }
 616 }
 617
 618
 619 // ////////////////////////////////////////////////////////////////////////// //
 620 // joint
 621 public final class Joint {
 622 private:
 623   World mWorld;
 624
 625 public:
 626   VFloat biasFactor = VFloatNum!(0.2);
 627   VFloat softness = VFloatNum!0;
 628
 629 public:
 630   Mat22 mt;
 631   Vec2 localAnchor1, localAnchor2;
 632   Vec2 r1, r2;
 633   Vec2 bias;
 634   Vec2 accimp = Vec2(0, 0); // accumulated impulse
 635   BodyBase body0, body1;
 636
 637 public:
 638   this() (World w, BodyBase b0, BodyBase b1, in auto ref Vec2 anchor, scope void delegate (typeof(this) self) dg=null) {
 639     set(b0, b1, anchor);
 640     if (dg !is null) dg(this);
 641     if (w !is null) w ~= this;
 642   }
 643
 644   void set() (BodyBase b0, BodyBase b1, in auto ref Vec2 anchor) {
 645     body0 = b0;
 646     body1 = b1;
 647
 648     auto rot1 = Mat22(body0.mRotation);
 649     auto rot2 = Mat22(body1.mRotation);
 650     Mat22 rot1T = rot1.transpose();
 651     Mat22 rot2T = rot2.transpose();
 652
 653     localAnchor1 = rot1T*(anchor-body0.mPosition);
 654     localAnchor2 = rot2T*(anchor-body1.mPosition);
 655
 656     accimp.set(VFloatNum!0, VFloatNum!0);
 657
 658     softness = VFloatNum!0;
 659     biasFactor = VFloatNum!(0.2);
 660   }
 661
 662   void preStep (VFloat invDt) {
 663     // pre-compute anchors, mass matrix, and bias
 664     auto rot1 = Mat22(body0.mRotation);
 665     auto rot2 = Mat22(body1.mRotation);
 666
 667     r1 = rot1*localAnchor1;
 668     r2 = rot2*localAnchor2;
 669
 670     // deltaV = deltaV0+kmt*impulse
 671     // invM = [(1/m1+1/m2)*eye(2)-skew(r1)*invI1*skew(r1)-skew(r2)*invI2*skew(r2)]
 672     //      = [1/m1+1/m2     0    ]+invI1*[r1.y*r1.y -r1.x*r1.y]+invI2*[r1.y*r1.y -r1.x*r1.y]
 673     //        [    0     1/m1+1/m2]       [-r1.x*r1.y r1.x*r1.x]       [-r1.x*r1.y r1.x*r1.x]
 674     Mat22 k1;
 675     k1.col1.x = body0.invMass+body1.invMass; k1.col2.x = VFloatNum!0;
 676     k1.col1.y = VFloatNum!0; k1.col2.y = body0.invMass+body1.invMass;
 677
 678     Mat22 k2;
 679     k2.col1.x =  body0.invI*r1.y*r1.y; k2.col2.x = -body0.invI*r1.x*r1.y;
 680     k2.col1.y = -body0.invI*r1.x*r1.y; k2.col2.y =  body0.invI*r1.x*r1.x;
 681
 682     Mat22 k3;
 683     k3.col1.x =  body1.invI*r2.y*r2.y; k3.col2.x = -body1.invI*r2.x*r2.y;
 684     k3.col1.y = -body1.invI*r2.x*r2.y; k3.col2.y =  body1.invI*r2.x*r2.x;
 685
 686     Mat22 kmt = k1+k2+k3;
 687     kmt.col1.x += softness;
 688     kmt.col2.y += softness;
 689
 690     mt = kmt.invert();
 691
 692     Vec2 p1 = body0.mPosition+r1;
 693     Vec2 p2 = body1.mPosition+r2;
 694     Vec2 dp = p2-p1;
 695
 696     if (World.positionCorrection) {
 697       bias = -biasFactor*invDt*dp;
 698     } else {
 699       bias.set(VFloatNum!0, VFloatNum!0);
 700     }
 701
 702     if (World.warmStarting) {
 703       // apply accumulated impulse
 704       body0.velocity -= body0.invMass*accimp;
 705       body0.angularVelocity -= body0.invI*r1.cross(accimp);
 706
 707       body1.velocity += body1.invMass*accimp;
 708       body1.angularVelocity += body1.invI*r2.cross(accimp);
 709     } else {
 710       accimp.set(VFloatNum!0, VFloatNum!0);
 711     }
 712   }
 713
 714   void applyImpulse () {
 715     Vec2 dv = body1.velocity+body1.angularVelocity.fcross(r2)-body0.velocity-body0.angularVelocity.fcross(r1);
 716     Vec2 impulse = mt*(bias-dv-softness*accimp);
 717
 718     body0.velocity -= body0.invMass*impulse;
 719     body0.angularVelocity -= body0.invI*r1.cross(impulse);
 720
 721     body1.velocity += body1.invMass*impulse;
 722     body1.angularVelocity += body1.invI*r2.cross(impulse);
 723
 724     accimp += impulse;
 725   }
 726 }
 727
 728
 729 // ////////////////////////////////////////////////////////////////////////// //
 730 private struct Contact {
 731 public:
 732   Vec2 position; // updated in collide()
 733   Vec2 normal; // updated in collide()
 734   Vec2 r1, r2;
 735   VFloat separation = VFloatNum!0; // updated in collide(), negative of penetration
 736   VFloat accimpN = VFloatNum!0; // accumulated normal impulse
 737   VFloat accimpT = VFloatNum!0; // accumulated tangent impulse
 738   VFloat accimpNb = VFloatNum!0;  // accumulated normal impulse for position bias
 739   VFloat massNormal, massTangent;
 740   VFloat bias = VFloatNum!0;
 741 }
 742
 743
 744 // ////////////////////////////////////////////////////////////////////////// //
 745 private struct Arbiter {
 746 public:
 747   enum MaxContactPoints = 2;
 748
 749   static private VFloat clamp() (VFloat a, VFloat low, VFloat high) { pragma(inline, true); import std.algorithm : min, max; return max(low, min(a, high)); }
 750
 751 public:
 752   Contact[MaxContactPoints] contacts;
 753   int numContacts;
 754
 755   BodyBase body0, body1;
 756   VFloat friction; // combined friction
 757   uint frameNum; // used to track "frame touch"
 758
 759 public:
 760   this (BodyBase b0, BodyBase b1, int aFrameNum) { setup(b0, b1, frameNum); }
 761
 762   @disable this (this);
 763
 764   void clear () { numContacts = 0; }
 765
 766   @property bool active () { return (numContacts > 0); }
 767
 768   void setup (BodyBase b0, BodyBase b1, int aFrameNum) {
 769     frameNum = aFrameNum;
 770     import core.stdc.math : sqrtf;
 771     if (b0 < b1) {
 772       body0 = b0;
 773       body1 = b1;
 774     } else {
 775       body0 = b1;
 776       body1 = b0;
 777     }
 778     numContacts = collide(contacts[], body0, body1);
 779     friction = sqrtf(body0.friction*body1.friction);
 780     if (b2dlDrawContactsCB !is null) {
 781       BodyContact bc;
 782       foreach (const ref ctx; contacts[0..numContacts]) {
 783         bc.position = ctx.position;
 784         bc.normal = ctx.normal;
 785         bc.separation = ctx.separation;
 786         b2dlDrawContactsCB(bc);
 787       }
 788     }
 789   }
 790
 791   void preStep (VFloat invDt) {
 792     import std.algorithm : min;
 793     enum AllowedPenetration = VFloatNum!(0.01);
 794     immutable VFloat biasFactor = (World.positionCorrection ? VFloatNum!(0.2) : VFloatNum!0);
 795     foreach (immutable idx; 0..numContacts) {
 796       Contact *c = contacts.ptr+idx;
 797       Vec2 r1 = c.position-body0.mPosition;
 798       Vec2 r2 = c.position-body1.mPosition;
 799
 800       // precompute normal mass, tangent mass, and bias
 801       VFloat rn1 = r1*c.normal; //Dot(r1, c.normal);
 802       VFloat rn2 = r2*c.normal; //Dot(r2, c.normal);
 803       VFloat kNormal = body0.invMass+body1.invMass;
 804       //kNormal += body0.invI*(Dot(r1, r1)-rn1*rn1)+body1.invI*(Dot(r2, r2)-rn2*rn2);
 805       kNormal += body0.invI*((r1*r1)-rn1*rn1)+body1.invI*((r2*r2)-rn2*rn2);
 806       c.massNormal = VFloatNum!1/kNormal;
 807
 808       //Vec2 tangent = c.normal.cross(VFloatNum!1);
 809       Vec2 tangent = Vec2(VFloatNum!1*c.normal.y, -VFloatNum!1*c.normal.x);
 810       VFloat rt1 = r1*tangent; //Dot(r1, tangent);
 811       VFloat rt2 = r2*tangent; //Dot(r2, tangent);
 812       VFloat kTangent = body0.invMass+body1.invMass;
 813       //kTangent += body0.invI*(Dot(r1, r1)-rt1*rt1)+body1.invI*(Dot(r2, r2)-rt2*rt2);
 814       kTangent += body0.invI*((r1*r1)-rt1*rt1)+body1.invI*((r2*r2)-rt2*rt2);
 815       c.massTangent = VFloatNum!1/kTangent;
 816
 817       c.bias = -biasFactor*invDt*min(VFloatNum!0, c.separation+AllowedPenetration);
 818
 819       if (World.accumulateImpulses) {
 820         // apply normal + friction impulse
 821         Vec2 accimp = c.accimpN*c.normal+c.accimpT*tangent;
 822
 823         body0.velocity -= body0.invMass*accimp;
 824         body0.angularVelocity -= body0.invI*r1.cross(accimp);
 825
 826         body1.velocity += body1.invMass*accimp;
 827         body1.angularVelocity += body1.invI*r2.cross(accimp);
 828       }
 829     }
 830   }
 831
 832   void applyImpulse () {
 833     import std.algorithm : max;
 834     BodyBase b0 = body0;
 835     BodyBase b1 = body1;
 836     foreach (immutable idx; 0..numContacts) {
 837       Contact *c = contacts.ptr+idx;
 838       c.r1 = c.position-b0.mPosition;
 839       c.r2 = c.position-b1.mPosition;
 840
 841       // relative velocity at contact
 842       Vec2 dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 843
 844       // compute normal impulse
 845       VFloat vn = dv*c.normal; //Dot(dv, c.normal);
 846
 847       VFloat dPn = c.massNormal*(-vn+c.bias);
 848
 849       if (World.accumulateImpulses) {
 850         // clamp the accumulated impulse
 851         VFloat accimpN0 = c.accimpN;
 852         c.accimpN = max(accimpN0+dPn, VFloatNum!0);
 853         dPn = c.accimpN-accimpN0;
 854       } else {
 855         dPn = max(dPn, VFloatNum!0);
 856       }
 857
 858       // apply contact impulse
 859       Vec2 accimpN = dPn*c.normal;
 860
 861       b0.velocity -= b0.invMass*accimpN;
 862       b0.angularVelocity -= b0.invI*c.r1.cross(accimpN);
 863
 864       b1.velocity += b1.invMass*accimpN;
 865       b1.angularVelocity += b1.invI*c.r2.cross(accimpN);
 866
 867       // relative velocity at contact
 868       dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 869
 870       //Vec2 tangent = c.normal.cross(VFloatNum!1);
 871       Vec2 tangent = Vec2(VFloatNum!1*c.normal.y, -VFloatNum!1*c.normal.x);
 872       VFloat vt = dv*tangent; //Dot(dv, tangent);
 873       VFloat dPt = c.massTangent*(-vt);
 874
 875       if (World.accumulateImpulses) {
 876         // compute friction impulse
 877         VFloat maxPt = friction*c.accimpN;
 878         // clamp friction
 879         VFloat oldTangentImpulse = c.accimpT;
 880         c.accimpT = clamp(oldTangentImpulse+dPt, -maxPt, maxPt);
 881         dPt = c.accimpT-oldTangentImpulse;
 882       } else {
 883         VFloat maxPt = friction*dPn;
 884         dPt = clamp(dPt, -maxPt, maxPt);
 885       }
 886
 887       // apply contact impulse
 888       Vec2 accimpT = dPt*tangent;
 889
 890       b0.velocity -= b0.invMass*accimpT;
 891       b0.angularVelocity -= b0.invI*c.r1.cross(accimpT);
 892
 893       b1.velocity += b1.invMass*accimpT;
 894       b1.angularVelocity += b1.invI*c.r2.cross(accimpT);
 895     }
 896   }
 897 }
 898
 899
 900 // ////////////////////////////////////////////////////////////////////////// //
 901 // collide
 902 // the normal points from A to B
 903 // return number of contact points
 904 // this fills:
 905 //   position (already moved out of body)
 906 //   normal
 907 //   separation (this is negative penetration)
 908 //   feature (used only in arbiter updates to merge contacts, and has no effect in b2dlite)
 909 // return number of contacts
 910 private int collide (Contact[] contacts, BodyBase bodyAb, BodyBase bodyBb) {
 911   auto pb0 = cast(PolyBody)bodyAb;
 912   auto pb1 = cast(PolyBody)bodyBb;
 913   if (pb0 is null || pb1 is null) assert(0);
 914
 915   ContactInfo ci;
 916   polygon2Polygon(ci, pb0, pb1);
 917   if (!ci.valid) return 0; // no contacts
 918
 919   foreach (immutable cidx; 0..ci.contactCount) {
 920     contacts[cidx].normal = ci.normal;
 921     contacts[cidx].separation = -ci.penetration;
 922     contacts[cidx].position = ci.contacts[cidx]+(ci.normal*ci.penetration);
 923   }
 924
 925   return ci.contactCount;
 926 }
 927
 928
 929 // ////////////////////////////////////////////////////////////////////////// //
 930 private struct ContactInfo {
 931   VFloat penetration; // depth of penetration from collision
 932   Vec2 normal; // from A to B
 933   Vec2[2] contacts; // points of contact during collision
 934   uint contactCount; // number of contacts that occured during collision
 935
 936   @property bool valid () const pure nothrow @safe @nogc { pragma(inline, true); return (contactCount > 0); }
 937 }
 938
 939
 940 private bool biasGreaterThan (VFloat a, VFloat b) pure nothrow @safe @nogc {
 941   pragma(inline, true);
 942   enum k_biasRelative = VFloatNum!(0.95);
 943   enum k_biasAbsolute = VFloatNum!(0.01);
 944   return (a >= b*k_biasRelative+a*k_biasAbsolute);
 945 }
 946
 947
 948 private VFloat findAxisLeastPenetration (uint* faceIndex, PolyBody flesha, PolyBody fleshb) {
 949   VFloat bestDistance = -VFloat.max;
 950   uint bestIndex;
 951   foreach (immutable i; 0..flesha.verts.length) {
 952     // retrieve a face normal from A
 953     Vec2 n = flesha.norms.ptr[i];
 954     Vec2 nw = flesha.rmat*n;
 955     // transform face normal into B's model space
 956     Mat22 buT = fleshb.rmat.transpose();
 957     n = buT*nw;
 958     // retrieve support point from B along -n
 959     Vec2 s = fleshb.support(-n);
 960     // retrieve vertex on face from A, transform into B's model space
 961     Vec2 v = flesha.verts.ptr[i];
 962     v = flesha.rmat*v+flesha.mPosition;
 963     v -= fleshb.mPosition;
 964     v = buT*v;
 965     // compute penetration distance (in B's model space)
 966     VFloat d = n.dot(s-v);
 967     // store greatest distance
 968     if (d > bestDistance) {
 969       bestDistance = d;
 970       bestIndex = i;
 971     }
 972   }
 973   *faceIndex = bestIndex;
 974   return bestDistance;
 975 }
 976
 977
 978 private void findIncidentFace (Vec2[] v, PolyBody refPoly, PolyBody incPoly, uint referenceIndex) {
 979   Vec2 referenceNormal = refPoly.norms.ptr[referenceIndex];
 980   // calculate normal in incident's frame of reference
 981   referenceNormal = refPoly.rmat*referenceNormal; // to world space
 982   referenceNormal = incPoly.rmat.transpose()*referenceNormal; // to incident's model space
 983   // find most anti-normal face on incident polygon
 984   uint incidentFace = 0;
 985   VFloat minDot = VFloat.max;
 986   foreach (immutable i; 0..incPoly.verts.length) {
 987     VFloat dot = referenceNormal.dot(incPoly.norms.ptr[i]);
 988     if (dot < minDot) {
 989       minDot = dot;
 990       incidentFace = i;
 991     }
 992   }
 993   // assign face vertices for incidentFace
 994   v.ptr[0] = incPoly.rmat*incPoly.verts.ptr[incidentFace]+incPoly.mPosition;
 995   incidentFace = (incidentFace+1)%incPoly.verts.length;
 996   v.ptr[1] = incPoly.rmat*incPoly.verts.ptr[incidentFace]+incPoly.mPosition;
 997 }
 998
 999
1000 private uint clip (Vec2 n, VFloat c, Vec2[] face) {
1001   uint sp = 0;
1002   Vec2[2] outv = face.ptr[0..2];
1003   // retrieve distances from each endpoint to the line
1004   // d = ax + by - c
1005   VFloat d1 = n.dot(face.ptr[0])-c;
1006   VFloat d2 = n.dot(face.ptr[1])-c;
1007   // if negative (behind plane) clip
1008   if (d1 <= VFloatNum!0) outv.ptr[sp++] = face.ptr[0];
1009   if (d2 <= VFloatNum!0) outv.ptr[sp++] = face.ptr[1];
1010   // if the points are on different sides of the plane
1011   if (d1*d2 < VFloatNum!0) { // less than to ignore -0.0f
1012     // push interesection point
1013     VFloat alpha = d1/(d1-d2);
1014     if (sp >= 2) assert(0, "internal collision detection error");
1015     outv.ptr[sp] = face.ptr[0]+alpha*(face.ptr[1]-face.ptr[0]);
1016     ++sp;
1017   }
1018   // assign our new converted values
1019   face.ptr[0] = outv.ptr[0];
1020   face.ptr[1] = outv.ptr[1];
1021   assert(sp != 3);
1022   return sp;
1023 }
1024
1025
1026 private void polygon2Polygon (ref ContactInfo m, PolyBody flesha, PolyBody fleshb) {
1027   //flesha.rmat = Mat22(flesha.mRotation);
1028   //fleshb.rmat = Mat22(fleshb.mRotation);
1029
1030   m.contactCount = 0;
1031
1032   // check for a separating axis with A's face planes
1033   uint faceA;
1034   VFloat penetrationA = findAxisLeastPenetration(&faceA, flesha, fleshb);
1035   if (penetrationA >= VFloatNum!0) return;
1036
1037   // check for a separating axis with B's face planes
1038   uint faceB;
1039   VFloat penetrationB = findAxisLeastPenetration(&faceB, fleshb, flesha);
1040   if (penetrationB >= VFloatNum!0) return;
1041
1042   uint referenceIndex;
1043   bool flip; // Always point from a to b
1044
1045   PolyBody refPoly; // Reference
1046   PolyBody incPoly; // Incident
1047
1048   // determine which shape contains reference face
1049   if (biasGreaterThan(penetrationA, penetrationB)) {
1050     refPoly = flesha;
1051     incPoly = fleshb;
1052     referenceIndex = faceA;
1053     flip = false;
1054   } else {
1055     refPoly = fleshb;
1056     incPoly = flesha;
1057     referenceIndex = faceB;
1058     flip = true;
1059   }
1060
1061   // world space incident face
1062   Vec2[2] incidentFace;
1063   findIncidentFace(incidentFace[], refPoly, incPoly, referenceIndex);
1064
1065   //        y
1066   //        ^  .n       ^
1067   //      +---c ------posPlane--
1068   //  x < | i |\
1069   //      +---+ c-----negPlane--
1070   //             \       v
1071   //              r
1072   //
1073   //  r : reference face
1074   //  i : incident poly
1075   //  c : clipped point
1076   //  n : incident normal
1077
1078   // setup reference face vertices
1079   Vec2 v1 = refPoly.verts.ptr[referenceIndex];
1080   referenceIndex = (referenceIndex+1)%refPoly.verts.length;
1081   Vec2 v2 = refPoly.verts.ptr[referenceIndex];
1082
1083   // transform vertices to world space
1084   v1 = refPoly.rmat*v1+refPoly.mPosition;
1085   v2 = refPoly.rmat*v2+refPoly.mPosition;
1086
1087   // calculate reference face side normal in world space
1088   Vec2 sidePlaneNormal = v2-v1;
1089   sidePlaneNormal.normalize;
1090
1091   // orthogonalize
1092   auto refFaceNormal = Vec2(sidePlaneNormal.y, -sidePlaneNormal.x);
1093
1094   // ax + by = c
1095   // c is distance from origin
1096   VFloat refC = refFaceNormal.dot(v1);
1097   VFloat negSide = -sidePlaneNormal.dot(v1);
1098   VFloat posSide =  sidePlaneNormal.dot(v2);
1099
1100   // clip incident face to reference face side planes
1101   if (clip(-sidePlaneNormal, negSide, incidentFace) < 2) return; // due to floating point error, possible to not have required points
1102   if (clip(sidePlaneNormal, posSide, incidentFace) < 2) return; // due to floating point error, possible to not have required points
1103
1104   // flip
1105   m.normal = (flip ? -refFaceNormal : refFaceNormal);
1106
1107   // keep points behind reference face
1108   uint cp = 0; // clipped points behind reference face
1109   VFloat separation = refFaceNormal.dot(incidentFace[0])-refC;
1110   if (separation <= VFloatNum!0) {
1111     m.contacts[cp] = incidentFace[0];
1112     m.penetration = -separation;
1113     ++cp;
1114   } else {
1115     m.penetration = 0;
1116   }
1117
1118   separation = refFaceNormal.dot(incidentFace[1])-refC;
1119   if (separation <= VFloatNum!0) {
1120     m.contacts[cp] = incidentFace[1];
1121     m.penetration += -separation;
1122     ++cp;
1123     // average penetration
1124     m.penetration /= cast(VFloat)cp;
1125   }
1126
1127   m.contactCount = cp;
1128 }
1129
1130
1131 // ////////////////////////////////////////////////////////////////////////// //
1132 /* Dynamic AABB tree (bounding volume hierarchy)
1133  * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
1134  * Copyright (c) 2010-2016 Daniel Chappuis
1135  *
1136  * This software is provided 'as-is', without any express or implied warranty.
1137  * In no event will the authors be held liable for any damages arising from the
1138  * use of this software.
1139  *
1140  * Permission is granted to anyone to use this software for any purpose,
1141  * including commercial applications, and to alter it and redistribute it
1142  * freely, subject to the following restrictions:
1143  *
1144  * 1. The origin of this software must not be misrepresented; you must not claim
1145  *    that you wrote the original software. If you use this software in a
1146  *    product, an acknowledgment in the product documentation would be
1147  *    appreciated but is not required.
1148  *
1149  * 2. Altered source versions must be plainly marked as such, and must not be
1150  *    misrepresented as being the original software.
1151  *
1152  * 3. This notice may not be removed or altered from any source distribution.
1153  */
1154 private struct AABBBase(VT) if (Vec2.isVector!VT) {
1155 private:
1156   VT mMin, mMax;
1157
1158 public:
1159   alias VType = VT;
1160
1161 public pure nothrow @safe @nogc:
1162   @property ref const(VT) min () const { pragma(inline, true); return mMin; }
1163   @property ref const(VT) max () const { pragma(inline, true); return mMax; }
1164
1165   @property void min() (in auto ref VT v) { pragma(inline, true); mMin = v; }
1166   @property void max() (in auto ref VT v) { pragma(inline, true); mMax = v; }
1167
1168   // return the volume of the AABB
1169   @property VFloat volume () const {
1170     pragma(inline, true);
1171     immutable diff = mMax-mMin;
1172     static if (VT.isVector3!VT) {
1173       return diff.x*diff.y*diff.z;
1174     } else {
1175       return diff.x*diff.y;
1176     }
1177   }
1178
1179   static auto mergeAABBs() (in auto ref AABB aabb1, in auto ref AABB aabb2) {
1180     import std.algorithm : max, min;
1181     typeof(this) res;
1182     res.merge(aabb1, aabb2);
1183     return res;
1184   }
1185
1186   void merge() (in auto ref AABB aabb1, in auto ref AABB aabb2) {
1187     import std.algorithm : max, min;
1188     pragma(inline, true);
1189     mMin.x = min(aabb1.mMin.x, aabb2.mMin.x);
1190     mMin.y = min(aabb1.mMin.y, aabb2.mMin.y);
1191     mMax.x = max(aabb1.mMax.x, aabb2.mMax.x);
1192     mMax.y = max(aabb1.mMax.y, aabb2.mMax.y);
1193     static if (VT.isVector3!VT) {
1194       mMin.z = min(aabb1.mMin.z, aabb2.mMin.z);
1195       mMax.z = max(aabb1.mMax.z, aabb2.mMax.z);
1196     }
1197   }
1198
1199   // return true if the current AABB contains the AABB given in parameter
1200   bool contains() (in auto ref AABB aabb) const {
1201     pragma(inline, true);
1202     bool isInside = true;
1203     isInside = isInside && mMin.x <= aabb.mMin.x;
1204     isInside = isInside && mMin.y <= aabb.mMin.y;
1205     isInside = isInside && mMax.x >= aabb.mMax.x;
1206     isInside = isInside && mMax.y >= aabb.mMax.y;
1207     static if (VT.isVector3!VT) {
1208       isInside = isInside && mMin.z <= aabb.mMin.z;
1209       isInside = isInside && mMax.z >= aabb.mMax.z;
1210     }
1211     return isInside;
1212   }
1213
1214   // return true if the current AABB is overlapping with the AABB in argument
1215   // two AABBs overlap if they overlap in the two(three) x, y (and z) axes at the same time
1216   bool overlaps() (in auto ref AABB aabb) const {
1217     //pragma(inline, true);
1218     if (mMax.x < aabb.mMin.x || aabb.mMax.x < mMin.x) return false;
1219     if (mMax.y < aabb.mMin.y || aabb.mMax.y < mMin.y) return false;
1220     static if (VT.isVector3!VT) {
1221       if (mMax.z < aabb.mMin.z || aabb.mMax.z < mMin.z) return false;
1222     }
1223     return true;
1224   }
1225 }
1226
1227 alias AABB = AABBBase!Vec2;
1228
1229
1230 // ////////////////////////////////////////////////////////////////////////// //
1231 private align(1) struct TreeNode {
1232 align(1):
1233   enum NullTreeNode = -1;
1234   enum { Left = 0, Right = 1 }
1235   // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
1236   union {
1237     int parentId;
1238     int nextNodeId;
1239   }
1240   // a node is either a leaf (has data) or is an internal node (has children)
1241   union {
1242     int[2] children; /// left and right child of the node (children[0] = left child)
1243     BodyBase flesh;
1244   }
1245   // height of the node in the tree
1246   short height;
1247   // fat axis aligned bounding box (AABB) corresponding to the node
1248   AABB aabb;
1249   // return true if the node is a leaf of the tree
1250   @property bool leaf () const pure nothrow @safe @nogc { pragma(inline, true); return (height == 0); }
1251 }
1252
1253
1254 // ////////////////////////////////////////////////////////////////////////// //
1255 /*
1256  * This class implements a dynamic AABB tree that is used for broad-phase
1257  * collision detection. This data structure is inspired by Nathanael Presson's
1258  * dynamic tree implementation in BulletPhysics. The following implementation is
1259  * based on the one from Erin Catto in Box2D as described in the book
1260  * "Introduction to Game Physics with Box2D" by Ian Parberry.
1261  */
1262 private final class DynamicAABBTree {
1263   private import std.algorithm : max, min;
1264
1265   // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
1266   // also inflated in direction of the linear motion of the body by mutliplying the
1267   // followin constant with the linear velocity and the elapsed time between two frames
1268   enum VFloat LinearMotionGapMultiplier = VFloatNum!(1.7);
1269
1270 public:
1271   // called when a overlapping node has been found during the call to reportAllShapesOverlappingWithAABB()
1272   alias OverlapCallback = void delegate (int nodeId);
1273
1274 private:
1275   TreeNode* mNodes; // pointer to the memory location of the nodes of the tree
1276   int mRootNodeId; // id of the root node of the tree
1277   int mFreeNodeId; // id of the first node of the list of free (allocated) nodes in the tree that we can use
1278   int mAllocCount; // number of allocated nodes in the tree
1279   int mNodeCount; // number of nodes in the tree
1280
1281   // extra AABB Gap used to allow the collision shape to move a little bit
1282   // without triggering a large modification of the tree which can be costly
1283   VFloat mExtraGap;
1284
1285 private:
1286   // allocate and return a node to use in the tree
1287   int allocateNode () {
1288     // if there is no more allocated node to use
1289     if (mFreeNodeId == TreeNode.NullTreeNode) {
1290       import core.stdc.stdlib : realloc;
1291       version(b2dlite_bvh_many_asserts) assert(mNodeCount == mAllocCount);
1292       // allocate more nodes in the tree
1293       auto newsz = mAllocCount*2;
1294       if (newsz-mAllocCount > 4096) newsz = mAllocCount+4096;
1295       TreeNode* nn = cast(TreeNode*)realloc(mNodes, newsz*TreeNode.sizeof);
1296       if (nn is null) assert(0, "out of memory");
1297       mAllocCount = newsz;
1298       mNodes = nn;
1299       // initialize the allocated nodes
1300       foreach (int i; mNodeCount..mAllocCount-1) {
1301         mNodes[i].nextNodeId = i+1;
1302         mNodes[i].height = -1;
1303       }
1304       mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
1305       mNodes[mAllocCount-1].height = -1;
1306       mFreeNodeId = mNodeCount;
1307     }
1308     // get the next free node
1309     int freeNodeId = mFreeNodeId;
1310     mFreeNodeId = mNodes[freeNodeId].nextNodeId;
1311     mNodes[freeNodeId].parentId = TreeNode.NullTreeNode;
1312     mNodes[freeNodeId].height = 0;
1313     ++mNodeCount;
1314     return freeNodeId;
1315   }
1316
1317   // release a node
1318   void releaseNode (int nodeId) {
1319     version(b2dlite_bvh_many_asserts) assert(mNodeCount > 0);
1320     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1321     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0);
1322     mNodes[nodeId].nextNodeId = mFreeNodeId;
1323     mNodes[nodeId].height = -1;
1324     mFreeNodeId = nodeId;
1325     --mNodeCount;
1326   }
1327
1328   // insert a leaf node in the tree
1329   // the process of inserting a new leaf node in the dynamic tree is described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
1330   void insertLeafNode (int nodeId) {
1331     // if the tree is empty
1332     if (mRootNodeId == TreeNode.NullTreeNode) {
1333       mRootNodeId = nodeId;
1334       mNodes[mRootNodeId].parentId = TreeNode.NullTreeNode;
1335       return;
1336     }
1337
1338     version(b2dlite_bvh_many_asserts) assert(mRootNodeId != TreeNode.NullTreeNode);
1339
1340     // find the best sibling node for the new node
1341     AABB newNodeAABB = mNodes[nodeId].aabb;
1342     int currentNodeId = mRootNodeId;
1343     while (!mNodes[currentNodeId].leaf) {
1344       int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1345       int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1346
1347       // compute the merged AABB
1348       VFloat volumeAABB = mNodes[currentNodeId].aabb.volume;
1349       AABB mergedAABBs = AABB.mergeAABBs(mNodes[currentNodeId].aabb, newNodeAABB);
1350       VFloat mergedVolume = mergedAABBs.volume;
1351
1352       // compute the cost of making the current node the sibbling of the new node
1353       VFloat costS = VFloatNum!(2.0)*mergedVolume;
1354
1355       // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
1356       VFloat costI = VFloatNum!(2.0)*(mergedVolume-volumeAABB);
1357
1358       // compute the cost of descending into the left child
1359       VFloat costLeft;
1360       AABB currentAndLeftAABB = AABB.mergeAABBs(newNodeAABB, mNodes[leftChild].aabb);
1361       if (mNodes[leftChild].leaf) {
1362         costLeft = currentAndLeftAABB.volume+costI;
1363       } else {
1364         VFloat leftChildVolume = mNodes[leftChild].aabb.volume;
1365         costLeft = costI+currentAndLeftAABB.volume-leftChildVolume;
1366       }
1367
1368       // compute the cost of descending into the right child
1369       VFloat costRight;
1370       AABB currentAndRightAABB = AABB.mergeAABBs(newNodeAABB, mNodes[rightChild].aabb);
1371       if (mNodes[rightChild].leaf) {
1372         costRight = currentAndRightAABB.volume+costI;
1373       } else {
1374         VFloat rightChildVolume = mNodes[rightChild].aabb.volume;
1375         costRight = costI+currentAndRightAABB.volume-rightChildVolume;
1376       }
1377
1378       // if the cost of making the current node a sibbling of the new node is smaller than the cost of going down into the left or right child
1379       if (costS < costLeft && costS < costRight) break;
1380
1381       // it is cheaper to go down into a child of the current node, choose the best child
1382       currentNodeId = (costLeft < costRight ? leftChild : rightChild);
1383     }
1384
1385     int siblingNode = currentNodeId;
1386
1387     // create a new parent for the new node and the sibling node
1388     int oldParentNode = mNodes[siblingNode].parentId;
1389     int newParentNode = allocateNode();
1390     mNodes[newParentNode].parentId = oldParentNode;
1391     mNodes[newParentNode].aabb.merge(mNodes[siblingNode].aabb, newNodeAABB);
1392     mNodes[newParentNode].height = cast(short)(mNodes[siblingNode].height+1);
1393     version(b2dlite_bvh_many_asserts) assert(mNodes[newParentNode].height > 0);
1394
1395     // If the sibling node was not the root node
1396     if (oldParentNode != TreeNode.NullTreeNode) {
1397       version(b2dlite_bvh_many_asserts) assert(!mNodes[oldParentNode].leaf);
1398       if (mNodes[oldParentNode].children.ptr[TreeNode.Left] == siblingNode) {
1399         mNodes[oldParentNode].children.ptr[TreeNode.Left] = newParentNode;
1400       } else {
1401         mNodes[oldParentNode].children.ptr[TreeNode.Right] = newParentNode;
1402       }
1403       mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
1404       mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
1405       mNodes[siblingNode].parentId = newParentNode;
1406       mNodes[nodeId].parentId = newParentNode;
1407     } else {
1408       // if the sibling node was the root node
1409       mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
1410       mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
1411       mNodes[siblingNode].parentId = newParentNode;
1412       mNodes[nodeId].parentId = newParentNode;
1413       mRootNodeId = newParentNode;
1414     }
1415
1416     // move up in the tree to change the AABBs that have changed
1417     currentNodeId = mNodes[nodeId].parentId;
1418     version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1419     while (currentNodeId != TreeNode.NullTreeNode) {
1420       // balance the sub-tree of the current node if it is not balanced
1421       currentNodeId = balanceSubTreeAtNode(currentNodeId);
1422       version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1423
1424       version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1425       int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1426       int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1427       version(b2dlite_bvh_many_asserts) assert(leftChild != TreeNode.NullTreeNode);
1428       version(b2dlite_bvh_many_asserts) assert(rightChild != TreeNode.NullTreeNode);
1429
1430       // recompute the height of the node in the tree
1431       mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChild].height, mNodes[rightChild].height)+1);
1432       version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0);
1433
1434       // recompute the AABB of the node
1435       mNodes[currentNodeId].aabb.merge(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
1436
1437       currentNodeId = mNodes[currentNodeId].parentId;
1438     }
1439
1440     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1441   }
1442
1443   // remove a leaf node from the tree
1444   void removeLeafNode (int nodeId) {
1445     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1446     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1447
1448     // If we are removing the root node (root node is a leaf in this case)
1449     if (mRootNodeId == nodeId) { mRootNodeId = TreeNode.NullTreeNode; return; }
1450
1451     int parentNodeId = mNodes[nodeId].parentId;
1452     int grandParentNodeId = mNodes[parentNodeId].parentId;
1453     int siblingNodeId;
1454
1455     if (mNodes[parentNodeId].children.ptr[TreeNode.Left] == nodeId) {
1456       siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Right];
1457     } else {
1458       siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Left];
1459     }
1460
1461     // if the parent of the node to remove is not the root node
1462     if (grandParentNodeId != TreeNode.NullTreeNode) {
1463       // destroy the parent node
1464       if (mNodes[grandParentNodeId].children.ptr[TreeNode.Left] == parentNodeId) {
1465         mNodes[grandParentNodeId].children.ptr[TreeNode.Left] = siblingNodeId;
1466       } else {
1467         version(b2dlite_bvh_many_asserts) assert(mNodes[grandParentNodeId].children.ptr[TreeNode.Right] == parentNodeId);
1468         mNodes[grandParentNodeId].children.ptr[TreeNode.Right] = siblingNodeId;
1469       }
1470       mNodes[siblingNodeId].parentId = grandParentNodeId;
1471       releaseNode(parentNodeId);
1472
1473       // now, we need to recompute the AABBs of the node on the path back to the root and make sure that the tree is still balanced
1474       int currentNodeId = grandParentNodeId;
1475       while (currentNodeId != TreeNode.NullTreeNode) {
1476         // balance the current sub-tree if necessary
1477         currentNodeId = balanceSubTreeAtNode(currentNodeId);
1478
1479         version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1480
1481         // get the two children.ptr of the current node
1482         int leftChildId = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1483         int rightChildId = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1484
1485         // recompute the AABB and the height of the current node
1486         mNodes[currentNodeId].aabb.merge(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb);
1487         mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChildId].height, mNodes[rightChildId].height)+1);
1488         version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0);
1489
1490         currentNodeId = mNodes[currentNodeId].parentId;
1491       }
1492     } else {
1493       // if the parent of the node to remove is the root node, the sibling node becomes the new root node
1494       mRootNodeId = siblingNodeId;
1495       mNodes[siblingNodeId].parentId = TreeNode.NullTreeNode;
1496       releaseNode(parentNodeId);
1497     }
1498   }
1499
1500   // balance the sub-tree of a given node using left or right rotations
1501   // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
1502   // this method returns the new root node Id
1503   int balanceSubTreeAtNode (int nodeId) {
1504     version(b2dlite_bvh_many_asserts) assert(nodeId != TreeNode.NullTreeNode);
1505
1506     TreeNode* nodeA = mNodes+nodeId;
1507
1508     // if the node is a leaf or the height of A's sub-tree is less than 2
1509     if (nodeA.leaf || nodeA.height < 2) return nodeId; // do not perform any rotation
1510
1511     // get the two children nodes
1512     int nodeBId = nodeA.children.ptr[TreeNode.Left];
1513     int nodeCId = nodeA.children.ptr[TreeNode.Right];
1514     version(b2dlite_bvh_many_asserts) assert(nodeBId >= 0 && nodeBId < mAllocCount);
1515     version(b2dlite_bvh_many_asserts) assert(nodeCId >= 0 && nodeCId < mAllocCount);
1516     TreeNode* nodeB = mNodes+nodeBId;
1517     TreeNode* nodeC = mNodes+nodeCId;
1518
1519     // compute the factor of the left and right sub-trees
1520     int balanceFactor = nodeC.height-nodeB.height;
1521
1522     // if the right node C is 2 higher than left node B
1523     if (balanceFactor > 1) {
1524       version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf);
1525
1526       int nodeFId = nodeC.children.ptr[TreeNode.Left];
1527       int nodeGId = nodeC.children.ptr[TreeNode.Right];
1528       version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
1529       version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
1530       TreeNode* nodeF = mNodes+nodeFId;
1531       TreeNode* nodeG = mNodes+nodeGId;
1532
1533       nodeC.children.ptr[TreeNode.Left] = nodeId;
1534       nodeC.parentId = nodeA.parentId;
1535       nodeA.parentId = nodeCId;
1536
1537       if (nodeC.parentId != TreeNode.NullTreeNode) {
1538         if (mNodes[nodeC.parentId].children.ptr[TreeNode.Left] == nodeId) {
1539           mNodes[nodeC.parentId].children.ptr[TreeNode.Left] = nodeCId;
1540         } else {
1541           version(b2dlite_bvh_many_asserts) assert(mNodes[nodeC.parentId].children.ptr[TreeNode.Right] == nodeId);
1542           mNodes[nodeC.parentId].children.ptr[TreeNode.Right] = nodeCId;
1543         }
1544       } else {
1545         mRootNodeId = nodeCId;
1546       }
1547
1548       version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf);
1549       version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf);
1550
1551       // if the right node C was higher than left node B because of the F node
1552       if (nodeF.height > nodeG.height) {
1553         nodeC.children.ptr[TreeNode.Right] = nodeFId;
1554         nodeA.children.ptr[TreeNode.Right] = nodeGId;
1555         nodeG.parentId = nodeId;
1556
1557         // recompute the AABB of node A and C
1558         nodeA.aabb.merge(nodeB.aabb, nodeG.aabb);
1559         nodeC.aabb.merge(nodeA.aabb, nodeF.aabb);
1560
1561         // recompute the height of node A and C
1562         nodeA.height = cast(short)(max(nodeB.height, nodeG.height)+1);
1563         nodeC.height = cast(short)(max(nodeA.height, nodeF.height)+1);
1564         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1565         version(b2dlite_bvh_many_asserts) assert(nodeC.height > 0);
1566       } else {
1567         // if the right node C was higher than left node B because of node G
1568         nodeC.children.ptr[TreeNode.Right] = nodeGId;
1569         nodeA.children.ptr[TreeNode.Right] = nodeFId;
1570         nodeF.parentId = nodeId;
1571
1572         // recompute the AABB of node A and C
1573         nodeA.aabb.merge(nodeB.aabb, nodeF.aabb);
1574         nodeC.aabb.merge(nodeA.aabb, nodeG.aabb);
1575
1576         // recompute the height of node A and C
1577         nodeA.height = cast(short)(max(nodeB.height, nodeF.height)+1);
1578         nodeC.height = cast(short)(max(nodeA.height, nodeG.height)+1);
1579         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1580         version(b2dlite_bvh_many_asserts) assert(nodeC.height > 0);
1581       }
1582
1583       // return the new root of the sub-tree
1584       return nodeCId;
1585     }
1586
1587     // if the left node B is 2 higher than right node C
1588     if (balanceFactor < -1) {
1589       version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf);
1590
1591       int nodeFId = nodeB.children.ptr[TreeNode.Left];
1592       int nodeGId = nodeB.children.ptr[TreeNode.Right];
1593       version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
1594       version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
1595       TreeNode* nodeF = mNodes+nodeFId;
1596       TreeNode* nodeG = mNodes+nodeGId;
1597
1598       nodeB.children.ptr[TreeNode.Left] = nodeId;
1599       nodeB.parentId = nodeA.parentId;
1600       nodeA.parentId = nodeBId;
1601
1602       if (nodeB.parentId != TreeNode.NullTreeNode) {
1603         if (mNodes[nodeB.parentId].children.ptr[TreeNode.Left] == nodeId) {
1604           mNodes[nodeB.parentId].children.ptr[TreeNode.Left] = nodeBId;
1605         } else {
1606           version(b2dlite_bvh_many_asserts) assert(mNodes[nodeB.parentId].children.ptr[TreeNode.Right] == nodeId);
1607           mNodes[nodeB.parentId].children.ptr[TreeNode.Right] = nodeBId;
1608         }
1609       } else {
1610         mRootNodeId = nodeBId;
1611       }
1612
1613       version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf);
1614       version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf);
1615
1616       // if the left node B was higher than right node C because of the F node
1617       if (nodeF.height > nodeG.height) {
1618         nodeB.children.ptr[TreeNode.Right] = nodeFId;
1619         nodeA.children.ptr[TreeNode.Left] = nodeGId;
1620         nodeG.parentId = nodeId;
1621
1622         // recompute the AABB of node A and B
1623         nodeA.aabb.merge(nodeC.aabb, nodeG.aabb);
1624         nodeB.aabb.merge(nodeA.aabb, nodeF.aabb);
1625
1626         // recompute the height of node A and B
1627         nodeA.height = cast(short)(max(nodeC.height, nodeG.height)+1);
1628         nodeB.height = cast(short)(max(nodeA.height, nodeF.height)+1);
1629         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1630         version(b2dlite_bvh_many_asserts) assert(nodeB.height > 0);
1631       } else {
1632         // if the left node B was higher than right node C because of node G
1633         nodeB.children.ptr[TreeNode.Right] = nodeGId;
1634         nodeA.children.ptr[TreeNode.Left] = nodeFId;
1635         nodeF.parentId = nodeId;
1636
1637         // recompute the AABB of node A and B
1638         nodeA.aabb.merge(nodeC.aabb, nodeF.aabb);
1639         nodeB.aabb.merge(nodeA.aabb, nodeG.aabb);
1640
1641         // recompute the height of node A and B
1642         nodeA.height = cast(short)(max(nodeC.height, nodeF.height)+1);
1643         nodeB.height = cast(short)(max(nodeA.height, nodeG.height)+1);
1644         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1645         version(b2dlite_bvh_many_asserts) assert(nodeB.height > 0);
1646       }
1647
1648       // return the new root of the sub-tree
1649       return nodeBId;
1650     }
1651
1652     // if the sub-tree is balanced, return the current root node
1653     return nodeId;
1654   }
1655
1656   // compute the height of a given node in the tree
1657   int computeHeight (int nodeId) {
1658     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1659     TreeNode* node = mNodes+nodeId;
1660
1661     // If the node is a leaf, its height is zero
1662     if (node.leaf) return 0;
1663
1664     // Compute the height of the left and right sub-tree
1665     int leftHeight = computeHeight(node.children.ptr[TreeNode.Left]);
1666     int rightHeight = computeHeight(node.children.ptr[TreeNode.Right]);
1667
1668     // Return the height of the node
1669     return 1+max(leftHeight, rightHeight);
1670   }
1671
1672   // internally add an object into the tree
1673   int addObjectInternal() (in auto ref AABB aabb) {
1674     // get the next available node (or allocate new ones if necessary)
1675     int nodeId = allocateNode();
1676
1677     // create the fat aabb to use in the tree
1678     immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap);
1679     mNodes[nodeId].aabb.min = aabb.min-gap;
1680     mNodes[nodeId].aabb.max = aabb.max+gap;
1681
1682     // set the height of the node in the tree
1683     mNodes[nodeId].height = 0;
1684
1685     // insert the new leaf node in the tree
1686     insertLeafNode(nodeId);
1687     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1688
1689     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0);
1690
1691     // return the Id of the node
1692     return nodeId;
1693   }
1694
1695   // initialize the tree
1696   void setup () {
1697     import core.stdc.stdlib : malloc;
1698     import core.stdc.string : memset;
1699
1700     mRootNodeId = TreeNode.NullTreeNode;
1701     mNodeCount = 0;
1702     mAllocCount = 64;
1703
1704     mNodes = cast(TreeNode*)malloc(mAllocCount*TreeNode.sizeof);
1705     if (mNodes is null) assert(0, "out of memory");
1706     memset(mNodes, 0, mAllocCount*TreeNode.sizeof);
1707
1708     // initialize the allocated nodes
1709     foreach (int i; 0..mAllocCount-1) {
1710       mNodes[i].nextNodeId = i+1;
1711       mNodes[i].height = -1;
1712     }
1713     mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
1714     mNodes[mAllocCount-1].height = -1;
1715     mFreeNodeId = 0;
1716   }
1717
1718   // also, checks if the tree structure is valid (for debugging purpose)
1719   void forEachLeaf (scope void delegate (int nodeId, in ref AABB aabb) dg) {
1720     void forEachNode (int nodeId) {
1721       if (nodeId == TreeNode.NullTreeNode) return;
1722       // if it is the root
1723       if (nodeId == mRootNodeId) {
1724         assert(mNodes[nodeId].parentId == TreeNode.NullTreeNode);
1725       }
1726       // get the children nodes
1727       TreeNode* pNode = mNodes+nodeId;
1728       assert(pNode.height >= 0);
1729       assert(pNode.aabb.volume > 0);
1730       // if the current node is a leaf
1731       if (pNode.leaf) {
1732         assert(pNode.height == 0);
1733         if (dg !is null) dg(nodeId, pNode.aabb);
1734       } else {
1735         int leftChild = pNode.children.ptr[TreeNode.Left];
1736         int rightChild = pNode.children.ptr[TreeNode.Right];
1737         // check that the children node Ids are valid
1738         assert(0 <= leftChild && leftChild < mAllocCount);
1739         assert(0 <= rightChild && rightChild < mAllocCount);
1740         // check that the children nodes have the correct parent node
1741         assert(mNodes[leftChild].parentId == nodeId);
1742         assert(mNodes[rightChild].parentId == nodeId);
1743         // check the height of node
1744         int height = 1+max(mNodes[leftChild].height, mNodes[rightChild].height);
1745         assert(mNodes[nodeId].height == height);
1746         // check the AABB of the node
1747         AABB aabb = AABB.mergeAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
1748         assert(aabb.min == mNodes[nodeId].aabb.min);
1749         assert(aabb.max == mNodes[nodeId].aabb.max);
1750         // recursively check the children nodes
1751         forEachNode(leftChild);
1752         forEachNode(rightChild);
1753       }
1754     }
1755     // recursively check each node
1756     forEachNode(mRootNodeId);
1757   }
1758
1759 public:
1760   this (VFloat extraAABBGap=VFloatNum!0) {
1761     mExtraGap = extraAABBGap;
1762     setup();
1763   }
1764
1765   ~this () {
1766     import core.stdc.stdlib : free;
1767     free(mNodes);
1768   }
1769
1770   // return the fat AABB corresponding to a given node Id
1771   /*const ref*/ AABB getFatAABB (int nodeId) const {
1772     pragma(inline, true);
1773     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1774     return mNodes[nodeId].aabb;
1775   }
1776
1777   // return the pointer to the data array of a given leaf node of the tree
1778   BodyBase getNodeBody (int nodeId) {
1779     pragma(inline, true);
1780     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1781     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1782     return mNodes[nodeId].flesh;
1783   }
1784
1785   // return the root AABB of the tree
1786   AABB getRootAABB () { pragma(inline, true); return getFatAABB(mRootNodeId); }
1787
1788   // add an object into the tree.
1789   // this method creates a new leaf node in the tree and returns the Id of the corresponding node
1790   int addObject (BodyBase flesh) {
1791     auto aabb = flesh.getAABB(); // can be passed as argument
1792     int nodeId = addObjectInternal(aabb);
1793     mNodes[nodeId].flesh = flesh;
1794     return nodeId;
1795   }
1796
1797   // remove an object from the tree
1798   void removeObject (int nodeId) {
1799     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1800     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1801     // remove the node from the tree
1802     removeLeafNode(nodeId);
1803     releaseNode(nodeId);
1804   }
1805
1806   // update the dynamic tree after an object has moved
1807   // if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
1808   // otherwise, the corresponding node is removed and reinserted into the tree.
1809   // the method returns true if the object has been reinserted into the tree.
1810   // the "displacement" argument is the linear velocity of the AABB multiplied by the elapsed time between two frames.
1811   // if the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
1812   // (this can be useful if the shape AABB has become much smaller than the previous one for instance).
1813   // return `true` if the tree was modified
1814   bool updateObject() (int nodeId, in auto ref AABB.VType displacement, bool forceReinsert=false) {
1815     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1816     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1817     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0);
1818
1819     auto newAABB = mNodes[nodeId].flesh.getAABB(); // can be passed as argument
1820
1821     // if the new AABB is still inside the fat AABB of the node
1822     if (!forceReinsert && mNodes[nodeId].aabb.contains(newAABB)) return false;
1823
1824     // if the new AABB is outside the fat AABB, we remove the corresponding node
1825     removeLeafNode(nodeId);
1826
1827     // compute the fat AABB by inflating the AABB with a constant gap
1828     mNodes[nodeId].aabb = newAABB;
1829     immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap);
1830     mNodes[nodeId].aabb.mMin -= gap;
1831     mNodes[nodeId].aabb.mMax += gap;
1832
1833     // inflate the fat AABB in direction of the linear motion of the AABB
1834     if (displacement.x < VFloatNum!0) {
1835       mNodes[nodeId].aabb.mMin.x += LinearMotionGapMultiplier*displacement.x;
1836     } else {
1837       mNodes[nodeId].aabb.mMax.x += LinearMotionGapMultiplier*displacement.x;
1838     }
1839     if (displacement.y < VFloatNum!0) {
1840       mNodes[nodeId].aabb.mMin.y += LinearMotionGapMultiplier*displacement.y;
1841     } else {
1842       mNodes[nodeId].aabb.mMax.y += LinearMotionGapMultiplier*displacement.y;
1843     }
1844     static if (AABB.VType.isVector3!(AABB.VType)) {
1845       if (displacement.z < VFloatNum!0) {
1846         mNodes[nodeId].aabb.mMin.z += LinearMotionGapMultiplier *displacement.z;
1847       } else {
1848         mNodes[nodeId].aabb.mMax.z += LinearMotionGapMultiplier *displacement.z;
1849       }
1850     }
1851
1852     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].aabb.contains(newAABB));
1853
1854     // reinsert the node into the tree
1855     insertLeafNode(nodeId);
1856
1857     return true;
1858   }
1859
1860   // report all shapes overlapping with the AABB given in parameter
1861   void reportAllShapesOverlappingWithAABB() (in auto ref AABB aabb, scope OverlapCallback callback) {
1862     int[256] stack = void; // stack with the nodes to visit
1863     int sp = 0;
1864
1865     void spush (int id) {
1866       if (sp >= stack.length) throw new Exception("stack overflow");
1867       stack.ptr[sp++] = id;
1868     }
1869
1870     int spop () {
1871       if (sp == 0) throw new Exception("stack underflow");
1872       return stack.ptr[--sp];
1873     }
1874
1875     spush(mRootNodeId);
1876
1877     // while there are still nodes to visit
1878     while (sp > 0) {
1879       // get the next node id to visit
1880       int nodeIdToVisit = spop();
1881       // skip it if it is a null node
1882       if (nodeIdToVisit == TreeNode.NullTreeNode) continue;
1883       // get the corresponding node
1884       const(TreeNode)* nodeToVisit = mNodes+nodeIdToVisit;
1885       // if the AABB in parameter overlaps with the AABB of the node to visit
1886       if (aabb.overlaps(nodeToVisit.aabb)) {
1887         // if the node is a leaf
1888         if (nodeToVisit.leaf) {
1889           // notify the broad-phase about a new potential overlapping pair
1890           callback(nodeIdToVisit);
1891         } else {
1892           // if the node is not a leaf
1893           // we need to visit its children
1894           spush(nodeToVisit.children.ptr[TreeNode.Left]);
1895           spush(nodeToVisit.children.ptr[TreeNode.Right]);
1896         }
1897       }
1898     }
1899   }
1900
1901   // compute the height of the tree
1902   int computeHeight () { pragma(inline, true); return computeHeight(mRootNodeId); }
1903
1904   // clear all the nodes and reset the tree
1905   void reset() {
1906     import core.stdc.stdlib : free;
1907     free(mNodes);
1908     setup();
1909   }
1910 }