src/FDM/YASim/Gear.cpp

   1
   2 #ifdef HAVE_CONFIG_H
   3 #  include "config.h"
   4 #endif
   5
   6 #include "Math.hpp"
   7 #include "BodyEnvironment.hpp"
   8 #include "RigidBody.hpp"
   9
  10 #include <simgear/scene/material/mat.hxx>
  11 #include <FDM/flight.hxx>
  12 #include "Gear.hpp"
  13 namespace yasim {
  14 static const float YASIM_PI = 3.14159265358979323846;
  15 static const float maxGroundBumpAmplitude=0.4;
  16         //Amplitude can be positive and negative
  17
  18 Gear::Gear()
  19 {
  20     int i;
  21     for(i=0; i<3; i++)
  22         _pos[i] = _cmpr[i] = 0;
  23     _spring = 1;
  24     _damp = 0;
  25     _sfric = 0.8f;
  26     _dfric = 0.7f;
  27     _brake = 0;
  28     _rot = 0;
  29     _initialLoad = 0;
  30     _extension = 1;
  31     _castering = false;
  32     _frac = 0;
  33     _ground_frictionFactor = 1;
  34     _ground_rollingFriction = 0.02;
  35     _ground_loadCapacity = 1e30;
  36     _ground_loadResistance = 1e30;
  37     _ground_isSolid = 1;
  38     _ground_bumpiness = 0;
  39     _onWater = 0;
  40     _onSolid = 1;
  41     _global_x = 0.0;
  42     _global_y = 0.0;
  43     _reduceFrictionByExtension = 0;
  44     _spring_factor_not_planing = 1;
  45     _speed_planing = 0;
  46     _isContactPoint = 0;
  47     _ignoreWhileSolving = 0;
  48
  49     for(i=0; i<3; i++)
  50         _global_ground[i] = _global_vel[i] = 0;
  51     _global_ground[2] = 1;
  52     _global_ground[3] = -1e3;
  53 }
  54
  55 void Gear::setPosition(float* position)
  56 {
  57     int i;
  58     for(i=0; i<3; i++) _pos[i] = position[i];
  59 }
  60
  61 void Gear::setCompression(float* compression)
  62 {
  63     int i;
  64     for(i=0; i<3; i++) _cmpr[i] = compression[i];
  65 }
  66
  67 void Gear::setSpring(float spring)
  68 {
  69     _spring = spring;
  70 }
  71
  72 void Gear::setDamping(float damping)
  73 {
  74     _damp = damping;
  75 }
  76
  77 void Gear::setStaticFriction(float sfric)
  78 {
  79     _sfric = sfric;
  80 }
  81
  82 void Gear::setDynamicFriction(float dfric)
  83 {
  84     _dfric = dfric;
  85 }
  86
  87 void Gear::setBrake(float brake)
  88 {
  89     _brake = Math::clamp(brake, 0, 1);
  90 }
  91
  92 void Gear::setRotation(float rotation)
  93 {
  94     _rot = rotation;
  95 }
  96
  97 void Gear::setExtension(float extension)
  98 {
  99     _extension = Math::clamp(extension, 0, 1);
 100 }
 101
 102 void Gear::setCastering(bool c)
 103 {
 104     _castering = c;
 105 }
 106
 107 void Gear::setContactPoint(bool c)
 108 {
 109     _isContactPoint=c;
 110 }
 111
 112 void Gear::setOnWater(bool c)
 113 {
 114     _onWater = c;
 115 }
 116
 117 void Gear::setOnSolid(bool c)
 118 {
 119     _onSolid = c;
 120 }
 121
 122 void Gear::setIgnoreWhileSolving(bool c)
 123 {
 124     _ignoreWhileSolving = c;
 125 }
 126
 127 void Gear::setSpringFactorNotPlaning(float f)
 128 {
 129     _spring_factor_not_planing = f;
 130 }
 131
 132 void Gear::setSpeedPlaning(float s)
 133 {
 134     _speed_planing = s;
 135 }
 136
 137 void Gear::setReduceFrictionByExtension(float s)
 138 {
 139     _reduceFrictionByExtension = s;
 140 }
 141
 142 void Gear::setInitialLoad(float l)
 143 {
 144     _initialLoad = l;
 145 }
 146
 147 void Gear::setGlobalGround(double *global_ground, float* global_vel,
 148                            double globalX, double globalY,
 149                            const SGMaterial *material)
 150 {
 151     int i;
 152     double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness;
 153     bool isSolid;
 154
 155     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 156     for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
 157
 158     if (material) {
 159         loadCapacity = (*material).get_load_resistance();
 160         frictionFactor =(*material).get_friction_factor();
 161         rollingFriction = (*material).get_rolling_friction();
 162         loadResistance = (*material).get_load_resistance();
 163         bumpiness = (*material).get_bumpiness();
 164         isSolid = (*material).get_solid();
 165     } else {
 166         // no material, assume solid
 167         loadCapacity = DBL_MAX;
 168         frictionFactor = 1.0;
 169         rollingFriction = 0.02;
 170         loadResistance = DBL_MAX;
 171         bumpiness = 0.0;
 172         isSolid = true;
 173     }
 174     _ground_frictionFactor = frictionFactor;
 175     _ground_rollingFriction = rollingFriction;
 176     _ground_loadCapacity = loadCapacity;
 177     _ground_loadResistance = loadResistance;
 178     _ground_bumpiness = bumpiness;
 179     _ground_isSolid = isSolid;
 180     _global_x = globalX;
 181     _global_y = globalY;
 182
 183     }
 184
 185 void Gear::getPosition(float* out)
 186 {
 187     int i;
 188     for(i=0; i<3; i++) out[i] = _pos[i];
 189 }
 190
 191 void Gear::getCompression(float* out)
 192 {
 193     int i;
 194     for(i=0; i<3; i++) out[i] = _cmpr[i];
 195 }
 196
 197 void Gear::getGlobalGround(double* global_ground)
 198 {
 199     int i;
 200     for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
 201 }
 202
 203 float Gear::getSpring()
 204 {
 205     return _spring;
 206 }
 207
 208 float Gear::getDamping()
 209 {
 210     return _damp;
 211 }
 212
 213 float Gear::getStaticFriction()
 214 {
 215     return _sfric;
 216 }
 217
 218 float Gear::getDynamicFriction()
 219 {
 220     return _dfric;
 221 }
 222
 223 float Gear::getBrake()
 224 {
 225     return _brake;
 226 }
 227
 228 float Gear::getRotation()
 229 {
 230     return _rot;
 231 }
 232
 233 float Gear::getExtension()
 234 {
 235     return _extension;
 236 }
 237
 238 void Gear::getForce(float* force, float* contact)
 239 {
 240     Math::set3(_force, force);
 241     Math::set3(_contact, contact);
 242 }
 243
 244 float Gear::getWoW()
 245 {
 246     return _wow;
 247 }
 248
 249 float Gear::getCompressFraction()
 250 {
 251     return _frac;
 252 }
 253
 254 bool Gear::getCastering()
 255 {
 256     return _castering;
 257 }
 258
 259 bool Gear::getGroundIsSolid()
 260 {
 261     return _ground_isSolid;
 262 }
 263
 264 float Gear::getBumpAltitude()
 265 {
 266     if (_ground_bumpiness<0.001) return 0.0;
 267     double x = _global_x*0.1;
 268     double y = _global_y*0.1;
 269     x -= Math::floor(x);
 270     y -= Math::floor(y);
 271     x *= 2*YASIM_PI;
 272     y *= 2*YASIM_PI;
 273     //now x and y are in the range of 0..2pi
 274     //we need a function, that is periodically on 2pi and gives some
 275     //height. This is not very fast, but for a beginning.
 276     //maybe this should be done by interpolating between some precalculated
 277     //values
 278     float h = Math::sin(x)+Math::sin(7*x)+Math::sin(8*x)+Math::sin(13*x);
 279     h += Math::sin(2*y)+Math::sin(5*y)+Math::sin(9*y*x)+Math::sin(17*y);
 280
 281     return h*(1/8.)*_ground_bumpiness*maxGroundBumpAmplitude;
 282 }
 283
 284 void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
 285 {
 286     // Init the return values
 287     int i;
 288     for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
 289
 290     // Don't bother if it's not down
 291     if(_extension < 1)
 292     {
 293         _wow = 0;
 294         _frac = 0;
 295         return;
 296     }
 297
 298     // Dont bother if we are in the "wrong" ground
 299     if (!((_onWater&&!_ground_isSolid)||(_onSolid&&_ground_isSolid)))  {
 300          _wow = 0;
 301          _frac = 0;
 302         _compressDist = 0;
 303         _rollSpeed = 0;
 304         _casterAngle = 0;
 305         return;
 306     }
 307
 308     // The ground plane transformed to the local frame.
 309     float ground[4];
 310     s->planeGlobalToLocal(_global_ground, ground);
 311
 312     // The velocity of the contact patch transformed to local coordinates.
 313     float glvel[3];
 314     s->velGlobalToLocal(_global_vel, glvel);
 315
 316     // First off, make sure that the gear "tip" is below the ground.
 317     // If it's not, there's no force.
 318     float a = ground[3] - Math::dot3(_pos, ground);
 319     float BumpAltitude=0;
 320     if (a<maxGroundBumpAmplitude)
 321     {
 322         BumpAltitude=getBumpAltitude();
 323         a+=BumpAltitude;
 324     }
 325     _compressDist = -a;
 326     if(a > 0) {
 327         _wow = 0;
 328         _frac = 0;
 329         _compressDist = 0;
 330         _rollSpeed = 0;
 331         _casterAngle = 0;
 332         return;
 333     }
 334
 335     // Now a is the distance from the tip to ground, so make b the
 336     // distance from the base to ground.  We can get the fraction
 337     // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
 338     // above ground is negative.
 339     float tmp[3];
 340     Math::add3(_cmpr, _pos, tmp);
 341     float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
 342
 343     // Calculate the point of ground _contact.
 344     if(b < 0)
 345         _frac = 1;
 346     else
 347         _frac = a/(a-b);
 348     for(i=0; i<3; i++)
 349         _contact[i] = _pos[i] + _frac*_cmpr[i];
 350
 351     // Turn _cmpr into a unit vector and a magnitude
 352     float cmpr[3];
 353     float clen = Math::mag3(_cmpr);
 354     Math::mul3(1/clen, _cmpr, cmpr);
 355
 356     // Now get the velocity of the point of contact
 357     float cv[3];
 358     body->pointVelocity(_contact, rot, cv);
 359     Math::add3(cv, v, cv);
 360     Math::sub3(cv, glvel, cv);
 361
 362     // Finally, we can start adding up the forces.  First the spring
 363     // compression.   (note the clamping of _frac to 1):
 364     _frac = (_frac > 1) ? 1 : _frac;
 365
 366     // Add the initial load to frac, but with continous transistion around 0
 367     float frac_with_initial_load;
 368     if (_frac>0.2 || _initialLoad==0.0)
 369         frac_with_initial_load = _frac+_initialLoad;
 370     else
 371         frac_with_initial_load = (_frac+_initialLoad)
 372             *_frac*_frac*3*25-_frac*_frac*_frac*2*125;
 373
 374     float fmag = frac_with_initial_load*clen*_spring;
 375     if (_speed_planing>0)
 376     {
 377         float v = Math::mag3(cv);
 378         if (v < _speed_planing)
 379         {
 380             float frac = v/_speed_planing;
 381             fmag = fmag*_spring_factor_not_planing*(1-frac)+fmag*frac;
 382         }
 383     }
 384     // Then the damping.  Use the only the velocity into the ground
 385     // (projection along "ground") projected along the compression
 386     // axis.  So Vdamp = ground*(ground dot cv) dot cmpr
 387     Math::mul3(Math::dot3(ground, cv), ground, tmp);
 388     float dv = Math::dot3(cmpr, tmp);
 389     float damp = _damp * dv;
 390     if(damp > fmag) damp = fmag; // can't pull the plane down!
 391     if(damp < -fmag) damp = -fmag; // sanity
 392
 393     // The actual force applied is only the component perpendicular to
 394     // the ground.  Side forces come from velocity only.
 395     _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
 396     Math::mul3(-_wow, ground, _force);
 397
 398     // Wheels are funky.  Split the velocity along the ground plane
 399     // into rolling and skidding components.  Assuming small angles,
 400     // we generate "forward" and "left" unit vectors (the compression
 401     // goes "up") for the gear, make a "steer" direction from these,
 402     // and then project it onto the ground plane.  Project the
 403     // velocity onto the ground plane too, and extract the "steer"
 404     // component.  The remainder is the skid velocity.
 405
 406     float gup[3]; // "up" unit vector from the ground
 407     Math::set3(ground, gup);
 408     Math::mul3(-1, gup, gup);
 409
 410     float xhat[] = {1,0,0};
 411     float steer[3], skid[3];
 412     Math::cross3(gup, xhat, skid);  // up cross xhat =~ skid
 413     Math::unit3(skid, skid);        //               == skid
 414
 415     Math::cross3(skid, gup, steer); // skid cross up == steer
 416
 417     if(_rot != 0) {
 418         // Correct for a rotation
 419         float srot = Math::sin(_rot);
 420         float crot = Math::cos(_rot);
 421         float tx = steer[0];
 422         float ty = steer[1];
 423         steer[0] =  crot*tx + srot*ty;
 424         steer[1] = -srot*tx + crot*ty;
 425
 426         tx = skid[0];
 427         ty = skid[1];
 428         skid[0] =  crot*tx + srot*ty;
 429         skid[1] = -srot*tx + crot*ty;
 430     }
 431
 432     float vsteer = Math::dot3(cv, steer);
 433     float vskid  = Math::dot3(cv, skid);
 434     float wgt = Math::dot3(_force, gup); // force into the ground
 435
 436     if(_castering) {
 437         _rollSpeed = Math::sqrt(vsteer*vsteer + vskid*vskid);
 438         // Don't modify caster angle when the wheel isn't moving,
 439         // or else the angle will animate the "jitter" of a stopped
 440         // gear.
 441         if(_rollSpeed > 0.05)
 442             _casterAngle = Math::atan2(vskid, vsteer);
 443         return;
 444     } else {
 445         _rollSpeed = vsteer;
 446         _casterAngle = _rot;
 447     }
 448     float fsteer,fskid;
 449     if(_ground_isSolid)
 450     {
 451         fsteer = (_brake * _ground_frictionFactor
 452                     +(1-_brake)*_ground_rollingFriction
 453                  )*calcFriction(wgt, vsteer);
 454         fskid  = calcFriction(wgt, vskid)*(_ground_frictionFactor);
 455     }
 456     else
 457     {
 458         fsteer = calcFrictionFluid(wgt, vsteer)*_ground_frictionFactor;
 459         fskid  = 10*calcFrictionFluid(wgt, vskid)*_ground_frictionFactor;
 460         //factor 10: floats have different drag in x and y.
 461     }
 462     if(vsteer > 0) fsteer = -fsteer;
 463     if(vskid > 0) fskid = -fskid;
 464
 465     //reduce friction if wanted by _reduceFrictionByExtension
 466     float factor = (1-_frac)*(1-_reduceFrictionByExtension)+_frac*1;
 467     factor = Math::clamp(factor,0,1);
 468     fsteer *= factor;
 469     fskid *= factor;
 470
 471     // Phoo!  All done.  Add it up and get out of here.
 472     Math::mul3(fsteer, steer, tmp);
 473     Math::add3(tmp, _force, _force);
 474
 475     Math::mul3(fskid, skid, tmp);
 476     Math::add3(tmp, _force, _force);
 477 }
 478
 479 float Gear::calcFriction(float wgt, float v) //used on solid ground
 480 {
 481     // How slow is stopped?  10 cm/second?
 482     const float STOP = 0.1f;
 483     const float iSTOP = 1.0f/STOP;
 484     v = Math::abs(v);
 485     if(v < STOP) return v*iSTOP * wgt * _sfric;
 486     else         return wgt * _dfric;
 487 }
 488
 489 float Gear::calcFrictionFluid(float wgt, float v) //used on fluid ground
 490 {
 491     // How slow is stopped?  1 cm/second?
 492     const float STOP = 0.01f;
 493     const float iSTOP = 1.0f/STOP;
 494     v = Math::abs(v);
 495     if(v < STOP) return v*iSTOP * wgt * _sfric;
 496     else return wgt * _dfric*v*v*0.01;
 497     //*0.01: to get _dfric of the same size than _dfric on solid
 498 }
 499 }; // namespace yasim
 500