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