275 lines
9.0 KiB
C#
275 lines
9.0 KiB
C#
using Sandbox;
|
|
using System;
|
|
|
|
namespace VeloX;
|
|
|
|
[Group( "VeloX" )]
|
|
[Title( "VeloX - Wheel" )]
|
|
public partial class VeloXWheel : Component
|
|
{
|
|
[Property, Group( "Suspension" )] public float Radius { get; set; } = 0.35f;
|
|
[Property, Group( "Suspension" )] public float Width { get; set; } = 0.1f;
|
|
[Property] public float Mass { get; set; } = 5;
|
|
[Property, Group( "Suspension" )] float RestLength { get; set; } = 0.22f;
|
|
[Property, Group( "Suspension" )] public float SpringStiffness { get; set; } = 20000.0f;
|
|
[Property, Group( "Suspension" )] float ReboundStiffness { get; set; } = 2200;
|
|
[Property, Group( "Suspension" )] float CompressionStiffness { get; set; } = 2400;
|
|
[Property, Group( "Suspension" )] float BumpStopStiffness { get; set; } = 5000;
|
|
|
|
[Property, Group( "Traction" )] public TirePreset Tire { get; set; } = ResourceLibrary.Get<TirePreset>( "frictions/default.tire" );
|
|
[Property, Group( "Traction" )] public float SurfaceGrip { get; set; } = 1f;
|
|
[Property, Group( "Traction" )] public float SurfaceResistance { get; set; } = 0.05f;
|
|
public bool AutoSimulate = true;
|
|
|
|
|
|
public float BaseInertia => Mass * (Radius * Radius); // kg·m²
|
|
[Property] public float Inertia { get; set; } = 1.5f; // kg·m²
|
|
public float SideSlip => SlipAngle;
|
|
public float ForwardSlip => DynamicSlipRatio;
|
|
[Sync] public float Torque { get; set; }
|
|
[Sync, Range( 0, 1 )] public float Brake { get; set; }
|
|
[Property] public bool IsFront { get; protected set; }
|
|
[Property] public float SteerMultiplier { get; set; }
|
|
|
|
public float RPM { get => AngularVelocity * 60f / MathF.Tau; set => AngularVelocity = value / (60 / MathF.Tau); }
|
|
[Sync] internal float DistributionFactor { get; set; }
|
|
|
|
private Vector3 StartPos { get; set; }
|
|
private static Rotation CylinderOffset = Rotation.FromRoll( 90 );
|
|
|
|
[Sync] public bool IsOnGround { get; private set; }
|
|
|
|
|
|
[Property] public float DriveTorque => Torque;
|
|
[Property] public float BrakeTorque => Brake * 5000f;
|
|
|
|
public float Compression { get; protected set; } // meters
|
|
public float LastLength { get; protected set; } // meters
|
|
public float Fz { get; protected set; } // N
|
|
public float AngularVelocity { get; protected set; } // rad/s
|
|
public float Fx { get; protected set; } // N
|
|
public float Fy { get; protected set; } // N
|
|
public float RollAngle { get; protected set; } // degrees
|
|
|
|
|
|
private VeloXBase Vehicle;
|
|
[Sync] public Vector3 ContactNormal { get; protected set; }
|
|
[Sync] public Vector3 ContactPosition { get; protected set; }
|
|
public float SlipRatio { get; protected set; }
|
|
public float SlipAngle { get; protected set; }
|
|
public float DynamicSlipRatio { get; protected set; }
|
|
public float DynamicSlipAngle { get; protected set; }
|
|
Rotation TransformRotationSteer => Vehicle.WorldTransform.RotationToWorld( Vehicle.SteerAngle * SteerMultiplier );
|
|
|
|
protected override void OnAwake()
|
|
{
|
|
Vehicle = Components.Get<VeloXBase>( FindMode.EverythingInSelfAndAncestors );
|
|
base.OnAwake();
|
|
if ( StartPos.IsNearZeroLength )
|
|
StartPos = LocalPosition;
|
|
Inertia = BaseInertia;
|
|
}
|
|
|
|
internal void Update( VeloXBase vehicle, in float dt )
|
|
{
|
|
UpdateVisuals( vehicle, dt );
|
|
}
|
|
|
|
private void UpdateVisuals( VeloXBase vehicle, in float dt )
|
|
{
|
|
WorldRotation = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier ).RotateAroundAxis( Vector3.Right, -RollAngle );
|
|
LocalPosition = StartPos + Vector3.Down * LastLength.MeterToInch();
|
|
}
|
|
|
|
private struct WheelTraceData
|
|
{
|
|
internal Vector3 ContactNormal;
|
|
internal Vector3 ContactPosition;
|
|
internal float Compression;
|
|
internal float Force;
|
|
}
|
|
public void UpdateForce()
|
|
{
|
|
Vehicle.Body.ApplyForceAt( ContactPosition, FrictionForce + ContactNormal * Fz.MeterToInch() );
|
|
}
|
|
|
|
internal void StepPhys( VeloXBase vehicle, in float dt )
|
|
{
|
|
|
|
var _rigidbody = vehicle.Body;
|
|
const int numSamples = 3;
|
|
float halfWidth = Width.MeterToInch() * 0.5f;
|
|
|
|
var ang = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier );
|
|
|
|
Vector3 right = Vector3.VectorPlaneProject( ang.Right, Vector3.Up ).Normal;
|
|
|
|
|
|
int hitCount = 0;
|
|
WheelTraceData wheelTraceData = new();
|
|
for ( int i = 0; i < numSamples; i++ )
|
|
{
|
|
float t = (float)i / (numSamples - 1);
|
|
float offset = MathX.Lerp( -halfWidth, halfWidth, t );
|
|
Vector3 start = vehicle.WorldTransform.PointToWorld( StartPos + Vector3.Right * offset );
|
|
Vector3 end = start + vehicle.WorldRotation.Down * RestLength.MeterToInch();
|
|
if ( TraceWheel( vehicle, ref wheelTraceData, start, end, Width.MeterToInch() / numSamples, dt ) )
|
|
hitCount++;
|
|
}
|
|
|
|
|
|
if ( hitCount > 0 )
|
|
{
|
|
|
|
IsOnGround = true;
|
|
|
|
//// Average all contacts
|
|
Fz = Math.Max( wheelTraceData.Force / hitCount, 0 );
|
|
Compression = wheelTraceData.Compression / hitCount;
|
|
ContactNormal = (wheelTraceData.ContactNormal / hitCount).Normal;
|
|
ContactPosition = wheelTraceData.ContactPosition / hitCount;
|
|
//DoSuspensionSounds( vehicle, (RestLength - Compression) * 0.8f);
|
|
LastLength = RestLength - Compression;
|
|
|
|
//DebugOverlay.Normal( ContactPosition, ContactNormal * Fz / 1000 );
|
|
// Apply suspension force
|
|
//_rigidbody.ApplyForceAt( ContactPosition, ContactNormal * Fz.MeterToInch() );
|
|
|
|
UpdateHitVariables();
|
|
// Friction
|
|
|
|
Vector3 forward = ContactNormal.Cross( right ).Normal;
|
|
right = Rotation.FromAxis( ContactNormal, 90f ) * forward;
|
|
|
|
var velAtContact = _rigidbody.GetVelocityAtPoint( ContactPosition + vehicle.Body.MassCenter ) * 0.0254f;
|
|
float vx = Vector3.Dot( velAtContact, forward );
|
|
float vy = Vector3.Dot( velAtContact, right );
|
|
float wheelLinearVel = AngularVelocity * Radius;
|
|
|
|
float creepVel = 0.5f;
|
|
SlipRatio = (wheelLinearVel - vx) / (MathF.Abs( vx ) + creepVel);
|
|
SlipAngle = MathX.RadianToDegree( MathF.Atan2( vy, MathF.Abs( vx ) + creepVel ) );
|
|
|
|
float latCoeff = 1.0f - MathF.Exp( -MathF.Max( MathF.Abs( vx ), 1f ) * dt / 0.01f );
|
|
float longCoeff = 1.0f - MathF.Exp( -MathF.Max( MathF.Abs( vx ), 1f ) * dt / 0.01f );
|
|
|
|
DynamicSlipAngle += (SlipAngle - DynamicSlipAngle) * latCoeff;
|
|
DynamicSlipRatio += (SlipRatio - DynamicSlipRatio) * longCoeff;
|
|
|
|
|
|
UpdateFriction( dt );
|
|
}
|
|
else
|
|
{
|
|
IsOnGround = false;
|
|
// Wheel is off the ground
|
|
Compression = 0f;
|
|
Fz = 0f;
|
|
Fx = 0f;
|
|
Fy = 0f;
|
|
ContactNormal = Vector3.Up;
|
|
ContactPosition = WorldPosition;
|
|
}
|
|
|
|
}
|
|
|
|
const string playerTag = "player";
|
|
|
|
private bool TraceWheel( VeloXBase vehicle, ref WheelTraceData wheelTraceData, Vector3 start, Vector3 end, float width, in float dt )
|
|
{
|
|
|
|
var trace = Scene.Trace
|
|
.FromTo( start, end )
|
|
.Cylinder( width, Radius.MeterToInch() )
|
|
.Rotated( vehicle.WorldRotation * CylinderOffset )
|
|
.UseHitPosition( false )
|
|
.IgnoreGameObjectHierarchy( Vehicle.GameObject )
|
|
.WithoutTags( playerTag )
|
|
.Run();
|
|
|
|
//DebugOverlay.Trace( trace, overlay: true );
|
|
if ( trace.Hit )
|
|
{
|
|
|
|
Vector3 contactPos = trace.EndPosition;
|
|
Vector3 contactNormal = trace.Normal;
|
|
float currentLength = trace.Distance.InchToMeter();
|
|
float compression = (RestLength - currentLength).Clamp( -RestLength, RestLength );
|
|
|
|
// Nonlinear spring
|
|
float springForce = SpringStiffness * compression * (1f + 2f * compression / RestLength);
|
|
|
|
// Damping
|
|
float damperVelocity = (LastLength - currentLength) / dt;
|
|
float damperForce = damperVelocity > 0 ? damperVelocity * ReboundStiffness : damperVelocity * CompressionStiffness;
|
|
|
|
float FzPoint = springForce + damperForce;
|
|
|
|
// Bump stop
|
|
float minCompression = -0.05f;
|
|
if ( compression <= minCompression ) FzPoint += (minCompression - compression) * BumpStopStiffness;
|
|
|
|
wheelTraceData.ContactNormal += contactNormal;
|
|
wheelTraceData.ContactPosition += contactPos;
|
|
wheelTraceData.Compression += compression;
|
|
wheelTraceData.Force += Math.Max( 0, FzPoint );
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
public void DoPhysics( in float dt )
|
|
{
|
|
if ( IsProxy )
|
|
return;
|
|
StepRotation( Vehicle, dt );
|
|
if ( AutoSimulate )
|
|
StepPhys( Vehicle, dt );
|
|
}
|
|
|
|
private void StepRotation( VeloXBase vehicle, in float dt )
|
|
{
|
|
float inertia = MathF.Max( 1f, Inertia );
|
|
|
|
float roadTorque = Fx * Radius;
|
|
float externalTorque = DriveTorque - roadTorque;
|
|
float rollingResistanceTorque = Fz * Radius * SurfaceResistance;
|
|
|
|
const float HubCoulombNm = 20f;
|
|
const float HubViscous = 0.1f;
|
|
|
|
float coulombTorque = BrakeTorque + rollingResistanceTorque + HubCoulombNm;
|
|
|
|
float omega = AngularVelocity;
|
|
|
|
if ( MathF.Abs( omega ) < 1e-6f && MathF.Abs( externalTorque ) <= coulombTorque )
|
|
{
|
|
AngularVelocity = 0f;
|
|
}
|
|
else
|
|
{
|
|
|
|
// viscous decay
|
|
if ( HubViscous > 0f ) omega *= MathF.Exp( -(HubViscous / inertia) * dt );
|
|
|
|
// Coulomb drag
|
|
if ( coulombTorque > 0f && omega != 0f )
|
|
{
|
|
float dir = MathF.Sign( omega );
|
|
float deltaOmega = (coulombTorque / inertia) * dt;
|
|
if ( deltaOmega >= MathF.Abs( omega ) ) omega = 0f;
|
|
else omega -= dir * deltaOmega;
|
|
}
|
|
|
|
if ( MathF.Abs( omega ) < 0.01f ) omega = 0f;
|
|
}
|
|
AngularVelocity = omega; // wider sanity range
|
|
|
|
RollAngle += MathX.RadianToDegree( AngularVelocity ) * dt;
|
|
RollAngle = (RollAngle % 360f + 360f) % 360f;
|
|
}
|
|
}
|