297 lines
12 KiB
C#
297 lines
12 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 Friction ForwardFriction = new();
|
|
public Friction SidewayFriction = new();
|
|
public Vector3 FrictionForce;
|
|
|
|
|
|
private Vector3 hitContactVelocity;
|
|
private Vector3 hitForwardDirection;
|
|
private Vector3 hitSidewaysDirection;
|
|
|
|
public Vector3 ContactRight => hitSidewaysDirection;
|
|
public Vector3 ContactForward => hitForwardDirection;
|
|
|
|
[Sync]
|
|
public float LongitudinalSlip
|
|
{
|
|
get => ForwardFriction.Slip;
|
|
private set => ForwardFriction.Slip = value;
|
|
}
|
|
|
|
public float LongitudinalSpeed => ForwardFriction.Speed;
|
|
public bool IsSkiddingLongitudinally => NormalizedLongitudinalSlip > 0.35f;
|
|
public float NormalizedLongitudinalSlip => Math.Clamp( Math.Abs( LongitudinalSlip ), 0, 1 );
|
|
|
|
|
|
[Sync]
|
|
public float LateralSlip
|
|
{
|
|
get => SidewayFriction.Slip;
|
|
private set => SidewayFriction.Slip = value;
|
|
}
|
|
|
|
public float LateralSpeed => SidewayFriction.Speed;
|
|
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;
|
|
|
|
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;
|
|
|
|
ForwardFriction.Speed = hitContactVelocity.Dot( hitForwardDirection ).InchToMeter();
|
|
SidewayFriction.Speed = hitContactVelocity.Dot( hitSidewaysDirection ).InchToMeter();
|
|
}
|
|
else
|
|
{
|
|
ForwardFriction.Speed = 0f;
|
|
SidewayFriction.Speed = 0f;
|
|
}
|
|
}
|
|
|
|
|
|
private Vector3 lowSpeedReferencePosition;
|
|
private bool lowSpeedReferenceIsSet;
|
|
private Vector3 currentPosition;
|
|
private Vector3 referenceError;
|
|
private Vector3 correctiveForce;
|
|
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;
|
|
//DebugOverlay.Text( WorldPosition, SidewayFriction.Speed.ToString(), overlay: true );
|
|
|
|
|
|
float mass = Vehicle.Body.Mass;
|
|
float absForwardSpeed = Math.Abs( ForwardFriction.Speed );
|
|
float forwardForceClamp = mass * LoadContribution * absForwardSpeed * invDt;
|
|
float absSideSpeed = Math.Abs( SidewayFriction.Speed );
|
|
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 );
|
|
|
|
// Calculate effect of camber on friction
|
|
float camberFrictionCoeff = Math.Max( 0, Vehicle.WorldRotation.Up.Dot( ContactNormal ) );
|
|
|
|
float peakForwardFrictionForce = forwardLoadFactor;
|
|
float absCombinedBrakeTorque = Math.Max( 0, brakeTorque + RollingResistanceTorque );
|
|
|
|
float signedCombinedBrakeTorque = absCombinedBrakeTorque * -Math.Sign( ForwardFriction.Speed );
|
|
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;
|
|
ForwardFriction.Force = forwardInputForce > maxForwardForce ? maxForwardForce
|
|
: forwardInputForce < -maxForwardForce ? -maxForwardForce : forwardInputForce;
|
|
|
|
bool 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 = ForwardFriction.Speed * 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 += ForwardFriction.Speed > 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 - ForwardFriction.Force) * mRadius, -maxCounterTorque, maxCounterTorque );
|
|
|
|
|
|
ForwardFriction.Slip = (ForwardFriction.Speed - AngularVelocity * mRadius) / clampedAbsForwardSpeed;
|
|
ForwardFriction.Slip *= slipLoadModifier;
|
|
|
|
SidewayFriction.Slip = MathF.Atan2( SidewayFriction.Speed, clampedAbsForwardSpeed );
|
|
SidewayFriction.Slip *= slipLoadModifier;
|
|
|
|
float sideSlipSign = SidewayFriction.Slip > 0 ? 1 : -1;
|
|
float absSideSlip = Math.Abs( SidewayFriction.Slip );
|
|
float peakSideFrictionForce = sideLoadFactor;
|
|
|
|
float sideForce = -sideSlipSign * Tire.Evaluate( absSideSlip ) * sideLoadFactor;
|
|
SidewayFriction.Force = Math.Clamp( sideForce, -sideForceClamp, sideForceClamp );
|
|
SidewayFriction.Force *= 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 )
|
|
{
|
|
ForwardFriction.Force += correctiveForce.Dot( hitForwardDirection );
|
|
}
|
|
SidewayFriction.Force += correctiveForce.Dot( hitSidewaysDirection );
|
|
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
lowSpeedReferenceIsSet = false;
|
|
}
|
|
|
|
ForwardFriction.Force = Math.Clamp( ForwardFriction.Force * Tire.Evaluate( Math.Abs( ForwardFriction.Slip ) ), -peakForwardFrictionForce, peakForwardFrictionForce );
|
|
|
|
SidewayFriction.Force = Math.Clamp( SidewayFriction.Force, -peakSideFrictionForce, peakSideFrictionForce );
|
|
|
|
if ( absForwardSpeed > 2f || absAngularVelocity > 4f )
|
|
{
|
|
|
|
float forwardSlipPercent = ForwardFriction.Slip / Tire.GetPeakSlip();
|
|
float sideSlipPercent = SidewayFriction.Slip / Tire.GetPeakSlip();
|
|
float slipCircleLimit = MathF.Sqrt( forwardSlipPercent * forwardSlipPercent + sideSlipPercent * sideSlipPercent );
|
|
if ( slipCircleLimit > 1f )
|
|
{
|
|
float beta = MathF.Atan2( sideSlipPercent, forwardSlipPercent * 0.9f );
|
|
float sinBeta = MathF.Sin( beta );
|
|
float cosBeta = MathF.Cos( beta );
|
|
|
|
float absForwardForce = ForwardFriction.Force < 0 ? -ForwardFriction.Force : ForwardFriction.Force;
|
|
|
|
float absSideForce = SidewayFriction.Force < 0 ? -SidewayFriction.Force : SidewayFriction.Force;
|
|
float f = absForwardForce * cosBeta * cosBeta + absSideForce * sinBeta * sinBeta;
|
|
|
|
ForwardFriction.Force = 0.5f * ForwardFriction.Force - f * cosBeta;
|
|
SidewayFriction.Force = 0.5f * SidewayFriction.Force - f * sinBeta;
|
|
}
|
|
}
|
|
|
|
if ( IsOnGround )
|
|
{
|
|
FrictionForce.x = (hitSidewaysDirection.x * SidewayFriction.Force + hitForwardDirection.x * ForwardFriction.Force).MeterToInch();
|
|
FrictionForce.y = (hitSidewaysDirection.y * SidewayFriction.Force + hitForwardDirection.y * ForwardFriction.Force).MeterToInch();
|
|
FrictionForce.z = (hitSidewaysDirection.z * SidewayFriction.Force + hitForwardDirection.z * ForwardFriction.Force).MeterToInch();
|
|
//DebugOverlay.Normal( WorldPosition, hitSidewaysDirection * 10, overlay: true );
|
|
//DebugOverlay.Normal( WorldPosition, hitForwardDirection * 10, overlay: true );
|
|
//DebugOverlay.Normal( WorldPosition, FrictionForce / 100, overlay: true );
|
|
//DebugOverlay.Normal( ContactPosition, Vector3.Up * AngularVelocity, overlay: true );
|
|
|
|
//DebugOverlay.Sphere( new( ContactPosition, 4 ), overlay: true );
|
|
//Vehicle.Body.ApplyForceAt( ContactPosition, FrictionForce );
|
|
|
|
}
|
|
else
|
|
FrictionForce = Vector3.Zero;
|
|
}
|
|
}
|