fly/flightgear
1
0
Fork 0
Code Activity

YASim gear: Clamping of _frac replaced

Browse source 
Instead use a a steep force increase

Implement Ticket https://sourceforge.net/p/flightgear/codetickets/1509/
This commit is contained in:
Jakub Kákona 2022-01-20 20:13:52 +01:00 • committed by James Turner
parent d3d4a8615a
commit 6df2904f75
1 changed files with 21 additions and 26 deletions

47
src/FDM/YASim/Gear.cpp
View file

@ -88,7 +88,7 @@ Gear::Gear()
Math::zero3(_ground_trans);
Math::identity33(_ground_rot);
_wheelAxle.set(0, 1, 0);
_wheelRadius = 0;
_tyreRadius = 0;
@ -268,14 +268,14 @@ bool gearCompression(
So:
compression_distance = -a / (compression . G)
*/
float ground_unit[3];
magnitudeUnit( ground, ground_unit);
/* Find S, the lowest point on wheel. */
float S[3];
Math::set3( wheel_pos, S);
if (wheel_radius) {
/* Find radial wheel vector pointing closest to ground using two
cross-products: wheel_axle_unit x ground_unit x wheel_axle_unit */
@ -287,11 +287,11 @@ bool gearCompression(
use it to find S. */
Math::unit3( R, R);
Math::mul3( wheel_radius, R, R);
/* Add R to S to get lowest point on wheel. */
Math::add3( S, R, S);
}
/* Calculate <a>, distance of S below ground. */
float a = Math::dot3( ground, S) - ( ground[3] - tyre_radius);
float bump_altitude = 0;
@ -299,15 +299,15 @@ bool gearCompression(
bump_altitude = bump_fn();
a -= bump_altitude;
}
if ( a < 0) {
/* S is above ground so we are not on ground. */
return false;
}
/* Lowest part of wheel is below ground. */
o_compression_distance_vertical = a;
/* Find compression_norm. First we need to find compression_distance, the
distance to compress the gear so that S (which is below ground+tyre_radius)
would move to just touch ground+tyre_radius. We need to move gear further
@ -325,10 +325,7 @@ bool gearCompression(
if ( compression.magnitude) {
o_compression_norm = compression_distance / compression.magnitude;
}
if (o_compression_norm > 1) {
o_compression_norm = 1;
}
/* Contact point on ground-plus-tyre-radius is S plus compression
vector. */
float delta[3];
@ -359,7 +356,7 @@ bool gearCompression(
Math::mul3( tyre_radius, ground_unit, delta);
Math::add3( o_contact, delta, o_contact);
}
{
/* Verify that <o_contact> is on ground; this can fail e.g. when resetting so for now we
just output a diagnostic rather than assert fail. */
@ -376,7 +373,7 @@ bool gearCompression(
);
}
}
return true;
}
@ -399,7 +396,7 @@ bool gearCompressionOld(
BumpAltitude = bump_fn();
a+=BumpAltitude;
}
if(a > 0) {
o_compression_distance_vertical = 0;
o_compression_norm = 0;
@ -407,7 +404,7 @@ bool gearCompressionOld(
}
o_compression_distance_vertical = -a;
// Now a is the distance from the tip to ground, so make b the
// distance from the base to ground. We can get the fraction
// (0-1) of compression from a/(a-b). Note the minus sign -- stuff
@ -417,10 +414,7 @@ bool gearCompressionOld(
float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
// Calculate the point of ground _contact.
if(b < 0)
o_compression_norm = 1;
else
o_compression_norm = a/(a-b);
o_compression_norm = a/(a-b);
for(int i=0; i<3; i++)
o_contact[i] = pos[i] + o_compression_norm * compression.v[i];
return true;
@ -489,10 +483,7 @@ void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* r
Math::add3(cv, v, cv);
Math::sub3(cv, glvel, cv);
// Finally, we can start adding up the forces. First the spring
// compression. (note the clamping of _frac to 1):
_frac = (_frac > 1) ? 1 : _frac;
// Finally, we can start adding up the forces. First the spring compression.
// Add the initial load to frac, but with continous transistion around 0
float frac_with_initial_load;
if (_frac>0.2 || _initialLoad==0.0)
@ -501,7 +492,12 @@ void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* r
frac_with_initial_load = (_frac+_initialLoad)
*_frac*_frac*3*25-_frac*_frac*_frac*2*125;
// in case of _frac >= 1 spring at bump stop has rapidly increasing rebound force
float fmag = frac_with_initial_load*clen*_spring;
if (_frac > 1 ) {
fmag = pow(frac_with_initial_load, 4) * clen * _spring;
}
if (_speed_planing>0)
{
float v = Math::mag3(cv);
@ -919,4 +915,3 @@ void Gear::updateStuckPoint(State* s)
}; // namespace yasim