2 #include "BodyEnvironment.hpp"
3 #include "RigidBody.hpp"
5 #include <simgear/scene/material/mat.hxx>
6 #include <FDM/flight.hxx>
9 static const float YASIM_PI = 3.14159265358979323846;
10 static const float maxGroundBumpAmplitude=0.4;
11 //Amplitude can be positive and negative
17 _pos[i] = _cmpr[i] = 0;
29 _ground_frictionFactor = 1;
30 _ground_rollingFriction = 0.02;
31 _ground_loadCapacity = 1e30;
32 _ground_loadResistance = 1e30;
34 _ground_bumpiness = 0;
39 _reduceFrictionByExtension = 0;
40 _spring_factor_not_planing = 1;
43 _ignoreWhileSolving = 0;
46 _global_ground[i] = _global_vel[i] = 0;
47 _global_ground[2] = 1;
48 _global_ground[3] = -1e3;
51 void Gear::setPosition(float* position)
54 for(i=0; i<3; i++) _pos[i] = position[i];
57 void Gear::setCompression(float* compression)
60 for(i=0; i<3; i++) _cmpr[i] = compression[i];
63 void Gear::setSpring(float spring)
68 void Gear::setDamping(float damping)
73 void Gear::setStaticFriction(float sfric)
78 void Gear::setDynamicFriction(float dfric)
83 void Gear::setBrake(float brake)
85 _brake = Math::clamp(brake, 0, 1);
88 void Gear::setRotation(float rotation)
93 void Gear::setExtension(float extension)
95 _extension = Math::clamp(extension, 0, 1);
98 void Gear::setCastering(bool c)
103 void Gear::setContactPoint(bool c)
108 void Gear::setOnWater(bool c)
113 void Gear::setOnSolid(bool c)
118 void Gear::setIgnoreWhileSolving(bool c)
120 _ignoreWhileSolving = c;
123 void Gear::setSpringFactorNotPlaning(float f)
125 _spring_factor_not_planing = f;
128 void Gear::setSpeedPlaning(float s)
133 void Gear::setReduceFrictionByExtension(float s)
135 _reduceFrictionByExtension = s;
138 void Gear::setInitialLoad(float l)
143 void Gear::setGlobalGround(double *global_ground, float* global_vel,
144 double globalX, double globalY,
145 int type, const SGMaterial *material)
148 double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness;
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];
155 loadCapacity = (*material).get_load_resistance();
156 frictionFactor =(*material).get_friction_factor();
157 rollingFriction = (*material).get_rolling_friction();
158 loadResistance = (*material).get_load_resistance();
159 bumpiness = (*material).get_bumpiness();
160 isSolid = (*material).get_solid();
162 if (type == FGInterface::Solid) {
163 loadCapacity = DBL_MAX;
164 frictionFactor = 1.0;
165 rollingFriction = 0.02;
166 loadResistance = DBL_MAX;
169 } else if (type == FGInterface::Water) {
170 loadCapacity = DBL_MAX;
171 frictionFactor = 1.0;
173 loadResistance = DBL_MAX;
177 loadCapacity = DBL_MAX;
178 frictionFactor = 0.9;
179 rollingFriction = 0.1;
180 loadResistance = DBL_MAX;
186 _ground_frictionFactor = frictionFactor;
187 _ground_rollingFriction = rollingFriction;
188 _ground_loadCapacity = loadCapacity;
189 _ground_loadResistance = loadResistance;
190 _ground_bumpiness = bumpiness;
191 _ground_isSolid = isSolid;
197 void Gear::getPosition(float* out)
200 for(i=0; i<3; i++) out[i] = _pos[i];
203 void Gear::getCompression(float* out)
206 for(i=0; i<3; i++) out[i] = _cmpr[i];
209 void Gear::getGlobalGround(double* global_ground)
212 for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
215 float Gear::getSpring()
220 float Gear::getDamping()
225 float Gear::getStaticFriction()
230 float Gear::getDynamicFriction()
235 float Gear::getBrake()
240 float Gear::getRotation()
245 float Gear::getExtension()
250 void Gear::getForce(float* force, float* contact)
252 Math::set3(_force, force);
253 Math::set3(_contact, contact);
261 float Gear::getCompressFraction()
266 bool Gear::getCastering()
271 bool Gear::getGroundIsSolid()
273 return _ground_isSolid;
276 float Gear::getBumpAltitude()
278 if (_ground_bumpiness<0.001) return 0.0;
279 double x = _global_x*0.1;
280 double y = _global_y*0.1;
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
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);
293 return h*(1/8.)*_ground_bumpiness*maxGroundBumpAmplitude;
296 void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
298 // Init the return values
300 for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
302 // Don't bother if it's not down
306 // Dont bother if we are in the "wrong" ground
307 if (!((_onWater&&!_ground_isSolid)||(_onSolid&&_ground_isSolid))) {
316 // The ground plane transformed to the local frame.
318 s->planeGlobalToLocal(_global_ground, ground);
320 // The velocity of the contact patch transformed to local coordinates.
322 s->velGlobalToLocal(_global_vel, glvel);
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)
330 BumpAltitude=getBumpAltitude();
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.
348 Math::add3(_cmpr, _pos, tmp);
349 float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
351 // Calculate the point of ground _contact.
355 _contact[i] = _pos[i] + _frac*_cmpr[i];
357 // Turn _cmpr into a unit vector and a magnitude
359 float clen = Math::mag3(_cmpr);
360 Math::mul3(1/clen, _cmpr, cmpr);
362 // Now get the velocity of the point of contact
364 body->pointVelocity(_contact, rot, cv);
365 Math::add3(cv, v, cv);
366 Math::sub3(cv, glvel, cv);
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;
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;
377 frac_with_initial_load = (_frac+_initialLoad)
378 *_frac*_frac*3*25-_frac*_frac*_frac*2*125;
380 float fmag = frac_with_initial_load*clen*_spring;
381 if (_speed_planing>0)
383 float v = Math::mag3(cv);
384 if (v < _speed_planing)
386 float frac = v/_speed_planing;
387 fmag = fmag*_spring_factor_not_planing*(1-frac)+fmag*frac;
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
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);
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.
412 float gup[3]; // "up" unit vector from the ground
413 Math::set3(ground, gup);
414 Math::mul3(-1, gup, gup);
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
421 Math::cross3(skid, gup, steer); // skid cross up == steer
424 // Correct for a rotation
425 float srot = Math::sin(_rot);
426 float crot = Math::cos(_rot);
429 steer[0] = crot*tx + srot*ty;
430 steer[1] = -srot*tx + crot*ty;
434 skid[0] = crot*tx + srot*ty;
435 skid[1] = -srot*tx + crot*ty;
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
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
447 if(_rollSpeed > 0.05)
448 _casterAngle = Math::atan2(vskid, vsteer);
457 fsteer = (_brake * _ground_frictionFactor
458 +(1-_brake)*_ground_rollingFriction
459 )*calcFriction(wgt, vsteer);
460 fskid = calcFriction(wgt, vskid)*(_ground_frictionFactor);
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.
468 if(vsteer > 0) fsteer = -fsteer;
469 if(vskid > 0) fskid = -fskid;
471 //reduce friction if wanted by _reduceFrictionByExtension
472 float factor = (1-_frac)*(1-_reduceFrictionByExtension)+_frac*1;
473 factor = Math::clamp(factor,0,1);
477 // Phoo! All done. Add it up and get out of here.
478 Math::mul3(fsteer, steer, tmp);
479 Math::add3(tmp, _force, _force);
481 Math::mul3(fskid, skid, tmp);
482 Math::add3(tmp, _force, _force);
485 float Gear::calcFriction(float wgt, float v) //used on solid ground
487 // How slow is stopped? 10 cm/second?
488 const float STOP = 0.1f;
489 const float iSTOP = 1.0f/STOP;
491 if(v < STOP) return v*iSTOP * wgt * _sfric;
492 else return wgt * _dfric;
495 float Gear::calcFrictionFluid(float wgt, float v) //used on fluid ground
497 // How slow is stopped? 1 cm/second?
498 const float STOP = 0.01f;
499 const float iSTOP = 1.0f/STOP;
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
505 }; // namespace yasim