velox/Code/Base/Wheel/VeloXWheel.cs

using Sandbox;
using Sandbox.Utility;
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( "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²
	[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); }

	private Vector3 StartPos { get; set; }
	private static Rotation CylinderOffset = Rotation.FromRoll( 90 );

	[Sync] public bool IsOnGround { get; private set; }


	[Property] public float DriveTorque { get; set; }
	[Property] public float BrakeTorque { get; set; }

	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 RollAngle { get; protected set; }    // degrees


	private VeloXBase Vehicle;
	[Sync] public Vector3 ContactNormal { get; protected set; }
	[Sync] public Vector3 ContactPosition { 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() );
		FrictionForce = 0;
	}

	internal void StepPhys( VeloXBase vehicle, in float dt )
	{

		const int numSamples = 3;
		float halfWidth = Width.MeterToInch() * 0.5f;

		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;

			Fz = Math.Max( wheelTraceData.Force / hitCount, 0 );
			Compression = wheelTraceData.Compression / hitCount;
			ContactNormal = (wheelTraceData.ContactNormal / hitCount).Normal;
			ContactPosition = wheelTraceData.ContactPosition / hitCount;
			LastLength = RestLength - Compression;

			UpdateHitVariables();
			UpdateFriction( dt );
		}
		else
		{
			IsOnGround = false;
			Compression = 0f;
			Fz = 0f;
			ContactNormal = Vector3.Up;
			ContactPosition = WorldPosition;
			LastLength = RestLength;

			UpdateHitVariables();
			UpdateFriction( dt );
		}

	}


	private bool TraceWheel( VeloXBase vehicle, ref WheelTraceData wheelTraceData, Vector3 start, Vector3 end, float width, in float dt )
	{
		SceneTraceResult trace;
		if ( IsOnGround && vehicle.TotalSpeed < 550 )
		{
			trace = Scene.Trace
						   .FromTo( start, end )
						   .Cylinder( width, Radius.MeterToInch() )
						   .Rotated( vehicle.WorldRotation * CylinderOffset )
						   .UseHitPosition( false )
						   .IgnoreGameObjectHierarchy( Vehicle.GameObject )
						   .WithCollisionRules( Vehicle.GameObject.Tags )
						   .Run();

			if ( trace.StartedSolid )
			{
				trace = Scene.Trace
								.FromTo( start, end + Vehicle.WorldRotation.Down * Radius.MeterToInch() )
								.UseHitPosition( false )
								.IgnoreGameObjectHierarchy( Vehicle.GameObject )
								.WithCollisionRules( Vehicle.GameObject.Tags )
								.Run();
				trace.EndPosition += Vehicle.WorldRotation.Up * Math.Min( Radius.MeterToInch(), trace.Distance );
			}
		}
		else
		{
			trace = Scene.Trace
						  .FromTo( start, end + Vehicle.WorldRotation.Down * Radius.MeterToInch() )
						  .UseHitPosition( false )
						  .IgnoreGameObjectHierarchy( Vehicle.GameObject )
						  .WithCollisionRules( Vehicle.GameObject.Tags )
						  .Run();
			trace.EndPosition += Vehicle.WorldRotation.Up * Math.Min( Radius.MeterToInch(), trace.Distance );
		}

		//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;

			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;
		if ( AutoSimulate )
			StepPhys( Vehicle, dt );
		StepRotation( Vehicle, dt );
	}

	private void StepRotation( VeloXBase vehicle, in float dt )
	{
		RollAngle += MathX.RadianToDegree( AngularVelocity ) * dt;
		RollAngle = (RollAngle % 360f + 360f) % 360f;
	}
}