src/FDM/YASim/Gear.cpp

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