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 module b2dlite;
  12 private:
  13
  14 public import iv.vmath;
  15
  16
  17 // ////////////////////////////////////////////////////////////////////////// //
  18 public alias Vec2 = VecN!(2, float);
  19 public alias VFloat = Vec2.VFloat;
  20 public alias VFloatNum = Vec2.VFloatNum;
  21
  22
  23 // ////////////////////////////////////////////////////////////////////////// //
  24 public void delegate (VFloat x, VFloat y) b2dlDrawContactsCB;
  25
  26
  27 // ////////////////////////////////////////////////////////////////////////// //
  28 public struct Mat22 {
  29 nothrow @safe @nogc:
  30 public:
  31   Vec2 col1, col2;
  32
  33 public:
  34   this (VFloat angle) {
  35     pragma(inline, true);
  36     import core.stdc.math : cosf, sinf;
  37     immutable VFloat c = cosf(angle), s = sinf(angle);
  38     col1.x =  c; col1.y = s;
  39     col2.x = -s; col2.y = c;
  40   }
  41
  42 pure:
  43   this() (in auto ref Vec2 acol1, in auto ref Vec2 acol2) { pragma(inline, true); col1 = acol1; col2 = acol2; }
  44
  45   Mat22 transpose () const { pragma(inline, true); return Mat22(Vec2(col1.x, col2.x), Vec2(col1.y, col2.y)); }
  46
  47   Mat22 invert () const {
  48     pragma(inline, true);
  49     immutable VFloat a = col1.x, b = col2.x, c = col1.y, d = col2.y;
  50     Mat22 bm;
  51     VFloat det = a*d-b*c;
  52     assert(det != VFloatNum!(0.0));
  53     det = VFloatNum!(1.0)/det;
  54     bm.col1.x = det*d;
  55     bm.col2.x = -det*b;
  56     bm.col1.y = -det*c;
  57     bm.col2.y = det*a;
  58     return bm;
  59   }
  60
  61   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); }
  62
  63   Mat22 opBinary(string op:"+") (in auto ref Mat22 bm) const { pragma(inline, true); return Mat22(col1+bm.col1, col2+bm.col2); }
  64   Mat22 opBinary(string op:"*") (in auto ref Mat22 bm) const { pragma(inline, true); return Mat22(this*bm.col1, this*bm.col2); }
  65
  66   Mat22 abs() () { pragma(inline, true); return Mat22(col1.abs, col2.abs); }
  67 }
  68
  69
  70 // ////////////////////////////////////////////////////////////////////////// //
  71 private Vec2 fcross() (VFloat s, in auto ref Vec2 a) { pragma(inline, true); return Vec2(-s*a.y, s*a.x); }
  72
  73
  74 // ////////////////////////////////////////////////////////////////////////// //
  75 // body
  76 public class Body {
  77 private:
  78   static shared uint lastidx;
  79
  80 private:
  81   uint midx;
  82
  83 public:
  84   Vec2 position;
  85   VFloat rotation;
  86
  87   Vec2 velocity;
  88   VFloat angularVelocity;
  89
  90   Vec2 force;
  91   VFloat torque;
  92
  93   Vec2 width;
  94
  95   VFloat friction;
  96   VFloat mass, invMass;
  97   VFloat i, invI;
  98
  99 public:
 100   this () @trusted {
 101     import core.atomic : atomicOp;
 102     midx = atomicOp!"+="(lastidx, 1);
 103
 104     position.set(VFloatNum!(0.0), VFloatNum!(0.0));
 105     rotation = VFloatNum!(0.0);
 106     velocity.set(VFloatNum!(0.0), VFloatNum!(0.0));
 107     angularVelocity = VFloatNum!(0.0);
 108     force.set(VFloatNum!(0.0), VFloatNum!(0.0));
 109     torque = VFloatNum!(0.0);
 110     friction = VFloatNum!(0.2);
 111     width.set(VFloatNum!(1.0), VFloatNum!(1.0));
 112     mass = VFloat.max;
 113     invMass = VFloatNum!(0.0);
 114     i = VFloat.max;
 115     invI = VFloatNum!(0.0);
 116   }
 117
 118   @property uint id () const pure nothrow @safe @nogc { pragma(inline, true); return midx; }
 119
 120   void set() (in auto ref Vec2 w, VFloat m) {
 121     position.set(VFloatNum!(0.0), VFloatNum!(0.0));
 122     rotation = VFloatNum!(0.0);
 123     velocity.set(VFloatNum!(0.0), VFloatNum!(0.0));
 124     angularVelocity = VFloatNum!(0.0);
 125     force.set(VFloatNum!(0.0), VFloatNum!(0.0));
 126     torque = VFloatNum!(0.0);
 127     friction = VFloatNum!(0.2);
 128     width = w;
 129     mass = m;
 130     if (mass < VFloat.max) {
 131       invMass = VFloatNum!(1.0)/mass;
 132       i = mass*(width.x*width.x+width.y*width.y)/VFloatNum!(12.0);
 133       invI = VFloatNum!(1.0)/i;
 134     } else {
 135       invMass = VFloatNum!(0.0);
 136       i = VFloat.max;
 137       invI = VFloatNum!(0.0);
 138     }
 139   }
 140
 141   void addForce() (in auto ref Vec2 f) { pragma(inline, true); force += f; }
 142
 143   bool opEquals() (Body b) pure const nothrow @trusted @nogc {
 144     pragma(inline, true);
 145     return (b !is null ? b.midx == midx : false);
 146   }
 147
 148   int opCmp() (Body b) pure const nothrow @trusted @nogc {
 149     pragma(inline, true);
 150     return (b !is null ? (midx < b.midx ? -1 : midx > b.midx ? 1 : 0) : 1);
 151   }
 152 }
 153
 154
 155 // ////////////////////////////////////////////////////////////////////////// //
 156 // joint
 157 public class Joint {
 158 public:
 159   Mat22 mt;
 160   Vec2 localAnchor1, localAnchor2;
 161   Vec2 r1, r2;
 162   Vec2 bias;
 163   Vec2 accimp = Vec2(0, 0); // accumulated impulse
 164   Body body0;
 165   Body body1;
 166   VFloat biasFactor = VFloatNum!(0.2);
 167   VFloat softness = VFloatNum!(0.0);
 168
 169 public:
 170   void set() (Body b0, Body b1, in auto ref Vec2 anchor) {
 171     body0 = b0;
 172     body1 = b1;
 173
 174     auto rot1 = Mat22(body0.rotation);
 175     auto rot2 = Mat22(body1.rotation);
 176     Mat22 rot1T = rot1.transpose();
 177     Mat22 rot2T = rot2.transpose();
 178
 179     localAnchor1 = rot1T*(anchor-body0.position);
 180     localAnchor2 = rot2T*(anchor-body1.position);
 181
 182     accimp.set(VFloatNum!(0.0), VFloatNum!(0.0));
 183
 184     softness = VFloatNum!(0.0);
 185     biasFactor = VFloatNum!(0.2);
 186   }
 187
 188   void preStep (VFloat invDt) {
 189     // pre-compute anchors, mass matrix, and bias
 190     auto rot1 = Mat22(body0.rotation);
 191     auto rot2 = Mat22(body1.rotation);
 192
 193     r1 = rot1*localAnchor1;
 194     r2 = rot2*localAnchor2;
 195
 196     // deltaV = deltaV0+kmt*impulse
 197     // invM = [(1/m1+1/m2)*eye(2)-skew(r1)*invI1*skew(r1)-skew(r2)*invI2*skew(r2)]
 198     //      = [1/m1+1/m2     0    ]+invI1*[r1.y*r1.y -r1.x*r1.y]+invI2*[r1.y*r1.y -r1.x*r1.y]
 199     //        [    0     1/m1+1/m2]       [-r1.x*r1.y r1.x*r1.x]       [-r1.x*r1.y r1.x*r1.x]
 200     Mat22 k1;
 201     k1.col1.x = body0.invMass+body1.invMass; k1.col2.x = VFloatNum!(0.0);
 202     k1.col1.y = VFloatNum!(0.0);                          k1.col2.y = body0.invMass+body1.invMass;
 203
 204     Mat22 k2;
 205     k2.col1.x =  body0.invI*r1.y*r1.y; k2.col2.x = -body0.invI*r1.x*r1.y;
 206     k2.col1.y = -body0.invI*r1.x*r1.y; k2.col2.y =  body0.invI*r1.x*r1.x;
 207
 208     Mat22 k3;
 209     k3.col1.x =  body1.invI*r2.y*r2.y; k3.col2.x = -body1.invI*r2.x*r2.y;
 210     k3.col1.y = -body1.invI*r2.x*r2.y; k3.col2.y =  body1.invI*r2.x*r2.x;
 211
 212     Mat22 kmt = k1+k2+k3;
 213     kmt.col1.x += softness;
 214     kmt.col2.y += softness;
 215
 216     mt = kmt.invert();
 217
 218     Vec2 p1 = body0.position+r1;
 219     Vec2 p2 = body1.position+r2;
 220     Vec2 dp = p2-p1;
 221
 222     if (World.positionCorrection) {
 223       bias = -biasFactor*invDt*dp;
 224     } else {
 225       bias.set(VFloatNum!(0.0), VFloatNum!(0.0));
 226     }
 227
 228     if (World.warmStarting) {
 229       // apply accumulated impulse
 230       body0.velocity -= body0.invMass*accimp;
 231       body0.angularVelocity -= body0.invI*r1.cross(accimp);
 232
 233       body1.velocity += body1.invMass*accimp;
 234       body1.angularVelocity += body1.invI*r2.cross(accimp);
 235     } else {
 236       accimp.set(VFloatNum!(0.0), VFloatNum!(0.0));
 237     }
 238   }
 239
 240   void applyImpulse () {
 241     Vec2 dv = body1.velocity+body1.angularVelocity.fcross(r2)-body0.velocity-body0.angularVelocity.fcross(r1);
 242     Vec2 impulse = mt*(bias-dv-softness*accimp);
 243
 244     body0.velocity -= body0.invMass*impulse;
 245     body0.angularVelocity -= body0.invI*r1.cross(impulse);
 246
 247     body1.velocity += body1.invMass*impulse;
 248     body1.angularVelocity += body1.invI*r2.cross(impulse);
 249
 250     accimp += impulse;
 251   }
 252 }
 253
 254
 255 // ////////////////////////////////////////////////////////////////////////// //
 256 // arbiter
 257 private VFloat clamp() (VFloat a, VFloat low, VFloat high) { pragma(inline, true); import std.algorithm : min, max; return max(low, min(a, high)); }
 258
 259
 260 // ////////////////////////////////////////////////////////////////////////// //
 261 private union FeaturePair {
 262   static struct Edges {
 263     byte inEdge1;
 264     byte outEdge1;
 265     byte inEdge2;
 266     byte outEdge2;
 267   }
 268   Edges e;
 269   int value;
 270 }
 271
 272
 273 // ////////////////////////////////////////////////////////////////////////// //
 274 private struct Contact {
 275 public:
 276   Vec2 position;
 277   Vec2 normal;
 278   Vec2 r1, r2;
 279   VFloat separation = VFloatNum!(0.0);
 280   VFloat accimpN = VFloatNum!(0.0); // accumulated normal impulse
 281   VFloat accimpT = VFloatNum!(0.0); // accumulated tangent impulse
 282   VFloat accimpNb = VFloatNum!(0.0);  // accumulated normal impulse for position bias
 283   VFloat massNormal, massTangent;
 284   VFloat bias = VFloatNum!(0.0);
 285   FeaturePair feature;
 286 }
 287
 288
 289 // ////////////////////////////////////////////////////////////////////////// //
 290 private class Arbiter {
 291 public:
 292   enum MAX_POINTS = 2;
 293
 294 public:
 295   Contact[MAX_POINTS] contacts;
 296   int numContacts;
 297
 298   Body body0;
 299   Body body1;
 300
 301   // combined friction
 302   VFloat friction;
 303
 304 public:
 305   this () {}
 306   this (Body b0, Body b1) { setup(b0, b1); }
 307
 308   void setup (Body b0, Body b1) {
 309     import core.stdc.math : sqrtf;
 310     if (b0 < b1) {
 311       body0 = b0;
 312       body1 = b1;
 313     } else {
 314       body0 = b1;
 315       body1 = b0;
 316     }
 317     numContacts = collide(contacts.ptr, body0, body1);
 318     friction = sqrtf(body0.friction*body1.friction);
 319     if (b2dlDrawContactsCB !is null) {
 320       foreach (const ref ctx; contacts[0..numContacts]) b2dlDrawContactsCB(ctx.position.x, ctx.position.y);
 321     }
 322   }
 323
 324   void update (Contact* newContacts, int numNewContacts) {
 325     Contact[MAX_POINTS] mergedContacts;
 326     assert(numNewContacts >= 0 && numNewContacts <= mergedContacts.length);
 327     foreach (immutable idx; 0..numNewContacts) {
 328       Contact* cNew = newContacts+idx;
 329       int k = -1;
 330       foreach (immutable j; 0..numContacts) {
 331         Contact* cOld = contacts.ptr+j;
 332         if (cNew.feature.value == cOld.feature.value) { k = j; break; }
 333       }
 334       if (k > -1) {
 335         Contact* c = mergedContacts.ptr+idx;
 336         Contact* cOld = contacts.ptr+k;
 337         *c = *cNew;
 338         if (World.warmStarting) {
 339           c.accimpN = cOld.accimpN;
 340           c.accimpT = cOld.accimpT;
 341           c.accimpNb = cOld.accimpNb;
 342         } else {
 343           c.accimpN = VFloatNum!(0.0);
 344           c.accimpT = VFloatNum!(0.0);
 345           c.accimpNb = VFloatNum!(0.0);
 346         }
 347       } else {
 348         mergedContacts[idx] = newContacts[idx];
 349       }
 350     }
 351     contacts[0..numNewContacts] = mergedContacts[0..numNewContacts];
 352     numContacts = numNewContacts;
 353   }
 354
 355   void preStep (VFloat invDt) {
 356     import std.algorithm : min;
 357     enum k_allowedPenetration = VFloatNum!(0.01);
 358     VFloat k_biasFactor = (World.positionCorrection ? VFloatNum!(0.2) : VFloatNum!(0.0));
 359     foreach (immutable idx; 0..numContacts) {
 360       Contact *c = contacts.ptr+idx;
 361       Vec2 r1 = c.position-body0.position;
 362       Vec2 r2 = c.position-body1.position;
 363
 364       // precompute normal mass, tangent mass, and bias
 365       VFloat rn1 = r1*c.normal; //Dot(r1, c.normal);
 366       VFloat rn2 = r2*c.normal; //Dot(r2, c.normal);
 367       VFloat kNormal = body0.invMass+body1.invMass;
 368       //kNormal += body0.invI*(Dot(r1, r1)-rn1*rn1)+body1.invI*(Dot(r2, r2)-rn2*rn2);
 369       kNormal += body0.invI*((r1*r1)-rn1*rn1)+body1.invI*((r2*r2)-rn2*rn2);
 370       c.massNormal = VFloatNum!(1.0)/kNormal;
 371
 372       //Vec2 tangent = c.normal.cross(VFloatNum!(1.0));
 373       Vec2 tangent = Vec2(VFloatNum!(1.0)*c.normal.y, -VFloatNum!(1.0)*c.normal.x);
 374       VFloat rt1 = r1*tangent; //Dot(r1, tangent);
 375       VFloat rt2 = r2*tangent; //Dot(r2, tangent);
 376       VFloat kTangent = body0.invMass+body1.invMass;
 377       //kTangent += body0.invI*(Dot(r1, r1)-rt1*rt1)+body1.invI*(Dot(r2, r2)-rt2*rt2);
 378       kTangent += body0.invI*((r1*r1)-rt1*rt1)+body1.invI*((r2*r2)-rt2*rt2);
 379       c.massTangent = VFloatNum!(1.0)/kTangent;
 380
 381       c.bias = -k_biasFactor*invDt*min(VFloatNum!(0.0), c.separation+k_allowedPenetration);
 382
 383       if (World.accumulateImpulses) {
 384         // apply normal + friction impulse
 385         Vec2 accimp = c.accimpN*c.normal+c.accimpT*tangent;
 386
 387         body0.velocity -= body0.invMass*accimp;
 388         body0.angularVelocity -= body0.invI*r1.cross(accimp);
 389
 390         body1.velocity += body1.invMass*accimp;
 391         body1.angularVelocity += body1.invI*r2.cross(accimp);
 392       }
 393     }
 394   }
 395
 396   void applyImpulse () {
 397     import std.algorithm : max;
 398     Body b0 = body0;
 399     Body b1 = body1;
 400     foreach (immutable idx; 0..numContacts) {
 401       Contact *c = contacts.ptr+idx;
 402       c.r1 = c.position-b0.position;
 403       c.r2 = c.position-b1.position;
 404
 405       // relative velocity at contact
 406       Vec2 dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 407
 408       // compute normal impulse
 409       VFloat vn = dv*c.normal; //Dot(dv, c.normal);
 410
 411       VFloat dPn = c.massNormal*(-vn+c.bias);
 412
 413       if (World.accumulateImpulses) {
 414         // clamp the accumulated impulse
 415         VFloat accimpN0 = c.accimpN;
 416         c.accimpN = max(accimpN0+dPn, VFloatNum!(0.0));
 417         dPn = c.accimpN-accimpN0;
 418       } else {
 419         dPn = max(dPn, VFloatNum!(0.0));
 420       }
 421
 422       // apply contact impulse
 423       Vec2 accimpN = dPn*c.normal;
 424
 425       b0.velocity -= b0.invMass*accimpN;
 426       b0.angularVelocity -= b0.invI*c.r1.cross(accimpN);
 427
 428       b1.velocity += b1.invMass*accimpN;
 429       b1.angularVelocity += b1.invI*c.r2.cross(accimpN);
 430
 431       // relative velocity at contact
 432       dv = b1.velocity+b1.angularVelocity.fcross(c.r2)-b0.velocity-b0.angularVelocity.fcross(c.r1);
 433
 434       //Vec2 tangent = c.normal.cross(VFloatNum!(1.0));
 435       Vec2 tangent = Vec2(VFloatNum!(1.0)*c.normal.y, -VFloatNum!(1.0)*c.normal.x);
 436       VFloat vt = dv*tangent; //Dot(dv, tangent);
 437       VFloat dPt = c.massTangent*(-vt);
 438
 439       if (World.accumulateImpulses) {
 440         // compute friction impulse
 441         VFloat maxPt = friction*c.accimpN;
 442         // clamp friction
 443         VFloat oldTangentImpulse = c.accimpT;
 444         c.accimpT = clamp(oldTangentImpulse+dPt, -maxPt, maxPt);
 445         dPt = c.accimpT-oldTangentImpulse;
 446       } else {
 447         VFloat maxPt = friction*dPn;
 448         dPt = clamp(dPt, -maxPt, maxPt);
 449       }
 450
 451       // apply contact impulse
 452       Vec2 accimpT = dPt*tangent;
 453
 454       b0.velocity -= b0.invMass*accimpT;
 455       b0.angularVelocity -= b0.invI*c.r1.cross(accimpT);
 456
 457       b1.velocity += b1.invMass*accimpT;
 458       b1.angularVelocity += b1.invI*c.r2.cross(accimpT);
 459     }
 460   }
 461 }
 462
 463
 464 // ////////////////////////////////////////////////////////////////////////// //
 465 // collide
 466 private VFloat sign() (VFloat x) { pragma(inline, true); return (x < VFloatNum!(0.0) ? -VFloatNum!(1.0) : VFloatNum!(1.0)); }
 467
 468 private void swap(T) (ref T a, ref T b) {
 469   pragma(inline, true);
 470   T tmp = a;
 471   a = b;
 472   b = tmp;
 473 }
 474
 475 private void flip (ref FeaturePair fp) {
 476   pragma(inline, true);
 477   swap(fp.e.inEdge1, fp.e.inEdge2);
 478   swap(fp.e.outEdge1, fp.e.outEdge2);
 479 }
 480
 481
 482 // Box vertex and edge numbering:
 483 //
 484 //        ^ y
 485 //        |
 486 //        e0
 487 //   v1 ------ v0
 488 //    |        |
 489 // e1 |        | e3  --> x
 490 //    |        |
 491 //   v2 ------ v3
 492 //        e2
 493
 494
 495 // ////////////////////////////////////////////////////////////////////////// //
 496 private enum Axis {
 497   FaceAX,
 498   FaceAY,
 499   FaceBX,
 500   FaceBY,
 501 }
 502
 503
 504 private enum EdgeNumbers {
 505   None = 0,
 506   Edge0,
 507   Edge1,
 508   Edge2,
 509   Edge3,
 510 }
 511
 512
 513 // ////////////////////////////////////////////////////////////////////////// //
 514 private struct ClipVertex {
 515   Vec2 v;
 516   FeaturePair fp;
 517 }
 518
 519
 520 // ////////////////////////////////////////////////////////////////////////// //
 521 private int clipSegmentToLine() (ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, in auto ref Vec2 normal, VFloat offset, byte clipEdge) {
 522   // start with no output points
 523   int numOut = 0;
 524   // calculate the distance of end points to the line
 525   VFloat distance0 = /*Dot*/(normal*vIn[0].v)-offset;
 526   VFloat distance1 = /*Dot*/(normal*vIn[1].v)-offset;
 527   // if the points are behind the plane
 528   if (distance0 <= VFloatNum!(0.0)) vOut[numOut++] = vIn[0];
 529   if (distance1 <= VFloatNum!(0.0)) vOut[numOut++] = vIn[1];
 530   // if the points are on different sides of the plane
 531   if (distance0*distance1 < VFloatNum!(0.0)) {
 532     // find intersection point of edge and plane
 533     VFloat interp = distance0/(distance0-distance1);
 534     vOut[numOut].v = vIn[0].v+interp*(vIn[1].v-vIn[0].v);
 535     if (distance0 > VFloatNum!(0.0)) {
 536       vOut[numOut].fp = vIn[0].fp;
 537       vOut[numOut].fp.e.inEdge1 = clipEdge;
 538       vOut[numOut].fp.e.inEdge2 = EdgeNumbers.None;
 539     } else {
 540       vOut[numOut].fp = vIn[1].fp;
 541       vOut[numOut].fp.e.outEdge1 = clipEdge;
 542       vOut[numOut].fp.e.outEdge2 = EdgeNumbers.None;
 543     }
 544     ++numOut;
 545   }
 546   return numOut;
 547 }
 548
 549
 550 // ////////////////////////////////////////////////////////////////////////// //
 551 private void computeIncidentEdge() (ClipVertex[/*2*/] c, in auto ref Vec2 h, in auto ref Vec2 pos, in auto ref Mat22 rot, in auto ref Vec2 normal) {
 552   // the normal is from the reference box; convert it to the incident boxe's frame and flip sign
 553   Mat22 rotT = rot.transpose();
 554   Vec2 n = -(rotT*normal);
 555   Vec2 nAbs = n.abs;
 556   if (nAbs.x > nAbs.y) {
 557     if (sign(n.x) > VFloatNum!(0.0)) {
 558       c[0].v.set(h.x, -h.y);
 559       c[0].fp.e.inEdge2 = EdgeNumbers.Edge2;
 560       c[0].fp.e.outEdge2 = EdgeNumbers.Edge3;
 561
 562       c[1].v.set(h.x, h.y);
 563       c[1].fp.e.inEdge2 = EdgeNumbers.Edge3;
 564       c[1].fp.e.outEdge2 = EdgeNumbers.Edge0;
 565     } else {
 566       c[0].v.set(-h.x, h.y);
 567       c[0].fp.e.inEdge2 = EdgeNumbers.Edge0;
 568       c[0].fp.e.outEdge2 = EdgeNumbers.Edge1;
 569
 570       c[1].v.set(-h.x, -h.y);
 571       c[1].fp.e.inEdge2 = EdgeNumbers.Edge1;
 572       c[1].fp.e.outEdge2 = EdgeNumbers.Edge2;
 573     }
 574   } else {
 575     if (sign(n.y) > VFloatNum!(0.0)) {
 576       c[0].v.set(h.x, h.y);
 577       c[0].fp.e.inEdge2 = EdgeNumbers.Edge3;
 578       c[0].fp.e.outEdge2 = EdgeNumbers.Edge0;
 579
 580       c[1].v.set(-h.x, h.y);
 581       c[1].fp.e.inEdge2 = EdgeNumbers.Edge0;
 582       c[1].fp.e.outEdge2 = EdgeNumbers.Edge1;
 583     } else {
 584       c[0].v.set(-h.x, -h.y);
 585       c[0].fp.e.inEdge2 = EdgeNumbers.Edge1;
 586       c[0].fp.e.outEdge2 = EdgeNumbers.Edge2;
 587
 588       c[1].v.set(h.x, -h.y);
 589       c[1].fp.e.inEdge2 = EdgeNumbers.Edge2;
 590       c[1].fp.e.outEdge2 = EdgeNumbers.Edge3;
 591     }
 592   }
 593
 594   c[0].v = pos+rot*c[0].v;
 595   c[1].v = pos+rot*c[1].v;
 596 }
 597
 598
 599 // ////////////////////////////////////////////////////////////////////////// //
 600 // the normal points from A to B
 601 // return number of contact points
 602 private int collide (Contact* contacts, Body bodyA, Body bodyB) {
 603   // setup
 604   Vec2 hA = VFloatNum!(0.5)*bodyA.width;
 605   Vec2 hB = VFloatNum!(0.5)*bodyB.width;
 606
 607   Vec2 posA = bodyA.position;
 608   Vec2 posB = bodyB.position;
 609
 610   auto rotA = Mat22(bodyA.rotation);
 611   auto rotB = Mat22(bodyB.rotation);
 612
 613   Mat22 rotAT = rotA.transpose();
 614   Mat22 rotBT = rotB.transpose();
 615
 616   //k8:Vec2 a1 = rotA.col1, a2 = rotA.col2;
 617   //k8:Vec2 b0 = rotB.col1, b1 = rotB.col2;
 618
 619   Vec2 dp = posB-posA;
 620   Vec2 dA = rotAT*dp;
 621   Vec2 dB = rotBT*dp;
 622
 623   Mat22 cmt = rotAT*rotB;
 624   Mat22 absC = cmt.abs;
 625   Mat22 absCT = absC.transpose();
 626
 627   // Box A faces
 628   Vec2 faceA = dA.abs-hA-absC*hB;
 629   if (faceA.x > VFloatNum!(0.0) || faceA.y > VFloatNum!(0.0)) return 0;
 630
 631   // Box B faces
 632   Vec2 faceB = dB.abs-absCT*hA-hB;
 633   if (faceB.x > VFloatNum!(0.0) || faceB.y > VFloatNum!(0.0)) return 0;
 634
 635   // Find best axis
 636   Axis axis;
 637   VFloat separation;
 638   Vec2 normal;
 639
 640   // Box A faces
 641   axis = Axis.FaceAX;
 642   separation = faceA.x;
 643   normal = (dA.x > VFloatNum!(0.0) ? rotA.col1 : -rotA.col1);
 644
 645   const VFloat relativeTol = VFloatNum!(0.95);
 646   const VFloat absoluteTol = VFloatNum!(0.01);
 647
 648   if (faceA.y > relativeTol*separation+absoluteTol*hA.y) {
 649     axis = Axis.FaceAY;
 650     separation = faceA.y;
 651     normal = (dA.y > VFloatNum!(0.0) ? rotA.col2 : -rotA.col2);
 652   }
 653
 654   // Box B faces
 655   if (faceB.x > relativeTol*separation+absoluteTol*hB.x) {
 656     axis = Axis.FaceBX;
 657     separation = faceB.x;
 658     normal = (dB.x > VFloatNum!(0.0) ? rotB.col1 : -rotB.col1);
 659   }
 660
 661   if (faceB.y > relativeTol*separation+absoluteTol*hB.y) {
 662     axis = Axis.FaceBY;
 663     separation = faceB.y;
 664     normal = (dB.y > VFloatNum!(0.0) ? rotB.col2 : -rotB.col2);
 665   }
 666
 667   // Setup clipping plane data based on the separating axis
 668   Vec2 frontNormal, sideNormal;
 669   ClipVertex[2] incidentEdge;
 670   VFloat front, negSide, posSide;
 671   byte negEdge, posEdge;
 672
 673   // Compute the clipping lines and the line segment to be clipped.
 674   switch (axis) {
 675     case Axis.FaceAX:
 676       frontNormal = normal;
 677       front = /*Dot*/(posA*frontNormal)+hA.x;
 678       sideNormal = rotA.col2;
 679       VFloat side = /*Dot*/(posA*sideNormal);
 680       negSide = -side+hA.y;
 681       posSide =  side+hA.y;
 682       negEdge = EdgeNumbers.Edge2;
 683       posEdge = EdgeNumbers.Edge0;
 684       computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
 685       break;
 686     case Axis.FaceAY:
 687       frontNormal = normal;
 688       front = /*Dot*/(posA*frontNormal)+hA.y;
 689       sideNormal = rotA.col1;
 690       VFloat side = /*Dot*/(posA*sideNormal);
 691       negSide = -side+hA.x;
 692       posSide =  side+hA.x;
 693       negEdge = EdgeNumbers.Edge1;
 694       posEdge = EdgeNumbers.Edge3;
 695       computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
 696       break;
 697     case Axis.FaceBX:
 698       frontNormal = -normal;
 699       front = /*Dot*/(posB*frontNormal)+hB.x;
 700       sideNormal = rotB.col2;
 701       VFloat side = /*Dot*/(posB*sideNormal);
 702       negSide = -side+hB.y;
 703       posSide =  side+hB.y;
 704       negEdge = EdgeNumbers.Edge2;
 705       posEdge = EdgeNumbers.Edge0;
 706       computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
 707       break;
 708     case Axis.FaceBY:
 709       frontNormal = -normal;
 710       front = /*Dot*/(posB*frontNormal)+hB.y;
 711       sideNormal = rotB.col1;
 712       VFloat side = /*Dot*/(posB*sideNormal);
 713       negSide = -side+hB.x;
 714       posSide =  side+hB.x;
 715       negEdge = EdgeNumbers.Edge1;
 716       posEdge = EdgeNumbers.Edge3;
 717       computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
 718       break;
 719     default: assert(0);
 720   }
 721
 722   // clip other face with 5 box planes (1 face plane, 4 edge planes)
 723   ClipVertex[2] clipPoints1;
 724   ClipVertex[2] clipPoints2;
 725   int np;
 726
 727   // clip to box side 1
 728   np = clipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge);
 729   if (np < 2) return 0;
 730
 731   // clip to negative box side 1
 732   np = clipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, posSide, posEdge);
 733   if (np < 2) return 0;
 734
 735   // Now clipPoints2 contains the clipping points.
 736   // Due to roundoff, it is possible that clipping removes all points.
 737   int numContacts = 0;
 738   foreach (immutable idx; 0..2) {
 739     separation = /*Dot*/(frontNormal*clipPoints2[idx].v)-front;
 740     if (separation <= 0) {
 741       contacts[numContacts].separation = separation;
 742       contacts[numContacts].normal = normal;
 743       // slide contact point onto reference face (easy to cull)
 744       contacts[numContacts].position = clipPoints2[idx].v-separation*frontNormal;
 745       contacts[numContacts].feature = clipPoints2[idx].fp;
 746       if (axis == Axis.FaceBX || axis == Axis.FaceBY) flip(contacts[numContacts].feature);
 747       ++numContacts;
 748     }
 749   }
 750
 751   return numContacts;
 752 }
 753
 754
 755 // ////////////////////////////////////////////////////////////////////////// //
 756 // world
 757 public class World {
 758 private:
 759   static struct ArbiterKey {
 760     // pointers, actually
 761     Body body0;
 762     Body body1;
 763     this (Body b0, Body b1) { if (b0 < b1) { body0 = b0; body1 = b1; } else { body0 = b1; body1 = b0; } }
 764   }
 765
 766 public:
 767   Body[] bodies;
 768   Joint[] joints;
 769   Arbiter[ArbiterKey] arbiters;
 770   Vec2 gravity;
 771   int iterations;
 772   Arbiter xarb; // temporary
 773
 774   // the following are world options
 775   static bool accumulateImpulses = true;
 776   static bool warmStarting = true;
 777   static bool positionCorrection = true;
 778
 779 public:
 780   this() (in auto ref Vec2 agravity, int aiterations) {
 781     gravity = agravity;
 782     iterations = aiterations;
 783     xarb = new Arbiter();
 784   }
 785
 786   void add (Body bbody) {
 787     if (bbody !is null) bodies ~= bbody;
 788   }
 789
 790   void add (Joint joint) {
 791     if (joint !is null) joints ~= joint;
 792   }
 793
 794   void opOpAssign(string op:"~", T) (T v) if (is(T : Body) || is(T : Joint)) {
 795     add(v);
 796   }
 797
 798   void clear () {
 799     bodies = null;
 800     joints = null;
 801     arbiters.clear();
 802   }
 803
 804   void step (VFloat dt) {
 805     VFloat invDt = (dt > VFloatNum!(0.0) ? VFloatNum!(1.0)/dt : VFloatNum!(0.0));
 806     // determine overlapping bodies and update contact points
 807     broadPhase();
 808     // integrate forces
 809     foreach (Body b; bodies) {
 810       if (b.invMass == VFloatNum!(0.0)) continue;
 811       b.velocity += (gravity+b.force*b.invMass)*dt;
 812       b.angularVelocity += dt*b.invI*b.torque;
 813     }
 814     // perform pre-steps
 815     foreach (Arbiter arb; arbiters.byValue) arb.preStep(invDt);
 816     for (int idx = 0; idx < joints.length; ++idx) joints[idx].preStep(invDt);
 817     // perform iterations
 818     foreach (immutable itnum; 0..iterations) {
 819       foreach (Arbiter arb; arbiters.byValue) arb.applyImpulse();
 820       foreach (Joint j; joints) j.applyImpulse();
 821     }
 822     // integrate velocities
 823     foreach (Body b; bodies) {
 824       b.position += b.velocity*dt;
 825       b.rotation += b.angularVelocity*dt;
 826
 827       b.force.set(VFloatNum!(0.0), VFloatNum!(0.0));
 828       b.torque = VFloatNum!(0.0);
 829     }
 830   }
 831
 832   void broadPhase () {
 833     // O(n^2) broad-phase
 834     foreach (immutable idx, Body bi; bodies) {
 835       foreach (Body bj; bodies[idx+1..$]) {
 836         if (bi.invMass == VFloatNum!(0.0) && bj.invMass == VFloatNum!(0.0)) continue;
 837         if (auto arb = ArbiterKey(bi, bj) in arbiters) {
 838           xarb.setup(bi, bj);
 839           arb.update(xarb.contacts.ptr, xarb.numContacts);
 840         } else {
 841           arbiters[ArbiterKey(bi, bj)] = new Arbiter(bi, bj);
 842         }
 843       }
 844     }
 845   }
 846 }