b2dlite/joint.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.joint;
  12
  13 import b2dlite.mathutils;
  14
  15 class Joint {
  16 private import b2dlite.bbody : Body;
  17 public:
  18   Mat22 M;
  19   Vec2 localAnchor1, localAnchor2;
  20   Vec2 r1, r2;
  21   Vec2 bias;
  22   Vec2 P;   // accumulated impulse
  23   Body body1;
  24   Body body2;
  25   float biasFactor = 0.2f;
  26   float softness = 0.0f;
  27
  28 public:
  29   //Joint () : P(0.0f, 0.0f), body1(0), body2(0), biasFactor(0.2f), softness(0.0f) {}
  30
  31   void Set() (Body body1, Body body2, in auto ref Vec2 anchor) {
  32     body1 = b1;
  33     body2 = b2;
  34
  35     Mat22 Rot1(body1.rotation);
  36     Mat22 Rot2(body2.rotation);
  37     Mat22 Rot1T = Rot1.Transpose();
  38     Mat22 Rot2T = Rot2.Transpose();
  39
  40     localAnchor1 = Rot1T*(anchor-body1.position);
  41     localAnchor2 = Rot2T*(anchor-body2.position);
  42
  43     P.Set(0.0f, 0.0f);
  44
  45     softness = 0.0f;
  46     biasFactor = 0.2f;
  47   }
  48
  49   void PreStep (float inv_dt) {
  50     import b2dlite.world : World;
  51
  52     // pre-compute anchors, mass matrix, and bias
  53     auto Rot1 = Mat22 (body1.rotation);
  54     auto Rot2 = Mat22 (body2.rotation);
  55
  56     r1 = Rot1*localAnchor1;
  57     r2 = Rot2*localAnchor2;
  58
  59     // deltaV = deltaV0 + K * impulse
  60     // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
  61     //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
  62     //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
  63     Mat22 K1;
  64     K1.col1.x = body1.invMass+body2.invMass; K1.col2.x = 0.0f;
  65     K1.col1.y = 0.0f;                          K1.col2.y = body1.invMass+body2.invMass;
  66
  67     Mat22 K2;
  68     K2.col1.x =  body1.invI*r1.y*r1.y; K2.col2.x = -body1.invI*r1.x*r1.y;
  69     K2.col1.y = -body1.invI*r1.x*r1.y; K2.col2.y =  body1.invI*r1.x*r1.x;
  70
  71     Mat22 K3;
  72     K3.col1.x =  body2.invI*r2.y*r2.y; K3.col2.x = -body2.invI*r2.x*r2.y;
  73     K3.col1.y = -body2.invI*r2.x*r2.y; K3.col2.y =  body2.invI*r2.x*r2.x;
  74
  75     Mat22 K = K1+K2+K3;
  76     K.col1.x += softness;
  77     K.col2.y += softness;
  78
  79     M = K.Invert();
  80
  81     Vec2 p1 = body1.position+r1;
  82     Vec2 p2 = body2.position+r2;
  83     Vec2 dp = p2-p1;
  84
  85     if (World.positionCorrection) {
  86       bias = -biasFactor*inv_dt*dp;
  87     } else {
  88       bias.Set(0.0f, 0.0f);
  89     }
  90
  91     if (World.warmStarting) {
  92       // apply accumulated impulse
  93       body1.velocity -= body1.invMass*P;
  94       body1.angularVelocity -= body1.invI*Cross(r1, P);
  95
  96       body2.velocity += body2.invMass*P;
  97       body2.angularVelocity += body2.invI*Cross(r2, P);
  98     } else {
  99       P.Set(0.0f, 0.0f);
 100     }
 101   }
 102
 103   void ApplyImpulse () {
 104     Vec2 dv = body2.velocity+Cross(body2.angularVelocity, r2)-body1.velocity-Cross(body1.angularVelocity, r1);
 105     Vec2 impulse = M*(bias-dv-softness*P);
 106
 107     body1.velocity -= body1.invMass*impulse;
 108     body1.angularVelocity -= body1.invI*Cross(r1, impulse);
 109
 110     body2.velocity += body2.invMass*impulse;
 111     body2.angularVelocity += body2.invI*Cross(r2, impulse);
 112
 113     P += impulse;
 114   }
 115 }