src/FDM/YASim/Gear.cpp

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