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