b2dlite/arbiter.d

   1 /*
   2  * Copyright (c) 2006-2009 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.arbiter;
  12
  13 import iv.vmath;
  14 import b2dlite.mathutils;
  15
  16 package(b2dlite):
  17 VFloat clamp() (VFloat a, VFloat low, VFloat high) { pragma(inline, true); import std.algorithm : min, max; return max(low, min(a, high)); }
  18
  19
  20 // ////////////////////////////////////////////////////////////////////////// //
  21 union FeaturePair {
  22   static struct Edges {
  23     char inEdge1;
  24     char outEdge1;
  25     char inEdge2;
  26     char outEdge2;
  27   }
  28   Edges e;
  29   int value;
  30 }
  31
  32
  33 // ////////////////////////////////////////////////////////////////////////// //
  34 struct Contact {
  35 public:
  36   Vec2 position;
  37   Vec2 normal;
  38   Vec2 r1, r2;
  39   VFloat separation = VFloatNum!(0.0);
  40   VFloat Pn = VFloatNum!(0.0); // accumulated normal impulse
  41   VFloat Pt = VFloatNum!(0.0); // accumulated tangent impulse
  42   VFloat Pnb = VFloatNum!(0.0);  // accumulated normal impulse for position bias
  43   VFloat massNormal, massTangent;
  44   VFloat bias = VFloatNum!(0.0);
  45   FeaturePair feature;
  46 }
  47
  48
  49 // ////////////////////////////////////////////////////////////////////////// //
  50 class Arbiter {
  51   private import b2dlite.bbody : Body;
  52
  53 public:
  54   enum MAX_POINTS = 2;
  55
  56 public:
  57   Contact[MAX_POINTS] contacts;
  58   int numContacts;
  59
  60   Body body1;
  61   Body body2;
  62
  63   // combined friction
  64   VFloat friction;
  65
  66 public:
  67   this () {}
  68   this (Body b1, Body b2) { setup(b1, b2); }
  69
  70   void setup (Body b1, Body b2) {
  71     import std.math : sqrt;
  72     import b2dlite.collide : collide;
  73
  74     if (b1 < b2) {
  75       body1 = b1;
  76       body2 = b2;
  77     } else {
  78       body1 = b2;
  79       body2 = b1;
  80     }
  81     numContacts = collide(contacts.ptr, body1, body2);
  82     friction = sqrt(body1.friction*body2.friction);
  83
  84     /*
  85     import iv.glbinds;
  86     glPointSize(VFloatNum!(4.0));
  87     glColor3f(VFloatNum!(1.0), VFloatNum!(0.0), VFloatNum!(0.0));
  88     glBegin(GL_POINTS);
  89     for (int i = 0; i < numContacts; ++i) glVertex2f(contacts[i].position.x, contacts[i].position.y);
  90     glEnd();
  91     glPointSize(VFloatNum!(1.0));
  92     */
  93   }
  94
  95   void update (Contact* newContacts, int numNewContacts) {
  96     import b2dlite.world : World;
  97
  98     Contact[2] mergedContacts;
  99     for (int i = 0; i < numNewContacts; ++i) {
 100       Contact *cNew = newContacts+i;
 101       int k = -1;
 102       for (int j = 0; j < numContacts; ++j) {
 103         Contact* cOld = contacts.ptr+j;
 104         if (cNew.feature.value == cOld.feature.value) { k = j; break; }
 105       }
 106       if (k > -1) {
 107         Contact* c = mergedContacts.ptr+i;
 108         Contact* cOld = contacts.ptr+k;
 109         *c = *cNew;
 110         if (World.warmStarting) {
 111           c.Pn = cOld.Pn;
 112           c.Pt = cOld.Pt;
 113           c.Pnb = cOld.Pnb;
 114         } else {
 115           c.Pn = VFloatNum!(0.0);
 116           c.Pt = VFloatNum!(0.0);
 117           c.Pnb = VFloatNum!(0.0);
 118         }
 119       } else {
 120         mergedContacts[i] = newContacts[i];
 121       }
 122     }
 123     for (int i = 0; i < numNewContacts; ++i) contacts[i] = mergedContacts[i];
 124     numContacts = numNewContacts;
 125   }
 126
 127   void preStep (VFloat inv_dt) {
 128     import b2dlite.world : World;
 129     import std.algorithm : min;
 130
 131     enum k_allowedPenetration = VFloatNum!(0.01);
 132     VFloat k_biasFactor = (World.positionCorrection ? VFloatNum!(0.2) : VFloatNum!(0.0));
 133     for (int i = 0; i < numContacts; ++i) {
 134       Contact *c = contacts.ptr+i;
 135       Vec2 r1 = c.position-body1.position;
 136       Vec2 r2 = c.position-body2.position;
 137
 138       // precompute normal mass, tangent mass, and bias
 139       VFloat rn1 = r1*c.normal; //Dot(r1, c.normal);
 140       VFloat rn2 = r2*c.normal; //Dot(r2, c.normal);
 141       VFloat kNormal = body1.invMass+body2.invMass;
 142       //kNormal += body1.invI*(Dot(r1, r1)-rn1*rn1)+body2.invI*(Dot(r2, r2)-rn2*rn2);
 143       kNormal += body1.invI*((r1*r1)-rn1*rn1)+body2.invI*((r2*r2)-rn2*rn2);
 144       c.massNormal = VFloatNum!(1.0)/kNormal;
 145
 146       //Vec2 tangent = c.normal.cross(VFloatNum!(1.0));
 147       Vec2 tangent = Vec2(VFloatNum!(1.0)*c.normal.y, -VFloatNum!(1.0)*c.normal.x);
 148       VFloat rt1 = r1*tangent; //Dot(r1, tangent);
 149       VFloat rt2 = r2*tangent; //Dot(r2, tangent);
 150       VFloat kTangent = body1.invMass+body2.invMass;
 151       //kTangent += body1.invI*(Dot(r1, r1)-rt1*rt1)+body2.invI*(Dot(r2, r2)-rt2*rt2);
 152       kTangent += body1.invI*((r1*r1)-rt1*rt1)+body2.invI*((r2*r2)-rt2*rt2);
 153       c.massTangent = VFloatNum!(1.0)/kTangent;
 154
 155       c.bias = -k_biasFactor*inv_dt*min(VFloatNum!(0.0), c.separation+k_allowedPenetration);
 156
 157       if (World.accumulateImpulses) {
 158         // apply normal + friction impulse
 159         Vec2 P = c.Pn*c.normal+c.Pt*tangent;
 160
 161         body1.velocity -= body1.invMass*P;
 162         body1.angularVelocity -= body1.invI*r1.cross(P);
 163
 164         body2.velocity += body2.invMass*P;
 165         body2.angularVelocity += body2.invI*r2.cross(P);
 166       }
 167     }
 168   }
 169
 170   void applyImpulse () {
 171     import b2dlite.world : World;
 172     import std.algorithm : max;
 173
 174     Body b1 = body1;
 175     Body b2 = body2;
 176
 177     for (int i = 0; i < numContacts; ++i) {
 178       Contact *c = contacts.ptr+i;
 179       c.r1 = c.position-b1.position;
 180       c.r2 = c.position-b2.position;
 181
 182       // relative velocity at contact
 183       Vec2 dv = b2.velocity+b2.angularVelocity.fcross(c.r2)-b1.velocity-b1.angularVelocity.fcross(c.r1);
 184
 185       // compute normal impulse
 186       VFloat vn = dv*c.normal; //Dot(dv, c.normal);
 187
 188       VFloat dPn = c.massNormal*(-vn+c.bias);
 189
 190       if (World.accumulateImpulses) {
 191         // clamp the accumulated impulse
 192         VFloat Pn0 = c.Pn;
 193         c.Pn = max(Pn0+dPn, VFloatNum!(0.0));
 194         dPn = c.Pn-Pn0;
 195       } else {
 196         dPn = max(dPn, VFloatNum!(0.0));
 197       }
 198
 199       // apply contact impulse
 200       Vec2 Pn = dPn*c.normal;
 201
 202       b1.velocity -= b1.invMass*Pn;
 203       b1.angularVelocity -= b1.invI*c.r1.cross(Pn);
 204
 205       b2.velocity += b2.invMass*Pn;
 206       b2.angularVelocity += b2.invI*c.r2.cross(Pn);
 207
 208       // relative velocity at contact
 209       dv = b2.velocity+b2.angularVelocity.fcross(c.r2)-b1.velocity-b1.angularVelocity.fcross(c.r1);
 210
 211       //Vec2 tangent = c.normal.cross(VFloatNum!(1.0));
 212       Vec2 tangent = Vec2(VFloatNum!(1.0)*c.normal.y, -VFloatNum!(1.0)*c.normal.x);
 213       VFloat vt = dv*tangent; //Dot(dv, tangent);
 214       VFloat dPt = c.massTangent*(-vt);
 215
 216       if (World.accumulateImpulses) {
 217         // compute friction impulse
 218         VFloat maxPt = friction*c.Pn;
 219         // clamp friction
 220         VFloat oldTangentImpulse = c.Pt;
 221         c.Pt = clamp(oldTangentImpulse+dPt, -maxPt, maxPt);
 222         dPt = c.Pt-oldTangentImpulse;
 223       } else {
 224         VFloat maxPt = friction*dPn;
 225         dPt = clamp(dPt, -maxPt, maxPt);
 226       }
 227
 228       // apply contact impulse
 229       Vec2 Pt = dPt*tangent;
 230
 231       b1.velocity -= b1.invMass*Pt;
 232       b1.angularVelocity -= b1.invI*c.r1.cross(Pt);
 233
 234       b2.velocity += b2.invMass*Pt;
 235       b2.angularVelocity += b2.invI*c.r2.cross(Pt);
 236     }
 237   }
 238 }