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:
parent
351ca111ad
commit
3535c6da7d
1 changed files with 26 additions and 21 deletions
|
@ -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
|
||||
|
||||
|
|
Loading…
Add table
Reference in a new issue