b2dlite.d

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