src/FDM/YASim/Gear.cpp

   1 #include "Math.hpp"
   2 #include "RigidBody.hpp"
   3
   4 #include "Gear.hpp"
   5 namespace yasim {
   6
   7 Gear::Gear()
   8 {
   9     for(int i=0; i<3; i++)
  10         _pos[i] = _cmpr[i] = 0;
  11     _spring = 1;
  12     _damp = 0;
  13     _sfric = 0.8;
  14     _dfric = 0.7;
  15     _brake = 0;
  16     _rot = 0;
  17     _extension = 1;
  18 }
  19
  20 void Gear::setPosition(float* position)
  21 {
  22     for(int i=0; i<3; i++) _pos[i] = position[i];
  23 }
  24
  25 void Gear::setCompression(float* compression)
  26 {
  27     for(int i=0; i<3; i++) _cmpr[i] = compression[i];
  28 }
  29
  30 void Gear::setSpring(float spring)
  31 {
  32     _spring = spring;
  33 }
  34
  35 void Gear::setDamping(float damping)
  36 {
  37     _damp = damping;
  38 }
  39
  40 void Gear::setStaticFriction(float sfric)
  41 {
  42     _sfric = sfric;
  43 }
  44
  45 void Gear::setDynamicFriction(float dfric)
  46 {
  47     _dfric = dfric;
  48 }
  49
  50 void Gear::setBrake(float brake)
  51 {
  52     _brake = brake;
  53 }
  54
  55 void Gear::setRotation(float rotation)
  56 {
  57     _rot = rotation;
  58 }
  59
  60 void Gear::setExtension(float extension)
  61 {
  62     _extension = extension;
  63 }
  64
  65 void Gear::getPosition(float* out)
  66 {
  67     for(int i=0; i<3; i++) out[i] = _pos[i];
  68 }
  69
  70 void Gear::getCompression(float* out)
  71 {
  72     for(int i=0; i<3; i++) out[i] = _cmpr[i];
  73 }
  74
  75 float Gear::getSpring()
  76 {
  77     return _spring;
  78 }
  79
  80 float Gear::getDamping()
  81 {
  82     return _damp;
  83 }
  84
  85 float Gear::getStaticFriction()
  86 {
  87     return _sfric;
  88 }
  89
  90 float Gear::getDynamicFriction()
  91 {
  92     return _dfric;
  93 }
  94
  95 float Gear::getBrake()
  96 {
  97     return _brake;
  98 }
  99
 100 float Gear::getRotation()
 101 {
 102     return _rot;
 103 }
 104
 105 float Gear::getExtension()
 106 {
 107     return _extension;
 108 }
 109
 110 void Gear::getForce(float* force, float* contact)
 111 {
 112     Math::set3(_force, force);
 113     Math::set3(_contact, contact);
 114 }
 115
 116 float Gear::getWoW()
 117 {
 118     return _wow;
 119 }
 120
 121 float Gear::getCompressFraction()
 122 {
 123     return _frac;
 124 }
 125
 126 void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
 127 {
 128     // Init the return values
 129     for(int i=0; i<3; i++) _force[i] = _contact[i] = 0;
 130
 131     // Don't bother if it's not down
 132     if(_extension < 1)
 133         return;
 134
 135     float tmp[3];
 136
 137     // First off, make sure that the gear "tip" is below the ground.
 138     // If it's not, there's no force.
 139     float a = ground[3] - Math::dot3(_pos, ground);
 140     if(a > 0)
 141         return;
 142
 143     // Now a is the distance from the tip to ground, so make b the
 144     // distance from the base to ground.  We can get the fraction
 145     // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
 146     // above ground is negative.
 147     Math::add3(_cmpr, _pos, tmp);
 148     float b = ground[3] - Math::dot3(tmp, ground);
 149
 150     // Calculate the point of ground _contact.
 151     _frac = a/(a-b);
 152     if(b < 0) _frac = 1;
 153     for(int i=0; i<3; i++)
 154         _contact[i] = _pos[i] + _frac*_cmpr[i];
 155
 156     // Turn _cmpr into a unit vector and a magnitude
 157     float cmpr[3];
 158     float clen = Math::mag3(_cmpr);
 159     Math::mul3(1/clen, _cmpr, cmpr);
 160
 161     // Now get the velocity of the point of contact
 162     float cv[3];
 163     body->pointVelocity(_contact, rot, cv);
 164     Math::add3(cv, v, cv);
 165
 166     // Finally, we can start adding up the forces.  First the
 167     // spring compression (note the clamping of _frac to 1):
 168     _frac = (_frac > 1) ? 1 : _frac;
 169     float fmag = _frac*clen*_spring;
 170     Math::mul3(fmag, cmpr, _force);
 171
 172     // Then the damping.  Use the only the velocity into the ground
 173     // (projection along "ground") projected along the compression
 174     // axis.  So Vdamp = ground*(ground dot cv) dot cmpr
 175     Math::mul3(Math::dot3(ground, cv), ground, tmp);
 176     float dv = Math::dot3(cmpr, tmp);
 177     float damp = _damp * dv;
 178     if(damp > fmag) damp = fmag; // can't pull the plane down!
 179     if(damp < -fmag) damp = -fmag; // sanity
 180     Math::mul3(-damp, cmpr, tmp);
 181     Math::add3(_force, tmp, _force);
 182
 183     _wow = fmag - damp;
 184
 185     // Wheels are funky.  Split the velocity along the ground plane
 186     // into rolling and skidding components.  Assuming small angles,
 187     // we generate "forward" and "left" unit vectors (the compression
 188     // goes "up") for the gear, make a "steer" direction from these,
 189     // and then project it onto the ground plane.  Project the
 190     // velocity onto the ground plane too, and extract the "steer"
 191     // component.  The remainder is the skid velocity.
 192
 193     float gup[3]; // "up" unit vector from the ground
 194     Math::set3(ground, gup);
 195     Math::mul3(-1, gup, gup);
 196
 197     float xhat[] = {1,0,0};
 198     float steer[3], skid[3];
 199     Math::cross3(gup, xhat, skid);  // up cross xhat =~ skid
 200     Math::unit3(skid, skid);        //               == skid
 201
 202     Math::cross3(skid, gup, steer); // skid cross up == steer
 203
 204     if(_rot != 0) {
 205         // Correct for a (small) rotation
 206         Math::mul3(_rot, steer, tmp);
 207         Math::add3(tmp, skid, skid);
 208         Math::unit3(skid, skid);
 209         Math::cross3(skid, gup, steer);
 210     }
 211
 212     float vsteer = Math::dot3(cv, steer);
 213     float vskid  = Math::dot3(cv, skid);
 214     float wgt = Math::dot3(_force, gup); // force into the ground
 215
 216     float fsteer = _brake * calcFriction(wgt, vsteer);
 217     float fskid  = calcFriction(wgt, vskid);
 218     if(vsteer > 0) fsteer = -fsteer;
 219     if(vskid > 0) fskid = -fskid;
 220
 221     // Phoo!  All done.  Add it up and get out of here.
 222     Math::mul3(fsteer, steer, tmp);
 223     Math::add3(tmp, _force, _force);
 224
 225     Math::mul3(fskid, skid, tmp);
 226     Math::add3(tmp, _force, _force);
 227 }
 228
 229 float Gear::calcFriction(float wgt, float v)
 230 {
 231     // How slow is stopped?  50 cm/second?
 232     const float STOP = 0.5;
 233     const float iSTOP = 1/STOP;
 234     v = Math::abs(v);
 235     if(v < STOP) return v*iSTOP * wgt * _sfric;
 236     else         return wgt * _dfric;
 237 }
 238
 239 }; // namespace yasim
 240