1
0
Fork 0

Revert "YASim gear: Clamping of _frac replaced"

This is causing simulator hangs at BIKF, likely due to previously
tolerated scenery data now triggering very large gear forces.

This reverts commit 6df2904f75.
This commit is contained in:
James Turner 2023-07-21 10:15:13 +01:00
parent 351ca111ad
commit 3535c6da7d

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,7 +325,10 @@ 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];
@ -356,7 +359,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. */
@ -373,7 +376,7 @@ bool gearCompression(
);
}
}
return true;
}
@ -396,7 +399,7 @@ bool gearCompressionOld(
BumpAltitude = bump_fn();
a+=BumpAltitude;
}
if(a > 0) {
o_compression_distance_vertical = 0;
o_compression_norm = 0;
@ -404,7 +407,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
@ -414,7 +417,10 @@ bool gearCompressionOld(
float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
// Calculate the point of ground _contact.
o_compression_norm = a/(a-b);
if(b < 0)
o_compression_norm = 1;
else
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;
@ -483,7 +489,10 @@ 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.
// Finally, we can start adding up the forces. First the spring
// compression. (note the clamping of _frac to 1):
_frac = (_frac > 1) ? 1 : _frac;
// Add the initial load to frac, but with continous transistion around 0
float frac_with_initial_load;
if (_frac>0.2 || _initialLoad==0.0)
@ -492,12 +501,7 @@ 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);
@ -915,3 +919,4 @@ void Gear::updateStuckPoint(State* s)
}; // namespace yasim