273 lines
10 KiB
C#
273 lines
10 KiB
C#
using Sandbox;
|
|
using System;
|
|
|
|
namespace VeloX;
|
|
|
|
|
|
public partial class VeloXWheel
|
|
{
|
|
/// <summary>
|
|
/// Constant torque acting similar to brake torque.
|
|
/// Imitates rolling resistance.
|
|
/// </summary>
|
|
[Property, Range( 0, 500 ), Sync] public float RollingResistanceTorque { get; set; } = 30f;
|
|
|
|
/// <summary>
|
|
/// The percentage this wheel is contributing to the total vehicle load bearing.
|
|
/// </summary>
|
|
public float LoadContribution { get; set; } = 0.25f;
|
|
|
|
/// <summary>
|
|
/// Maximum load the tire is rated for in [N].
|
|
/// Used to calculate friction.Default value is adequate for most cars but
|
|
/// larger and heavier vehicles such as semi trucks will use higher values.
|
|
/// A good rule of the thumb is that this value should be 2x the Load
|
|
/// while vehicle is stationary.
|
|
/// </summary>
|
|
[Property, Sync] public float LoadRating { get; set; } = 5400;
|
|
|
|
|
|
/// <summary>
|
|
/// The amount of torque returned by the wheel.
|
|
/// Under no-slip conditions this will be equal to the torque that was input.
|
|
/// When there is wheel spin, the value will be less than the input torque.
|
|
/// </summary>
|
|
public float CounterTorque { get; private set; }
|
|
|
|
//[Property, Range( 0, 2 )] public float BrakeMult { get; set; } = 1f;
|
|
public Vector3 FrictionForce;
|
|
|
|
|
|
private Vector3 hitContactVelocity;
|
|
private Vector3 hitForwardDirection;
|
|
private Vector3 hitSidewaysDirection;
|
|
|
|
public Vector3 ContactRight => hitSidewaysDirection;
|
|
public Vector3 ContactForward => hitForwardDirection;
|
|
|
|
public float LongitudinalSlip => sx;
|
|
public float LongitudinalSpeed => vx;
|
|
public bool IsSkiddingLongitudinally => NormalizedLongitudinalSlip > 0.35f;
|
|
public float NormalizedLongitudinalSlip => Math.Clamp( Math.Abs( LongitudinalSlip ), 0, 1 );
|
|
|
|
public float LateralSlip => sy;
|
|
public float LateralSpeed => vy;
|
|
public bool IsSkiddingLaterally => NormalizedLateralSlip > 0.35f;
|
|
public float NormalizedLateralSlip => Math.Clamp( Math.Abs( LateralSlip ), 0, 1 );
|
|
|
|
public bool IsSkidding => IsSkiddingLaterally || IsSkiddingLongitudinally;
|
|
public float NormalizedSlip => (NormalizedLateralSlip + NormalizedLongitudinalSlip) / 2f;
|
|
|
|
|
|
// speed
|
|
[Sync] private float vx { get; set; }
|
|
[Sync] private float vy { get; set; }
|
|
|
|
// force
|
|
[Sync] private float fx { get; set; }
|
|
[Sync] private float fy { get; set; }
|
|
|
|
// slip
|
|
[Sync] private float sx { get; set; }
|
|
[Sync] private float sy { get; set; }
|
|
|
|
private void UpdateHitVariables()
|
|
{
|
|
if ( IsOnGround )
|
|
{
|
|
hitContactVelocity = Vehicle.Body.GetVelocityAtPoint( ContactPosition + Vehicle.Body.MassCenter );
|
|
|
|
hitForwardDirection = ContactNormal.Cross( TransformRotationSteer.Right ).Normal;
|
|
hitSidewaysDirection = Rotation.FromAxis( ContactNormal, 90f ) * hitForwardDirection;
|
|
|
|
vx = hitContactVelocity.Dot( hitForwardDirection ).InchToMeter();
|
|
vy = hitContactVelocity.Dot( hitSidewaysDirection ).InchToMeter();
|
|
}
|
|
else
|
|
{
|
|
vx = 0;
|
|
vy = 0;
|
|
}
|
|
}
|
|
|
|
|
|
private Vector3 lowSpeedReferencePosition;
|
|
private bool lowSpeedReferenceIsSet;
|
|
private Vector3 currentPosition;
|
|
private Vector3 referenceError;
|
|
private Vector3 correctiveForce;
|
|
public bool wheelIsBlocked;
|
|
private void UpdateFriction( float dt )
|
|
{
|
|
var motorTorque = DriveTorque;
|
|
var brakeTorque = BrakeTorque;
|
|
|
|
float allWheelLoadSum = Vehicle.CombinedLoad;
|
|
|
|
LoadContribution = allWheelLoadSum == 0 ? 1f : Fz / allWheelLoadSum;
|
|
|
|
float mRadius = Radius;
|
|
|
|
float invDt = 1f / dt;
|
|
float invRadius = 1f / mRadius;
|
|
float inertia = Inertia;
|
|
float invInertia = 1f / Inertia;
|
|
|
|
float loadClamped = Math.Clamp( Fz, 0, LoadRating );
|
|
|
|
float forwardLoadFactor = loadClamped * 1.35f;
|
|
float sideLoadFactor = loadClamped * 1.9f;
|
|
|
|
float loadPercent = Math.Clamp( Fz / LoadRating, 0f, 1f );
|
|
float slipLoadModifier = 1f - loadPercent * 0.4f;
|
|
|
|
float mass = Vehicle.Body.Mass;
|
|
float absForwardSpeed = Math.Abs( vx );
|
|
float forwardForceClamp = mass * LoadContribution * absForwardSpeed * invDt;
|
|
float absSideSpeed = Math.Abs( vy );
|
|
float sideForceClamp = mass * LoadContribution * absSideSpeed * invDt;
|
|
|
|
float forwardSpeedClamp = 1.5f * (dt / 0.005f);
|
|
forwardSpeedClamp = Math.Clamp( forwardSpeedClamp, 1.5f, 10f );
|
|
float clampedAbsForwardSpeed = Math.Max( absForwardSpeed, forwardSpeedClamp );
|
|
|
|
float peakForwardFrictionForce = 11000 * (1 - MathF.Exp( -0.00014f * forwardLoadFactor ));
|
|
float absCombinedBrakeTorque = Math.Max( 0, brakeTorque + RollingResistanceTorque );
|
|
|
|
float signedCombinedBrakeTorque = absCombinedBrakeTorque * (vx > 0 ? -1 : 1);
|
|
float signedCombinedBrakeForce = signedCombinedBrakeTorque * invRadius;
|
|
float motorForce = motorTorque * invRadius;
|
|
float forwardInputForce = motorForce + signedCombinedBrakeForce;
|
|
float absMotorTorque = Math.Abs( motorTorque );
|
|
float absBrakeTorque = Math.Abs( brakeTorque );
|
|
|
|
float maxForwardForce = Math.Min( peakForwardFrictionForce, forwardForceClamp );
|
|
|
|
maxForwardForce = absMotorTorque < absBrakeTorque ? maxForwardForce : peakForwardFrictionForce;
|
|
fx = forwardInputForce > maxForwardForce ? maxForwardForce
|
|
: forwardInputForce < -maxForwardForce ? -maxForwardForce : forwardInputForce;
|
|
|
|
wheelIsBlocked = false;
|
|
if ( IsOnGround )
|
|
{
|
|
float combinedWheelForce = motorForce + absCombinedBrakeTorque * invRadius * -Math.Sign( AngularVelocity );
|
|
float wheelForceClampOverflow = 0;
|
|
if ( (combinedWheelForce >= 0 && AngularVelocity < 0) || (combinedWheelForce < 0 && AngularVelocity > 0) )
|
|
{
|
|
float absWheelForceClamp = Math.Abs( AngularVelocity ) * inertia * invRadius * invDt;
|
|
float absCombinedWheelForce = combinedWheelForce < 0 ? -combinedWheelForce : combinedWheelForce;
|
|
float wheelForceDiff = absCombinedWheelForce - absWheelForceClamp;
|
|
wheelForceClampOverflow = Math.Max( 0, wheelForceDiff ) * Math.Sign( combinedWheelForce );
|
|
combinedWheelForce = Math.Clamp( combinedWheelForce, -absWheelForceClamp, absWheelForceClamp );
|
|
}
|
|
AngularVelocity += combinedWheelForce * mRadius * invInertia * dt;
|
|
|
|
// Surface (corrective) force
|
|
float noSlipAngularVelocity = vx * invRadius;
|
|
float angularVelocityError = AngularVelocity - noSlipAngularVelocity;
|
|
|
|
float angularVelocityCorrectionForce = Math.Clamp( -angularVelocityError * inertia * invRadius * invDt, -maxForwardForce, maxForwardForce );
|
|
if ( absMotorTorque < absBrakeTorque && Math.Abs( wheelForceClampOverflow ) > Math.Abs( angularVelocityCorrectionForce ) )
|
|
{
|
|
wheelIsBlocked = true;
|
|
AngularVelocity += vx > 0 ? 1e-10f : -1e-10f;
|
|
}
|
|
else
|
|
{
|
|
AngularVelocity += angularVelocityCorrectionForce * mRadius * invInertia * dt;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float maxBrakeTorque = AngularVelocity * inertia * invDt + motorTorque;
|
|
maxBrakeTorque = maxBrakeTorque < 0 ? -maxBrakeTorque : maxBrakeTorque;
|
|
float brakeTorqueSign = AngularVelocity < 0f ? -1f : 1f;
|
|
float clampedBrakeTorque = Math.Clamp( absCombinedBrakeTorque, -maxBrakeTorque, maxBrakeTorque );
|
|
AngularVelocity += (motorTorque - brakeTorqueSign * clampedBrakeTorque) * invInertia * dt;
|
|
}
|
|
|
|
float absAngularVelocity = AngularVelocity < 0 ? -AngularVelocity : AngularVelocity;
|
|
float maxCounterTorque = inertia * absAngularVelocity;
|
|
CounterTorque = Math.Clamp( (signedCombinedBrakeForce - fx) * mRadius, -maxCounterTorque, maxCounterTorque );
|
|
|
|
sx = (vx - AngularVelocity * mRadius) / clampedAbsForwardSpeed;
|
|
sx *= slipLoadModifier;
|
|
|
|
sy = MathF.Atan2( vy, clampedAbsForwardSpeed );
|
|
sy *= slipLoadModifier;
|
|
|
|
float sideSlipSign = sy > 0 ? 1 : -1;
|
|
float absSideSlip = Math.Abs( sy );
|
|
float peakSideFrictionForce = 18000 * (1 - MathF.Exp( -0.0001f * sideLoadFactor ));
|
|
|
|
float sideForce = -sideSlipSign * Tire.Evaluate( absSideSlip ) * peakSideFrictionForce;
|
|
fy = Math.Clamp( sideForce, -sideForceClamp, sideForceClamp );
|
|
|
|
// Calculate effect of camber on friction
|
|
float camberFrictionCoeff = Math.Max( 0, Vehicle.WorldRotation.Up.Dot( ContactNormal ) );
|
|
fy *= camberFrictionCoeff;
|
|
|
|
if ( IsOnGround && absForwardSpeed < 0.12f && absSideSpeed < 0.12f )
|
|
{
|
|
float verticalOffset = RestLength + mRadius;
|
|
var transformPosition = WorldPosition;
|
|
|
|
var transformUp = TransformRotationSteer.Up;
|
|
|
|
currentPosition.x = transformPosition.x - transformUp.x * verticalOffset;
|
|
currentPosition.y = transformPosition.y - transformUp.y * verticalOffset;
|
|
currentPosition.z = transformPosition.z - transformUp.z * verticalOffset;
|
|
|
|
if ( !lowSpeedReferenceIsSet )
|
|
{
|
|
lowSpeedReferenceIsSet = true;
|
|
lowSpeedReferencePosition = currentPosition;
|
|
}
|
|
else
|
|
{
|
|
referenceError.x = lowSpeedReferencePosition.x - currentPosition.x;
|
|
referenceError.y = lowSpeedReferencePosition.y - currentPosition.y;
|
|
referenceError.z = lowSpeedReferencePosition.z - currentPosition.z;
|
|
|
|
correctiveForce.x = invDt * LoadContribution * mass * referenceError.x;
|
|
correctiveForce.y = invDt * LoadContribution * mass * referenceError.y;
|
|
correctiveForce.z = invDt * LoadContribution * mass * referenceError.z;
|
|
|
|
if ( wheelIsBlocked && absAngularVelocity < 0.5f )
|
|
{
|
|
fx += correctiveForce.Dot( hitForwardDirection );
|
|
}
|
|
fy += correctiveForce.Dot( hitSidewaysDirection );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
lowSpeedReferenceIsSet = false;
|
|
}
|
|
|
|
fx = Math.Clamp( fx, -peakForwardFrictionForce, peakForwardFrictionForce );
|
|
fy = Math.Clamp( fy, -peakSideFrictionForce, peakSideFrictionForce );
|
|
|
|
if ( absForwardSpeed > 0.01f || absAngularVelocity > 0.01f )
|
|
{
|
|
var f = MathF.Sqrt( fx * fx + fy * fy );
|
|
var d = Math.Abs( new Vector2( sx, sy ).Normal.y );
|
|
fy = f * d * Math.Sign( fy );
|
|
|
|
}
|
|
if ( IsOnGround )
|
|
{
|
|
FrictionForce.x = (hitSidewaysDirection.x * fy + hitForwardDirection.x * fx).MeterToInch();
|
|
FrictionForce.y = (hitSidewaysDirection.y * fy + hitForwardDirection.y * fx).MeterToInch();
|
|
FrictionForce.z = (hitSidewaysDirection.z * fy + hitForwardDirection.z * fx).MeterToInch();
|
|
|
|
//DebugOverlay.Normal( WorldPosition, hitSidewaysDirection * 10, overlay: true, color: Color.Red );
|
|
//DebugOverlay.Normal( WorldPosition, hitForwardDirection * 10, overlay: true, color: Color.Green );
|
|
//DebugOverlay.Normal( WorldPosition, FrictionForce.ClampLength( 30 ), overlay: true, color: Color.Cyan );
|
|
//DebugOverlay.ScreenText( Scene.Camera.PointToScreenPixels( WorldPosition ), $"{ForwardFriction}\nMotor:{(int)motorTorque}\nBrake:{(int)brakeTorque}", flags: TextFlag.LeftTop );
|
|
}
|
|
else
|
|
FrictionForce = Vector3.Zero;
|
|
}
|
|
}
|