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