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         mWorld.bvh.updateObject(nodeId, Vec2(VFloatNum!0, VFloatNum!0), true); // force reinsert
 290       }
 291     }
 292   }
 293
 294 public:
 295   this () @trusted {
 296     import core.atomic : atomicOp;
 297     midx = atomicOp!"+="(lastidx, 1);
 298
 299     mPosition.set(VFloatNum!0, VFloatNum!0);
 300     mRotation = VFloatNum!0;
 301     velocity.set(VFloatNum!0, VFloatNum!0);
 302     angularVelocity = VFloatNum!0;
 303     force.set(VFloatNum!0, VFloatNum!0);
 304     torque = VFloatNum!0;
 305     friction = VFloatNum!(0.2);
 306     mass = VFloat.max;
 307     invMass = VFloatNum!0;
 308     i = VFloat.max;
 309     invI = VFloatNum!0;
 310   }
 311
 312   @property uint id () const pure nothrow @safe @nogc { pragma(inline, true); return midx; }
 313
 314   void addForce() (in auto ref Vec2 f) pure nothrow @safe @nogc { pragma(inline, true); force += f; }
 315
 316   final ref const(Vec2) position () const pure nothrow @safe @nogc { pragma(inline, true); return mPosition; }
 317   final void position() (in auto ref Vec2 p) { pragma(inline, true); mPosition = p; updateWorld(); }
 318   final void setPosition() (VFloat ax, VFloat ay) { pragma(inline, true); mPosition.set(ax, ay); updateWorld(); }
 319
 320   final VFloat rotation () const pure nothrow @safe @nogc { pragma(inline, true); return mRotation; }
 321   final void rotation() (VFloat aangle) { pragma(inline, true); mRotation = aangle; updateWorld(); }
 322
 323   override bool opEquals (Object b) const pure nothrow @trusted @nogc {
 324     //pragma(inline, true);
 325     if (b is null) return false;
 326     if (b is this) return true;
 327     if (auto bb = cast(BodyBase)b) return (bb.midx == midx);
 328     return false;
 329   }
 330
 331   override int opCmp (Object b) const pure nothrow @trusted @nogc {
 332     //pragma(inline, true);
 333     if (b is null) return 1;
 334     if (b is this) return 0;
 335     if (auto bb = cast(BodyBase)b) return (midx < bb.midx ? -1 : midx > bb.midx ? 1 : 0);
 336     return -1;
 337   }
 338
 339   override size_t toHash () nothrow @safe @nogc {
 340     return hashu32(midx);
 341   }
 342
 343   abstract AABB getAABB ();
 344
 345 static:
 346   uint hashu32 (uint a) pure nothrow @safe @nogc {
 347     a -= (a<<6);
 348     a ^= (a>>17);
 349     a -= (a<<9);
 350     a ^= (a<<4);
 351     a -= (a<<3);
 352     a ^= (a<<10);
 353     a ^= (a>>15);
 354     return a;
 355   }
 356 }
 357
 358
 359 // ////////////////////////////////////////////////////////////////////////// //
 360 public final class PolyBody : BodyBase {
 361 public:
 362   Vec2[] verts; // vertices
 363   Vec2[] norms; // normals
 364
 365 public:
 366   this () @trusted {
 367     super();
 368     computeMass(VFloatNum!1);
 369   }
 370
 371   static PolyBody Box() (in auto ref Vec2 width, VFloat density) {
 372     auto res = new PolyBody();
 373     res.setBox(width);
 374     res.computeMass(density);
 375     return res;
 376   }
 377
 378   void set() (const(Vec2)[] averts, VFloat m) {
 379     mPosition.set(VFloatNum!0, VFloatNum!0);
 380     mRotation = VFloatNum!0;
 381     velocity.set(VFloatNum!0, VFloatNum!0);
 382     angularVelocity = VFloatNum!0;
 383     force.set(VFloatNum!0, VFloatNum!0);
 384     torque = VFloatNum!0;
 385     friction = VFloatNum!(0.2);
 386     setVerts(averts);
 387     computeMass(m);
 388     updateWorld();
 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   private 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 support() (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   // return AABB for properly moved and rotated body
 562   override AABB getAABB () {
 563     AABB res;
 564     res.mMin = Vec2(float.max, float.max);
 565     res.mMax = Vec2(-float.max, -float.max);
 566     auto rmt = Mat22(mRotation);
 567     foreach (const ref vx; verts) {
 568       import std.algorithm : max, min;
 569       auto vp = mPosition+rmt*vx;
 570       res.mMin.x = min(res.mMin.x, vp.x);
 571       res.mMin.y = min(res.mMin.y, vp.y);
 572       res.mMax.x = max(res.mMax.x, vp.x);
 573       res.mMax.y = max(res.mMax.y, vp.y);
 574     }
 575     return res;
 576   }
 577 }
 578
 579
 580 // ////////////////////////////////////////////////////////////////////////// //
 581 // joint
 582 public final class Joint {
 583 private:
 584   World mWorld;
 585
 586 public:
 587   Mat22 mt;
 588   Vec2 localAnchor1, localAnchor2;
 589   Vec2 r1, r2;
 590   Vec2 bias;
 591   Vec2 accimp = Vec2(0, 0); // accumulated impulse
 592   BodyBase body0;
 593   BodyBase body1;
 594   VFloat biasFactor = VFloatNum!(0.2);
 595   VFloat softness = VFloatNum!0;
 596
 597 public:
 598   void set() (BodyBase b0, BodyBase b1, in auto ref Vec2 anchor) {
 599     body0 = b0;
 600     body1 = b1;
 601
 602     auto rot1 = Mat22(body0.mRotation);
 603     auto rot2 = Mat22(body1.mRotation);
 604     Mat22 rot1T = rot1.transpose();
 605     Mat22 rot2T = rot2.transpose();
 606
 607     localAnchor1 = rot1T*(anchor-body0.mPosition);
 608     localAnchor2 = rot2T*(anchor-body1.mPosition);
 609
 610     accimp.set(VFloatNum!0, VFloatNum!0);
 611
 612     softness = VFloatNum!0;
 613     biasFactor = VFloatNum!(0.2);
 614   }
 615
 616   void preStep (VFloat invDt) {
 617     // pre-compute anchors, mass matrix, and bias
 618     auto rot1 = Mat22(body0.mRotation);
 619     auto rot2 = Mat22(body1.mRotation);
 620
 621     r1 = rot1*localAnchor1;
 622     r2 = rot2*localAnchor2;
 623
 624     // deltaV = deltaV0+kmt*impulse
 625     // invM = [(1/m1+1/m2)*eye(2)-skew(r1)*invI1*skew(r1)-skew(r2)*invI2*skew(r2)]
 626     //      = [1/m1+1/m2     0    ]+invI1*[r1.y*r1.y -r1.x*r1.y]+invI2*[r1.y*r1.y -r1.x*r1.y]
 627     //        [    0     1/m1+1/m2]       [-r1.x*r1.y r1.x*r1.x]       [-r1.x*r1.y r1.x*r1.x]
 628     Mat22 k1;
 629     k1.col1.x = body0.invMass+body1.invMass; k1.col2.x = VFloatNum!0;
 630     k1.col1.y = VFloatNum!0;                          k1.col2.y = body0.invMass+body1.invMass;
 631
 632     Mat22 k2;
 633     k2.col1.x =  body0.invI*r1.y*r1.y; k2.col2.x = -body0.invI*r1.x*r1.y;
 634     k2.col1.y = -body0.invI*r1.x*r1.y; k2.col2.y =  body0.invI*r1.x*r1.x;
 635
 636     Mat22 k3;
 637     k3.col1.x =  body1.invI*r2.y*r2.y; k3.col2.x = -body1.invI*r2.x*r2.y;
 638     k3.col1.y = -body1.invI*r2.x*r2.y; k3.col2.y =  body1.invI*r2.x*r2.x;
 639
 640     Mat22 kmt = k1+k2+k3;
 641     kmt.col1.x += softness;
 642     kmt.col2.y += softness;
 643
 644     mt = kmt.invert();
 645
 646     Vec2 p1 = body0.mPosition+r1;
 647     Vec2 p2 = body1.mPosition+r2;
 648     Vec2 dp = p2-p1;
 649
 650     if (World.positionCorrection) {
 651       bias = -biasFactor*invDt*dp;
 652     } else {
 653       bias.set(VFloatNum!0, VFloatNum!0);
 654     }
 655
 656     if (World.warmStarting) {
 657       // apply accumulated impulse
 658       body0.velocity -= body0.invMass*accimp;
 659       body0.angularVelocity -= body0.invI*r1.cross(accimp);
 660
 661       body1.velocity += body1.invMass*accimp;
 662       body1.angularVelocity += body1.invI*r2.cross(accimp);
 663     } else {
 664       accimp.set(VFloatNum!0, VFloatNum!0);
 665     }
 666   }
 667
 668   void applyImpulse () {
 669     Vec2 dv = body1.velocity+body1.angularVelocity.fcross(r2)-body0.velocity-body0.angularVelocity.fcross(r1);
 670     Vec2 impulse = mt*(bias-dv-softness*accimp);
 671
 672     body0.velocity -= body0.invMass*impulse;
 673     body0.angularVelocity -= body0.invI*r1.cross(impulse);
 674
 675     body1.velocity += body1.invMass*impulse;
 676     body1.angularVelocity += body1.invI*r2.cross(impulse);
 677
 678     accimp += impulse;
 679   }
 680 }
 681
 682
 683 // ////////////////////////////////////////////////////////////////////////// //
 684 private struct Contact {
 685 public:
 686   Vec2 position; // updated in collide()
 687   Vec2 normal; // updated in collide()
 688   Vec2 r1, r2;
 689   VFloat separation = VFloatNum!0; // updated in collide(), negative of penetration
 690   VFloat accimpN = VFloatNum!0; // accumulated normal impulse
 691   VFloat accimpT = VFloatNum!0; // accumulated tangent impulse
 692   VFloat accimpNb = VFloatNum!0;  // accumulated normal impulse for position bias
 693   VFloat massNormal, massTangent;
 694   VFloat bias = VFloatNum!0;
 695 }
 696
 697
 698 // ////////////////////////////////////////////////////////////////////////// //
 699 private struct Arbiter {
 700 public:
 701   enum MaxContactPoints = 2;
 702
 703   static private VFloat clamp() (VFloat a, VFloat low, VFloat high) { pragma(inline, true); import std.algorithm : min, max; return max(low, min(a, high)); }
 704
 705 public:
 706   Contact[MaxContactPoints] contacts;
 707   int numContacts;
 708
 709   BodyBase body0, body1;
 710   VFloat friction; // combined friction
 711   uint frameNum; // used to track "frame touch"
 712
 713 public:
 714   this (BodyBase b0, BodyBase b1, int aFrameNum) { setup(b0, b1, frameNum); }
 715
 716   @disable this (this);
 717
 718   void clear () { numContacts = 0; }
 719
 720   @property bool active () { return (numContacts > 0); }
 721
 722   void setup (BodyBase b0, BodyBase b1, int aFrameNum) {
 723     frameNum = aFrameNum;
 724     import core.stdc.math : sqrtf;
 725     if (b0 < b1) {
 726       body0 = b0;
 727       body1 = b1;
 728     } else {
 729       body0 = b1;
 730       body1 = b0;
 731     }
 732     numContacts = collide(contacts[], body0, body1);
 733     friction = sqrtf(body0.friction*body1.friction);
 734     if (b2dlDrawContactsCB !is null) {
 735       BodyContact bc;
 736       foreach (const ref ctx; contacts[0..numContacts]) {
 737         bc.position = ctx.position;
 738         bc.normal = ctx.normal;
 739         bc.separation = ctx.separation;
 740         b2dlDrawContactsCB(bc);
 741       }
 742     }
 743   }
 744
 745   void preStep (VFloat invDt) {
 746     import std.algorithm : min;
 747     enum AllowedPenetration = VFloatNum!(0.01);
 748     immutable VFloat biasFactor = (World.positionCorrection ? VFloatNum!(0.2) : VFloatNum!0);
 749     foreach (immutable idx; 0..numContacts) {
 750       Contact *c = contacts.ptr+idx;
 751       Vec2 r1 = c.position-body0.mPosition;
 752       Vec2 r2 = c.position-body1.mPosition;
 753
 754       // precompute normal mass, tangent mass, and bias
 755       VFloat rn1 = r1*c.normal; //Dot(r1, c.normal);
 756       VFloat rn2 = r2*c.normal; //Dot(r2, c.normal);
 757       VFloat kNormal = body0.invMass+body1.invMass;
 758       //kNormal += body0.invI*(Dot(r1, r1)-rn1*rn1)+body1.invI*(Dot(r2, r2)-rn2*rn2);
 759       kNormal += body0.invI*((r1*r1)-rn1*rn1)+body1.invI*((r2*r2)-rn2*rn2);
 760       c.massNormal = VFloatNum!1/kNormal;
 761
 762       //Vec2 tangent = c.normal.cross(VFloatNum!1);
 763       Vec2 tangent = Vec2(VFloatNum!1*c.normal.y, -VFloatNum!1*c.normal.x);
 764       VFloat rt1 = r1*tangent; //Dot(r1, tangent);
 765       VFloat rt2 = r2*tangent; //Dot(r2, tangent);
 766       VFloat kTangent = body0.invMass+body1.invMass;
 767       //kTangent += body0.invI*(Dot(r1, r1)-rt1*rt1)+body1.invI*(Dot(r2, r2)-rt2*rt2);
 768       kTangent += body0.invI*((r1*r1)-rt1*rt1)+body1.invI*((r2*r2)-rt2*rt2);
 769       c.massTangent = VFloatNum!1/kTangent;
 770
 771       c.bias = -biasFactor*invDt*min(VFloatNum!0, c.separation+AllowedPenetration);
 772
 773       if (World.accumulateImpulses) {
 774         // apply normal + friction impulse
 775         Vec2 accimp = c.accimpN*c.normal+c.accimpT*tangent;
 776
 777         body0.velocity -= body0.invMass*accimp;
 778         body0.angularVelocity -= body0.invI*r1.cross(accimp);
 779
 780         body1.velocity += body1.invMass*accimp;
 781         body1.angularVelocity += body1.invI*r2.cross(accimp);
 782       }
 783     }
 784   }
 785
 786   void applyImpulse () {
 787     import std.algorithm : max;
 788     BodyBase b0 = body0;
 789     BodyBase b1 = body1;
 790     foreach (immutable idx; 0..numContacts) {
 791       Contact *c = contacts.ptr+idx;
 792       c.r1 = c.position-b0.mPosition;
 793       c.r2 = c.position-b1.mPosition;
 794
 795       // relative velocity at contact
 796       Vec2 dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 797
 798       // compute normal impulse
 799       VFloat vn = dv*c.normal; //Dot(dv, c.normal);
 800
 801       VFloat dPn = c.massNormal*(-vn+c.bias);
 802
 803       if (World.accumulateImpulses) {
 804         // clamp the accumulated impulse
 805         VFloat accimpN0 = c.accimpN;
 806         c.accimpN = max(accimpN0+dPn, VFloatNum!0);
 807         dPn = c.accimpN-accimpN0;
 808       } else {
 809         dPn = max(dPn, VFloatNum!0);
 810       }
 811
 812       // apply contact impulse
 813       Vec2 accimpN = dPn*c.normal;
 814
 815       b0.velocity -= b0.invMass*accimpN;
 816       b0.angularVelocity -= b0.invI*c.r1.cross(accimpN);
 817
 818       b1.velocity += b1.invMass*accimpN;
 819       b1.angularVelocity += b1.invI*c.r2.cross(accimpN);
 820
 821       // relative velocity at contact
 822       dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 823
 824       //Vec2 tangent = c.normal.cross(VFloatNum!1);
 825       Vec2 tangent = Vec2(VFloatNum!1*c.normal.y, -VFloatNum!1*c.normal.x);
 826       VFloat vt = dv*tangent; //Dot(dv, tangent);
 827       VFloat dPt = c.massTangent*(-vt);
 828
 829       if (World.accumulateImpulses) {
 830         // compute friction impulse
 831         VFloat maxPt = friction*c.accimpN;
 832         // clamp friction
 833         VFloat oldTangentImpulse = c.accimpT;
 834         c.accimpT = clamp(oldTangentImpulse+dPt, -maxPt, maxPt);
 835         dPt = c.accimpT-oldTangentImpulse;
 836       } else {
 837         VFloat maxPt = friction*dPn;
 838         dPt = clamp(dPt, -maxPt, maxPt);
 839       }
 840
 841       // apply contact impulse
 842       Vec2 accimpT = dPt*tangent;
 843
 844       b0.velocity -= b0.invMass*accimpT;
 845       b0.angularVelocity -= b0.invI*c.r1.cross(accimpT);
 846
 847       b1.velocity += b1.invMass*accimpT;
 848       b1.angularVelocity += b1.invI*c.r2.cross(accimpT);
 849     }
 850   }
 851 }
 852
 853
 854 // ////////////////////////////////////////////////////////////////////////// //
 855 // collide
 856 // the normal points from A to B
 857 // return number of contact points
 858 // this fills:
 859 //   position (already moved out of body)
 860 //   normal
 861 //   separation (this is negative penetration)
 862 //   feature (used only in arbiter updates to merge contacts, and has no effect in b2dlite)
 863 // return number of contacts
 864 private int collide (Contact[] contacts, BodyBase bodyAb, BodyBase bodyBb) {
 865   auto pb0 = cast(PolyBody)bodyAb;
 866   auto pb1 = cast(PolyBody)bodyBb;
 867   if (pb0 is null || pb1 is null) assert(0);
 868
 869   ContactInfo ci;
 870   polygon2Polygon(ci, pb0, pb1);
 871   if (!ci.valid) return 0; // no contacts
 872
 873   foreach (immutable cidx; 0..ci.contactCount) {
 874     contacts[cidx].normal = ci.normal;
 875     contacts[cidx].separation = -ci.penetration;
 876     contacts[cidx].position = ci.contacts[cidx]+(ci.normal*ci.penetration);
 877   }
 878
 879   return ci.contactCount;
 880 }
 881
 882
 883 // ////////////////////////////////////////////////////////////////////////// //
 884 private struct ContactInfo {
 885   VFloat penetration; // depth of penetration from collision
 886   Vec2 normal; // from A to B
 887   Vec2[2] contacts; // points of contact during collision
 888   uint contactCount; // number of contacts that occured during collision
 889
 890   @property bool valid () const pure nothrow @safe @nogc { pragma(inline, true); return (contactCount > 0); }
 891 }
 892
 893
 894 private bool biasGreaterThan (VFloat a, VFloat b) {
 895   enum k_biasRelative = VFloatNum!(0.95);
 896   enum k_biasAbsolute = VFloatNum!(0.01);
 897   return (a >= b*k_biasRelative+a*k_biasAbsolute);
 898 }
 899
 900
 901 private VFloat findAxisLeastPenetration (uint *faceIndex, PolyBody flesha, PolyBody fleshb) {
 902   VFloat bestDistance = -VFloat.max;
 903   uint bestIndex;
 904   foreach (immutable i; 0..flesha.verts.length) {
 905     // retrieve a face normal from A
 906     Vec2 n = flesha.norms[i];
 907     Vec2 nw = flesha.rmattmp*n;
 908     // transform face normal into B's model space
 909     Mat22 buT = fleshb.rmattmp.transpose();
 910     n = buT*nw;
 911     // retrieve support point from B along -n
 912     Vec2 s = fleshb.support(-n);
 913     // retrieve vertex on face from A, transform into B's model space
 914     Vec2 v = flesha.verts[i];
 915     v = flesha.rmattmp*v+flesha.mPosition;
 916     v -= fleshb.mPosition;
 917     v = buT*v;
 918     // compute penetration distance (in B's model space)
 919     VFloat d = n.dot(s-v);
 920     // store greatest distance
 921     if (d > bestDistance) {
 922       bestDistance = d;
 923       bestIndex = i;
 924     }
 925   }
 926   *faceIndex = bestIndex;
 927   return bestDistance;
 928 }
 929
 930
 931 private void findIncidentFace (Vec2[] v, PolyBody RefPoly, PolyBody IncPoly, uint referenceIndex) {
 932   Vec2 referenceNormal = RefPoly.norms[referenceIndex];
 933   // calculate normal in incident's frame of reference
 934   referenceNormal = RefPoly.rmattmp*referenceNormal; // to world space
 935   referenceNormal = IncPoly.rmattmp.transpose()*referenceNormal; // to incident's model space
 936   // find most anti-normal face on incident polygon
 937   uint incidentFace = 0;
 938   VFloat minDot = VFloat.max;
 939   foreach (immutable i; 0..IncPoly.verts.length) {
 940     VFloat dot = referenceNormal.dot(IncPoly.norms[i]);
 941     if (dot < minDot) {
 942       minDot = dot;
 943       incidentFace = i;
 944     }
 945   }
 946   // assign face vertices for incidentFace
 947   v[0] = IncPoly.rmattmp*IncPoly.verts[incidentFace]+IncPoly.mPosition;
 948   incidentFace = (incidentFace+1)%IncPoly.verts.length;
 949   v[1] = IncPoly.rmattmp*IncPoly.verts[incidentFace]+IncPoly.mPosition;
 950 }
 951
 952
 953 private uint clip (Vec2 n, VFloat c, Vec2[] face) {
 954   uint sp = 0;
 955   Vec2[2] outv = face[0..2];
 956   // retrieve distances from each endpoint to the line
 957   // d = ax + by - c
 958   VFloat d1 = n.dot(face[0])-c;
 959   VFloat d2 = n.dot(face[1])-c;
 960   // if negative (behind plane) clip
 961   if (d1 <= VFloatNum!0) outv[sp++] = face[0];
 962   if (d2 <= VFloatNum!0) outv[sp++] = face[1];
 963   // if the points are on different sides of the plane
 964   if (d1*d2 < VFloatNum!0) { // less than to ignore -0.0f
 965     // push interesection point
 966     VFloat alpha = d1/(d1-d2);
 967     outv[sp] = face[0]+alpha*(face[1]-face[0]);
 968     ++sp;
 969   }
 970   // assign our new converted values
 971   face[0] = outv[0];
 972   face[1] = outv[1];
 973   assert(sp != 3);
 974   return sp;
 975 }
 976
 977
 978 private void polygon2Polygon (ref ContactInfo m, PolyBody flesha, PolyBody fleshb) {
 979   flesha.rmattmp = Mat22(flesha.mRotation);
 980   fleshb.rmattmp = Mat22(fleshb.mRotation);
 981
 982   m.contactCount = 0;
 983
 984   // check for a separating axis with A's face planes
 985   uint faceA;
 986   VFloat penetrationA = findAxisLeastPenetration(&faceA, flesha, fleshb);
 987   if (penetrationA >= VFloatNum!0) return;
 988
 989   // check for a separating axis with B's face planes
 990   uint faceB;
 991   VFloat penetrationB = findAxisLeastPenetration(&faceB, fleshb, flesha);
 992   if (penetrationB >= VFloatNum!0) return;
 993
 994   uint referenceIndex;
 995   bool flip; // Always point from a to b
 996
 997   PolyBody RefPoly; // Reference
 998   PolyBody IncPoly; // Incident
 999
1000   // determine which shape contains reference face
1001   if (biasGreaterThan(penetrationA, penetrationB)) {
1002     RefPoly = flesha;
1003     IncPoly = fleshb;
1004     referenceIndex = faceA;
1005     flip = false;
1006   } else {
1007     RefPoly = fleshb;
1008     IncPoly = flesha;
1009     referenceIndex = faceB;
1010     flip = true;
1011   }
1012
1013   // world space incident face
1014   Vec2[2] incidentFace;
1015   findIncidentFace(incidentFace[], RefPoly, IncPoly, referenceIndex);
1016
1017   //        y
1018   //        ^  .n       ^
1019   //      +---c ------posPlane--
1020   //  x < | i |\
1021   //      +---+ c-----negPlane--
1022   //             \       v
1023   //              r
1024   //
1025   //  r : reference face
1026   //  i : incident poly
1027   //  c : clipped point
1028   //  n : incident normal
1029
1030   // setup reference face vertices
1031   Vec2 v1 = RefPoly.verts[referenceIndex];
1032   referenceIndex = (referenceIndex+1)%RefPoly.verts.length;
1033   Vec2 v2 = RefPoly.verts[referenceIndex];
1034
1035   // transform vertices to world space
1036   v1 = RefPoly.rmattmp*v1+RefPoly.mPosition;
1037   v2 = RefPoly.rmattmp*v2+RefPoly.mPosition;
1038
1039   // calculate reference face side normal in world space
1040   Vec2 sidePlaneNormal = v2-v1;
1041   sidePlaneNormal.normalize;
1042
1043   // orthogonalize
1044   auto refFaceNormal = Vec2(sidePlaneNormal.y, -sidePlaneNormal.x);
1045
1046   // ax + by = c
1047   // c is distance from origin
1048   VFloat refC = refFaceNormal.dot(v1);
1049   VFloat negSide = -sidePlaneNormal.dot(v1);
1050   VFloat posSide =  sidePlaneNormal.dot(v2);
1051
1052   // clip incident face to reference face side planes
1053   if (clip(-sidePlaneNormal, negSide, incidentFace) < 2) return; // due to floating point error, possible to not have required points
1054   if (clip(sidePlaneNormal, posSide, incidentFace) < 2) return; // due to floating point error, possible to not have required points
1055
1056   // flip
1057   m.normal = (flip ? -refFaceNormal : refFaceNormal);
1058
1059   // keep points behind reference face
1060   uint cp = 0; // clipped points behind reference face
1061   VFloat separation = refFaceNormal.dot(incidentFace[0])-refC;
1062   if (separation <= VFloatNum!0) {
1063     m.contacts[cp] = incidentFace[0];
1064     m.penetration = -separation;
1065     ++cp;
1066   } else {
1067     m.penetration = 0;
1068   }
1069
1070   separation = refFaceNormal.dot(incidentFace[1])-refC;
1071   if (separation <= VFloatNum!0) {
1072     m.contacts[cp] = incidentFace[1];
1073     m.penetration += -separation;
1074     ++cp;
1075     // average penetration
1076     m.penetration /= cast(VFloat)cp;
1077   }
1078
1079   m.contactCount = cp;
1080 }
1081
1082
1083 // ////////////////////////////////////////////////////////////////////////// //
1084 /* Dynamic AABB tree (bounding volume hierarchy)
1085  * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
1086  * Copyright (c) 2010-2016 Daniel Chappuis
1087  *
1088  * This software is provided 'as-is', without any express or implied warranty.
1089  * In no event will the authors be held liable for any damages arising from the
1090  * use of this software.
1091  *
1092  * Permission is granted to anyone to use this software for any purpose,
1093  * including commercial applications, and to alter it and redistribute it
1094  * freely, subject to the following restrictions:
1095  *
1096  * 1. The origin of this software must not be misrepresented; you must not claim
1097  *    that you wrote the original software. If you use this software in a
1098  *    product, an acknowledgment in the product documentation would be
1099  *    appreciated but is not required.
1100  *
1101  * 2. Altered source versions must be plainly marked as such, and must not be
1102  *    misrepresented as being the original software.
1103  *
1104  * 3. This notice may not be removed or altered from any source distribution.
1105  */
1106 private struct AABBBase(VT) if (Vec2.isVector!VT) {
1107 private:
1108   VT mMin, mMax;
1109
1110 public:
1111   alias VType = VT;
1112
1113 public pure nothrow @safe @nogc:
1114   @property ref const(VT) min () const { pragma(inline, true); return mMin; }
1115   @property ref const(VT) max () const { pragma(inline, true); return mMax; }
1116
1117   @property void min() (in auto ref VT v) { pragma(inline, true); mMin = v; }
1118   @property void max() (in auto ref VT v) { pragma(inline, true); mMax = v; }
1119
1120   // return the volume of the AABB
1121   @property VFloat volume () const {
1122     immutable diff = mMax-mMin;
1123     static if (VT.isVector3!VT) {
1124       return diff.x*diff.y*diff.z;
1125     } else {
1126       return diff.x*diff.y;
1127     }
1128   }
1129
1130   static auto mergeAABBs() (in auto ref AABB aabb1, in auto ref AABB aabb2) {
1131     import std.algorithm : max, min;
1132     typeof(this) res;
1133     res.merge(aabb1, aabb2);
1134     return res;
1135   }
1136
1137   void merge() (in auto ref AABB aabb1, in auto ref AABB aabb2) {
1138     import std.algorithm : max, min;
1139     mMin.x = min(aabb1.mMin.x, aabb2.mMin.x);
1140     mMin.y = min(aabb1.mMin.y, aabb2.mMin.y);
1141     mMax.x = max(aabb1.mMax.x, aabb2.mMax.x);
1142     mMax.y = max(aabb1.mMax.y, aabb2.mMax.y);
1143     static if (VT.isVector3!VT) {
1144       mMin.z = min(aabb1.mMin.z, aabb2.mMin.z);
1145       mMax.z = max(aabb1.mMax.z, aabb2.mMax.z);
1146     }
1147   }
1148
1149   // return true if the current AABB contains the AABB given in parameter
1150   bool contains() (in auto ref AABB aabb) const {
1151     bool isInside = true;
1152     isInside = isInside && mMin.x <= aabb.mMin.x;
1153     isInside = isInside && mMin.y <= aabb.mMin.y;
1154     isInside = isInside && mMax.x >= aabb.mMax.x;
1155     isInside = isInside && mMax.y >= aabb.mMax.y;
1156     static if (VT.isVector3!VT) {
1157       isInside = isInside && mMin.z <= aabb.mMin.z;
1158       isInside = isInside && mMax.z >= aabb.mMax.z;
1159     }
1160     return isInside;
1161   }
1162
1163   // return true if the current AABB is overlapping with the AABB in argument
1164   // two AABBs overlap if they overlap in the two(three) x, y (and z) axes at the same time
1165   bool overlaps() (in auto ref AABB aabb) const {
1166     if (mMax.x < aabb.mMin.x || aabb.mMax.x < mMin.x) return false;
1167     if (mMax.y < aabb.mMin.y || aabb.mMax.y < mMin.y) return false;
1168     static if (VT.isVector3!VT) {
1169       if (mMax.z < aabb.mMin.z || aabb.mMax.z < mMin.z) return false;
1170     }
1171     return true;
1172   }
1173 }
1174
1175 alias AABB = AABBBase!Vec2;
1176
1177
1178 // ////////////////////////////////////////////////////////////////////////// //
1179 private align(1) struct TreeNode {
1180 align(1):
1181   enum NullTreeNode = -1;
1182   enum { Left = 0, Right = 1 }
1183   // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
1184   union {
1185     int parentId;
1186     int nextNodeId;
1187   }
1188   // a node is either a leaf (has data) or is an internal node (has children)
1189   union {
1190     int[2] children; /// left and right child of the node (children[0] = left child)
1191     BodyBase flesh;
1192   }
1193   // height of the node in the tree
1194   short height;
1195   // fat axis aligned bounding box (AABB) corresponding to the node
1196   AABB aabb;
1197   // return true if the node is a leaf of the tree
1198   @property bool leaf () const pure nothrow @safe @nogc { pragma(inline, true); return (height == 0); }
1199 }
1200
1201
1202 // ////////////////////////////////////////////////////////////////////////// //
1203 /*
1204  * This class implements a dynamic AABB tree that is used for broad-phase
1205  * collision detection. This data structure is inspired by Nathanael Presson's
1206  * dynamic tree implementation in BulletPhysics. The following implementation is
1207  * based on the one from Erin Catto in Box2D as described in the book
1208  * "Introduction to Game Physics with Box2D" by Ian Parberry.
1209  */
1210 private final class DynamicAABBTree {
1211   private import std.algorithm : max, min;
1212
1213   // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
1214   // also inflated in direction of the linear motion of the body by mutliplying the
1215   // followin constant with the linear velocity and the elapsed time between two frames
1216   enum VFloat LinearMotionGapMultiplier = VFloatNum!(1.7);
1217
1218 public:
1219   // called when a overlapping node has been found during the call to reportAllShapesOverlappingWithAABB()
1220   alias OverlapCallback = void delegate (int nodeId);
1221
1222 private:
1223   TreeNode* mNodes; // pointer to the memory location of the nodes of the tree
1224   int mRootNodeId; // id of the root node of the tree
1225   int mFreeNodeId; // id of the first node of the list of free (allocated) nodes in the tree that we can use
1226   int mAllocCount; // number of allocated nodes in the tree
1227   int mNodeCount; // number of nodes in the tree
1228
1229   // extra AABB Gap used to allow the collision shape to move a little bit
1230   // without triggering a large modification of the tree which can be costly
1231   VFloat mExtraGap;
1232
1233 private:
1234   // allocate and return a node to use in the tree
1235   int allocateNode () {
1236     // if there is no more allocated node to use
1237     if (mFreeNodeId == TreeNode.NullTreeNode) {
1238       import core.stdc.stdlib : realloc;
1239       version(b2dlite_bvh_many_asserts) assert(mNodeCount == mAllocCount);
1240       // allocate more nodes in the tree
1241       auto newsz = mAllocCount*2;
1242       if (newsz-mAllocCount > 4096) newsz = mAllocCount+4096;
1243       TreeNode* nn = cast(TreeNode*)realloc(mNodes, newsz*TreeNode.sizeof);
1244       if (nn is null) assert(0, "out of memory");
1245       mAllocCount = newsz;
1246       mNodes = nn;
1247       // initialize the allocated nodes
1248       foreach (int i; mNodeCount..mAllocCount-1) {
1249         mNodes[i].nextNodeId = i+1;
1250         mNodes[i].height = -1;
1251       }
1252       mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
1253       mNodes[mAllocCount-1].height = -1;
1254       mFreeNodeId = mNodeCount;
1255     }
1256     // get the next free node
1257     int freeNodeId = mFreeNodeId;
1258     mFreeNodeId = mNodes[freeNodeId].nextNodeId;
1259     mNodes[freeNodeId].parentId = TreeNode.NullTreeNode;
1260     mNodes[freeNodeId].height = 0;
1261     ++mNodeCount;
1262     return freeNodeId;
1263   }
1264
1265   // release a node
1266   void releaseNode (int nodeId) {
1267     version(b2dlite_bvh_many_asserts) assert(mNodeCount > 0);
1268     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1269     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0);
1270     mNodes[nodeId].nextNodeId = mFreeNodeId;
1271     mNodes[nodeId].height = -1;
1272     mFreeNodeId = nodeId;
1273     --mNodeCount;
1274   }
1275
1276   // insert a leaf node in the tree
1277   // 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
1278   void insertLeafNode (int nodeId) {
1279     // if the tree is empty
1280     if (mRootNodeId == TreeNode.NullTreeNode) {
1281       mRootNodeId = nodeId;
1282       mNodes[mRootNodeId].parentId = TreeNode.NullTreeNode;
1283       return;
1284     }
1285
1286     version(b2dlite_bvh_many_asserts) assert(mRootNodeId != TreeNode.NullTreeNode);
1287
1288     // find the best sibling node for the new node
1289     AABB newNodeAABB = mNodes[nodeId].aabb;
1290     int currentNodeId = mRootNodeId;
1291     while (!mNodes[currentNodeId].leaf) {
1292       int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1293       int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1294
1295       // compute the merged AABB
1296       VFloat volumeAABB = mNodes[currentNodeId].aabb.volume;
1297       AABB mergedAABBs = AABB.mergeAABBs(mNodes[currentNodeId].aabb, newNodeAABB);
1298       VFloat mergedVolume = mergedAABBs.volume;
1299
1300       // compute the cost of making the current node the sibbling of the new node
1301       VFloat costS = VFloatNum!(2.0)*mergedVolume;
1302
1303       // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
1304       VFloat costI = VFloatNum!(2.0)*(mergedVolume-volumeAABB);
1305
1306       // compute the cost of descending into the left child
1307       VFloat costLeft;
1308       AABB currentAndLeftAABB = AABB.mergeAABBs(newNodeAABB, mNodes[leftChild].aabb);
1309       if (mNodes[leftChild].leaf) {
1310         costLeft = currentAndLeftAABB.volume+costI;
1311       } else {
1312         VFloat leftChildVolume = mNodes[leftChild].aabb.volume;
1313         costLeft = costI+currentAndLeftAABB.volume-leftChildVolume;
1314       }
1315
1316       // compute the cost of descending into the right child
1317       VFloat costRight;
1318       AABB currentAndRightAABB = AABB.mergeAABBs(newNodeAABB, mNodes[rightChild].aabb);
1319       if (mNodes[rightChild].leaf) {
1320         costRight = currentAndRightAABB.volume+costI;
1321       } else {
1322         VFloat rightChildVolume = mNodes[rightChild].aabb.volume;
1323         costRight = costI+currentAndRightAABB.volume-rightChildVolume;
1324       }
1325
1326       // 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
1327       if (costS < costLeft && costS < costRight) break;
1328
1329       // it is cheaper to go down into a child of the current node, choose the best child
1330       currentNodeId = (costLeft < costRight ? leftChild : rightChild);
1331     }
1332
1333     int siblingNode = currentNodeId;
1334
1335     // create a new parent for the new node and the sibling node
1336     int oldParentNode = mNodes[siblingNode].parentId;
1337     int newParentNode = allocateNode();
1338     mNodes[newParentNode].parentId = oldParentNode;
1339     mNodes[newParentNode].aabb.merge(mNodes[siblingNode].aabb, newNodeAABB);
1340     mNodes[newParentNode].height = cast(short)(mNodes[siblingNode].height+1);
1341     version(b2dlite_bvh_many_asserts) assert(mNodes[newParentNode].height > 0);
1342
1343     // If the sibling node was not the root node
1344     if (oldParentNode != TreeNode.NullTreeNode) {
1345       version(b2dlite_bvh_many_asserts) assert(!mNodes[oldParentNode].leaf);
1346       if (mNodes[oldParentNode].children.ptr[TreeNode.Left] == siblingNode) {
1347         mNodes[oldParentNode].children.ptr[TreeNode.Left] = newParentNode;
1348       } else {
1349         mNodes[oldParentNode].children.ptr[TreeNode.Right] = newParentNode;
1350       }
1351       mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
1352       mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
1353       mNodes[siblingNode].parentId = newParentNode;
1354       mNodes[nodeId].parentId = newParentNode;
1355     } else {
1356       // if the sibling node was the root node
1357       mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
1358       mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
1359       mNodes[siblingNode].parentId = newParentNode;
1360       mNodes[nodeId].parentId = newParentNode;
1361       mRootNodeId = newParentNode;
1362     }
1363
1364     // move up in the tree to change the AABBs that have changed
1365     currentNodeId = mNodes[nodeId].parentId;
1366     version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1367     while (currentNodeId != TreeNode.NullTreeNode) {
1368       // balance the sub-tree of the current node if it is not balanced
1369       currentNodeId = balanceSubTreeAtNode(currentNodeId);
1370       version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1371
1372       version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1373       int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1374       int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1375       version(b2dlite_bvh_many_asserts) assert(leftChild != TreeNode.NullTreeNode);
1376       version(b2dlite_bvh_many_asserts) assert(rightChild != TreeNode.NullTreeNode);
1377
1378       // recompute the height of the node in the tree
1379       mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChild].height, mNodes[rightChild].height)+1);
1380       version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0);
1381
1382       // recompute the AABB of the node
1383       mNodes[currentNodeId].aabb.merge(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
1384
1385       currentNodeId = mNodes[currentNodeId].parentId;
1386     }
1387
1388     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1389   }
1390
1391   // remove a leaf node from the tree
1392   void removeLeafNode (int nodeId) {
1393     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1394     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1395
1396     // If we are removing the root node (root node is a leaf in this case)
1397     if (mRootNodeId == nodeId) { mRootNodeId = TreeNode.NullTreeNode; return; }
1398
1399     int parentNodeId = mNodes[nodeId].parentId;
1400     int grandParentNodeId = mNodes[parentNodeId].parentId;
1401     int siblingNodeId;
1402
1403     if (mNodes[parentNodeId].children.ptr[TreeNode.Left] == nodeId) {
1404       siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Right];
1405     } else {
1406       siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Left];
1407     }
1408
1409     // if the parent of the node to remove is not the root node
1410     if (grandParentNodeId != TreeNode.NullTreeNode) {
1411       // destroy the parent node
1412       if (mNodes[grandParentNodeId].children.ptr[TreeNode.Left] == parentNodeId) {
1413         mNodes[grandParentNodeId].children.ptr[TreeNode.Left] = siblingNodeId;
1414       } else {
1415         version(b2dlite_bvh_many_asserts) assert(mNodes[grandParentNodeId].children.ptr[TreeNode.Right] == parentNodeId);
1416         mNodes[grandParentNodeId].children.ptr[TreeNode.Right] = siblingNodeId;
1417       }
1418       mNodes[siblingNodeId].parentId = grandParentNodeId;
1419       releaseNode(parentNodeId);
1420
1421       // 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
1422       int currentNodeId = grandParentNodeId;
1423       while (currentNodeId != TreeNode.NullTreeNode) {
1424         // balance the current sub-tree if necessary
1425         currentNodeId = balanceSubTreeAtNode(currentNodeId);
1426
1427         version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf);
1428
1429         // get the two children.ptr of the current node
1430         int leftChildId = mNodes[currentNodeId].children.ptr[TreeNode.Left];
1431         int rightChildId = mNodes[currentNodeId].children.ptr[TreeNode.Right];
1432
1433         // recompute the AABB and the height of the current node
1434         mNodes[currentNodeId].aabb.merge(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb);
1435         mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChildId].height, mNodes[rightChildId].height)+1);
1436         version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0);
1437
1438         currentNodeId = mNodes[currentNodeId].parentId;
1439       }
1440     } else {
1441       // if the parent of the node to remove is the root node, the sibling node becomes the new root node
1442       mRootNodeId = siblingNodeId;
1443       mNodes[siblingNodeId].parentId = TreeNode.NullTreeNode;
1444       releaseNode(parentNodeId);
1445     }
1446   }
1447
1448   // balance the sub-tree of a given node using left or right rotations
1449   // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
1450   // this method returns the new root node Id
1451   int balanceSubTreeAtNode (int nodeId) {
1452     version(b2dlite_bvh_many_asserts) assert(nodeId != TreeNode.NullTreeNode);
1453
1454     TreeNode* nodeA = mNodes+nodeId;
1455
1456     // if the node is a leaf or the height of A's sub-tree is less than 2
1457     if (nodeA.leaf || nodeA.height < 2) return nodeId; // do not perform any rotation
1458
1459     // get the two children nodes
1460     int nodeBId = nodeA.children.ptr[TreeNode.Left];
1461     int nodeCId = nodeA.children.ptr[TreeNode.Right];
1462     version(b2dlite_bvh_many_asserts) assert(nodeBId >= 0 && nodeBId < mAllocCount);
1463     version(b2dlite_bvh_many_asserts) assert(nodeCId >= 0 && nodeCId < mAllocCount);
1464     TreeNode* nodeB = mNodes+nodeBId;
1465     TreeNode* nodeC = mNodes+nodeCId;
1466
1467     // compute the factor of the left and right sub-trees
1468     int balanceFactor = nodeC.height-nodeB.height;
1469
1470     // if the right node C is 2 higher than left node B
1471     if (balanceFactor > 1) {
1472       version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf);
1473
1474       int nodeFId = nodeC.children.ptr[TreeNode.Left];
1475       int nodeGId = nodeC.children.ptr[TreeNode.Right];
1476       version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
1477       version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
1478       TreeNode* nodeF = mNodes+nodeFId;
1479       TreeNode* nodeG = mNodes+nodeGId;
1480
1481       nodeC.children.ptr[TreeNode.Left] = nodeId;
1482       nodeC.parentId = nodeA.parentId;
1483       nodeA.parentId = nodeCId;
1484
1485       if (nodeC.parentId != TreeNode.NullTreeNode) {
1486         if (mNodes[nodeC.parentId].children.ptr[TreeNode.Left] == nodeId) {
1487           mNodes[nodeC.parentId].children.ptr[TreeNode.Left] = nodeCId;
1488         } else {
1489           version(b2dlite_bvh_many_asserts) assert(mNodes[nodeC.parentId].children.ptr[TreeNode.Right] == nodeId);
1490           mNodes[nodeC.parentId].children.ptr[TreeNode.Right] = nodeCId;
1491         }
1492       } else {
1493         mRootNodeId = nodeCId;
1494       }
1495
1496       version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf);
1497       version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf);
1498
1499       // if the right node C was higher than left node B because of the F node
1500       if (nodeF.height > nodeG.height) {
1501         nodeC.children.ptr[TreeNode.Right] = nodeFId;
1502         nodeA.children.ptr[TreeNode.Right] = nodeGId;
1503         nodeG.parentId = nodeId;
1504
1505         // recompute the AABB of node A and C
1506         nodeA.aabb.merge(nodeB.aabb, nodeG.aabb);
1507         nodeC.aabb.merge(nodeA.aabb, nodeF.aabb);
1508
1509         // recompute the height of node A and C
1510         nodeA.height = cast(short)(max(nodeB.height, nodeG.height)+1);
1511         nodeC.height = cast(short)(max(nodeA.height, nodeF.height)+1);
1512         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1513         version(b2dlite_bvh_many_asserts) assert(nodeC.height > 0);
1514       } else {
1515         // if the right node C was higher than left node B because of node G
1516         nodeC.children.ptr[TreeNode.Right] = nodeGId;
1517         nodeA.children.ptr[TreeNode.Right] = nodeFId;
1518         nodeF.parentId = nodeId;
1519
1520         // recompute the AABB of node A and C
1521         nodeA.aabb.merge(nodeB.aabb, nodeF.aabb);
1522         nodeC.aabb.merge(nodeA.aabb, nodeG.aabb);
1523
1524         // recompute the height of node A and C
1525         nodeA.height = cast(short)(max(nodeB.height, nodeF.height)+1);
1526         nodeC.height = cast(short)(max(nodeA.height, nodeG.height)+1);
1527         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1528         version(b2dlite_bvh_many_asserts) assert(nodeC.height > 0);
1529       }
1530
1531       // return the new root of the sub-tree
1532       return nodeCId;
1533     }
1534
1535     // if the left node B is 2 higher than right node C
1536     if (balanceFactor < -1) {
1537       version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf);
1538
1539       int nodeFId = nodeB.children.ptr[TreeNode.Left];
1540       int nodeGId = nodeB.children.ptr[TreeNode.Right];
1541       version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
1542       version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
1543       TreeNode* nodeF = mNodes+nodeFId;
1544       TreeNode* nodeG = mNodes+nodeGId;
1545
1546       nodeB.children.ptr[TreeNode.Left] = nodeId;
1547       nodeB.parentId = nodeA.parentId;
1548       nodeA.parentId = nodeBId;
1549
1550       if (nodeB.parentId != TreeNode.NullTreeNode) {
1551         if (mNodes[nodeB.parentId].children.ptr[TreeNode.Left] == nodeId) {
1552           mNodes[nodeB.parentId].children.ptr[TreeNode.Left] = nodeBId;
1553         } else {
1554           version(b2dlite_bvh_many_asserts) assert(mNodes[nodeB.parentId].children.ptr[TreeNode.Right] == nodeId);
1555           mNodes[nodeB.parentId].children.ptr[TreeNode.Right] = nodeBId;
1556         }
1557       } else {
1558         mRootNodeId = nodeBId;
1559       }
1560
1561       version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf);
1562       version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf);
1563
1564       // if the left node B was higher than right node C because of the F node
1565       if (nodeF.height > nodeG.height) {
1566         nodeB.children.ptr[TreeNode.Right] = nodeFId;
1567         nodeA.children.ptr[TreeNode.Left] = nodeGId;
1568         nodeG.parentId = nodeId;
1569
1570         // recompute the AABB of node A and B
1571         nodeA.aabb.merge(nodeC.aabb, nodeG.aabb);
1572         nodeB.aabb.merge(nodeA.aabb, nodeF.aabb);
1573
1574         // recompute the height of node A and B
1575         nodeA.height = cast(short)(max(nodeC.height, nodeG.height)+1);
1576         nodeB.height = cast(short)(max(nodeA.height, nodeF.height)+1);
1577         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1578         version(b2dlite_bvh_many_asserts) assert(nodeB.height > 0);
1579       } else {
1580         // if the left node B was higher than right node C because of node G
1581         nodeB.children.ptr[TreeNode.Right] = nodeGId;
1582         nodeA.children.ptr[TreeNode.Left] = nodeFId;
1583         nodeF.parentId = nodeId;
1584
1585         // recompute the AABB of node A and B
1586         nodeA.aabb.merge(nodeC.aabb, nodeF.aabb);
1587         nodeB.aabb.merge(nodeA.aabb, nodeG.aabb);
1588
1589         // recompute the height of node A and B
1590         nodeA.height = cast(short)(max(nodeC.height, nodeF.height)+1);
1591         nodeB.height = cast(short)(max(nodeA.height, nodeG.height)+1);
1592         version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0);
1593         version(b2dlite_bvh_many_asserts) assert(nodeB.height > 0);
1594       }
1595
1596       // return the new root of the sub-tree
1597       return nodeBId;
1598     }
1599
1600     // if the sub-tree is balanced, return the current root node
1601     return nodeId;
1602   }
1603
1604   // compute the height of a given node in the tree
1605   int computeHeight (int nodeId) {
1606     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1607     TreeNode* node = mNodes+nodeId;
1608
1609     // If the node is a leaf, its height is zero
1610     if (node.leaf) return 0;
1611
1612     // Compute the height of the left and right sub-tree
1613     int leftHeight = computeHeight(node.children.ptr[TreeNode.Left]);
1614     int rightHeight = computeHeight(node.children.ptr[TreeNode.Right]);
1615
1616     // Return the height of the node
1617     return 1+max(leftHeight, rightHeight);
1618   }
1619
1620   // internally add an object into the tree
1621   int addObjectInternal() (in auto ref AABB aabb) {
1622     // get the next available node (or allocate new ones if necessary)
1623     int nodeId = allocateNode();
1624
1625     // create the fat aabb to use in the tree
1626     immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap);
1627     mNodes[nodeId].aabb.min = aabb.min-gap;
1628     mNodes[nodeId].aabb.max = aabb.max+gap;
1629
1630     // set the height of the node in the tree
1631     mNodes[nodeId].height = 0;
1632
1633     // insert the new leaf node in the tree
1634     insertLeafNode(nodeId);
1635     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1636
1637     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0);
1638
1639     // return the Id of the node
1640     return nodeId;
1641   }
1642
1643   // initialize the tree
1644   void setup () {
1645     import core.stdc.stdlib : malloc;
1646     import core.stdc.string : memset;
1647
1648     mRootNodeId = TreeNode.NullTreeNode;
1649     mNodeCount = 0;
1650     mAllocCount = 64;
1651
1652     mNodes = cast(TreeNode*)malloc(mAllocCount*TreeNode.sizeof);
1653     if (mNodes is null) assert(0, "out of memory");
1654     memset(mNodes, 0, mAllocCount*TreeNode.sizeof);
1655
1656     // initialize the allocated nodes
1657     foreach (int i; 0..mAllocCount-1) {
1658       mNodes[i].nextNodeId = i+1;
1659       mNodes[i].height = -1;
1660     }
1661     mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
1662     mNodes[mAllocCount-1].height = -1;
1663     mFreeNodeId = 0;
1664   }
1665
1666   // also, checks if the tree structure is valid (for debugging purpose)
1667   void forEachLeaf (scope void delegate (int nodeId, in ref AABB aabb) dg) {
1668     void forEachNode (int nodeId) {
1669       if (nodeId == TreeNode.NullTreeNode) return;
1670       // if it is the root
1671       if (nodeId == mRootNodeId) {
1672         assert(mNodes[nodeId].parentId == TreeNode.NullTreeNode);
1673       }
1674       // get the children nodes
1675       TreeNode* pNode = mNodes+nodeId;
1676       assert(pNode.height >= 0);
1677       assert(pNode.aabb.volume > 0);
1678       // if the current node is a leaf
1679       if (pNode.leaf) {
1680         assert(pNode.height == 0);
1681         if (dg !is null) dg(nodeId, pNode.aabb);
1682       } else {
1683         int leftChild = pNode.children.ptr[TreeNode.Left];
1684         int rightChild = pNode.children.ptr[TreeNode.Right];
1685         // check that the children node Ids are valid
1686         assert(0 <= leftChild && leftChild < mAllocCount);
1687         assert(0 <= rightChild && rightChild < mAllocCount);
1688         // check that the children nodes have the correct parent node
1689         assert(mNodes[leftChild].parentId == nodeId);
1690         assert(mNodes[rightChild].parentId == nodeId);
1691         // check the height of node
1692         int height = 1+max(mNodes[leftChild].height, mNodes[rightChild].height);
1693         assert(mNodes[nodeId].height == height);
1694         // check the AABB of the node
1695         AABB aabb = AABB.mergeAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
1696         assert(aabb.min == mNodes[nodeId].aabb.min);
1697         assert(aabb.max == mNodes[nodeId].aabb.max);
1698         // recursively check the children nodes
1699         forEachNode(leftChild);
1700         forEachNode(rightChild);
1701       }
1702     }
1703     // recursively check each node
1704     forEachNode(mRootNodeId);
1705   }
1706
1707 public:
1708   this (VFloat extraAABBGap=VFloatNum!0) {
1709     mExtraGap = extraAABBGap;
1710     setup();
1711   }
1712
1713   ~this () {
1714     import core.stdc.stdlib : free;
1715     free(mNodes);
1716   }
1717
1718   // return the fat AABB corresponding to a given node Id
1719   /*const ref*/ AABB getFatAABB (int nodeId) const {
1720     pragma(inline, true);
1721     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1722     return mNodes[nodeId].aabb;
1723   }
1724
1725   // return the pointer to the data array of a given leaf node of the tree
1726   BodyBase getNodeBody (int nodeId) {
1727     pragma(inline, true);
1728     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1729     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1730     return mNodes[nodeId].flesh;
1731   }
1732
1733   // return the root AABB of the tree
1734   AABB getRootAABB () { pragma(inline, true); return getFatAABB(mRootNodeId); }
1735
1736   // add an object into the tree.
1737   // this method creates a new leaf node in the tree and returns the Id of the corresponding node
1738   int addObject (BodyBase flesh) {
1739     auto aabb = flesh.getAABB(); // can be passed as argument
1740     int nodeId = addObjectInternal(aabb);
1741     mNodes[nodeId].flesh = flesh;
1742     return nodeId;
1743   }
1744
1745   // remove an object from the tree
1746   void removeObject (int nodeId) {
1747     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1748     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1749     // remove the node from the tree
1750     removeLeafNode(nodeId);
1751     releaseNode(nodeId);
1752   }
1753
1754   // update the dynamic tree after an object has moved
1755   // if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
1756   // otherwise, the corresponding node is removed and reinserted into the tree.
1757   // the method returns true if the object has been reinserted into the tree.
1758   // the "displacement" argument is the linear velocity of the AABB multiplied by the elapsed time between two frames.
1759   // if the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
1760   // (this can be useful if the shape AABB has become much smaller than the previous one for instance).
1761   // return `true` if the tree was modified
1762   bool updateObject() (int nodeId, in auto ref AABB.VType displacement, bool forceReinsert=false) {
1763     version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
1764     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf);
1765     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0);
1766
1767     auto newAABB = mNodes[nodeId].flesh.getAABB(); // can be passed as argument
1768
1769     // if the new AABB is still inside the fat AABB of the node
1770     if (!forceReinsert && mNodes[nodeId].aabb.contains(newAABB)) return false;
1771
1772     // if the new AABB is outside the fat AABB, we remove the corresponding node
1773     removeLeafNode(nodeId);
1774
1775     // compute the fat AABB by inflating the AABB with a constant gap
1776     mNodes[nodeId].aabb = newAABB;
1777     immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap);
1778     mNodes[nodeId].aabb.mMin -= gap;
1779     mNodes[nodeId].aabb.mMax += gap;
1780
1781     // inflate the fat AABB in direction of the linear motion of the AABB
1782     if (displacement.x < VFloatNum!0) {
1783       mNodes[nodeId].aabb.mMin.x += LinearMotionGapMultiplier*displacement.x;
1784     } else {
1785       mNodes[nodeId].aabb.mMax.x += LinearMotionGapMultiplier*displacement.x;
1786     }
1787     if (displacement.y < VFloatNum!0) {
1788       mNodes[nodeId].aabb.mMin.y += LinearMotionGapMultiplier*displacement.y;
1789     } else {
1790       mNodes[nodeId].aabb.mMax.y += LinearMotionGapMultiplier*displacement.y;
1791     }
1792     static if (AABB.VType.isVector3!(AABB.VType)) {
1793       if (displacement.z < VFloatNum!0) {
1794         mNodes[nodeId].aabb.mMin.z += LinearMotionGapMultiplier *displacement.z;
1795       } else {
1796         mNodes[nodeId].aabb.mMax.z += LinearMotionGapMultiplier *displacement.z;
1797       }
1798     }
1799
1800     version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].aabb.contains(newAABB));
1801
1802     // reinsert the node into the tree
1803     insertLeafNode(nodeId);
1804
1805     return true;
1806   }
1807
1808   // report all shapes overlapping with the AABB given in parameter
1809   void reportAllShapesOverlappingWithAABB() (in auto ref AABB aabb, scope OverlapCallback callback) {
1810     int[256] stack = void; // stack with the nodes to visit
1811     int sp = 0;
1812
1813     void spush (int id) {
1814       if (sp >= stack.length) throw new Exception("stack overflow");
1815       stack.ptr[sp++] = id;
1816     }
1817
1818     int spop () {
1819       if (sp == 0) throw new Exception("stack underflow");
1820       return stack.ptr[--sp];
1821     }
1822
1823     spush(mRootNodeId);
1824
1825     // while there are still nodes to visit
1826     while (sp > 0) {
1827       // get the next node id to visit
1828       int nodeIdToVisit = spop();
1829       // skip it if it is a null node
1830       if (nodeIdToVisit == TreeNode.NullTreeNode) continue;
1831       // get the corresponding node
1832       const(TreeNode)* nodeToVisit = mNodes+nodeIdToVisit;
1833       // if the AABB in parameter overlaps with the AABB of the node to visit
1834       if (aabb.overlaps(nodeToVisit.aabb)) {
1835         // if the node is a leaf
1836         if (nodeToVisit.leaf) {
1837           // notify the broad-phase about a new potential overlapping pair
1838           callback(nodeIdToVisit);
1839         } else {
1840           // if the node is not a leaf
1841           // we need to visit its children
1842           spush(nodeToVisit.children.ptr[TreeNode.Left]);
1843           spush(nodeToVisit.children.ptr[TreeNode.Right]);
1844         }
1845       }
1846     }
1847   }
1848
1849   // compute the height of the tree
1850   int computeHeight () { pragma(inline, true); return computeHeight(mRootNodeId); }
1851
1852   // clear all the nodes and reset the tree
1853   void reset() {
1854     import core.stdc.stdlib : free;
1855     free(mNodes);
1856     setup();
1857   }
1858 }