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