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