velox/Code/Base/Wheel/VeloXWheel.cs

using Sandbox;
using System;
using System.Runtime.Intrinsics.Arm;
using System.Text.RegularExpressions;

namespace VeloX;

[Group( "VeloX" )]
[Title( "VeloX - Wheel" )]
public partial class VeloXWheel : Component
{

	[Property] public float Radius { get; set; } = 15;
	[Property] public float Width { get; set; } = 6;
	[Sync] public float SideSlip { get; private set; }
	[Sync] public float ForwardSlip { get; private set; }
	[Sync, Range( 0, 1 )] public float BrakePower { get; set; }
	[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 Spin { get; private set; }

	public float RPM { get => angularVelocity * 60f / MathF.Tau; set => angularVelocity = value / (60 / MathF.Tau); }
	internal float DistributionFactor { get; set; }

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

	public SceneTraceResult Trace { get; private set; }
	public bool IsOnGround => Trace.Hit;

	private float lastSpringOffset;
	private Vector2 tractionCycle;
	private float angularVelocity;
	private float lastFraction;
	private RealTimeUntil expandSoundCD;
	private RealTimeUntil contractSoundCD;


	protected override void OnAwake()
	{
		base.OnAwake();
		if ( StartPos.IsNearZeroLength )
			StartPos = LocalPosition;
	}

	private static float TractionRamp( float slipAngle, float sideTractionMaxAng, float sideTractionMax, float sideTractionMin )
	{
		sideTractionMaxAng /= 90; // Convert max slip angle to the 0 - 1 range
		var x = (slipAngle - sideTractionMaxAng) / (1 - sideTractionMaxAng);


		return slipAngle < sideTractionMaxAng ? sideTractionMax : (sideTractionMax * (1 - x)) + (sideTractionMin * x);
	}

	private void DoSuspensionSounds( VeloXBase vehicle, float change )
	{
		if ( change > 0.1f && expandSoundCD )
		{
			expandSoundCD = 0.3f;
			var sound = Sound.Play( vehicle.SuspensionUpSound, WorldPosition );
			sound.Volume = Math.Clamp( Math.Abs( change ) * 5f, 0, 0.5f );
		}

		if ( change < -0.1f && contractSoundCD )
		{
			contractSoundCD = 0.3f;
			change = MathF.Abs( change );
			var sound = Sound.Play( change > 0.3f ? vehicle.SuspensionHeavySound : vehicle.SuspensionDownSound, WorldPosition );
			sound.Volume = Math.Clamp( change * 5f, 0, 1 );
		}
	}

	internal void Update( VeloXBase vehicle, in float dt )
	{
		UpdateVisuals( vehicle, dt );

		if ( !IsOnGround )
		{
			angularVelocity += (Torque / 20) * dt;
			angularVelocity = MathX.Approach( angularVelocity, 0, dt * 4 );
		}

	}

	private void UpdateVisuals( VeloXBase vehicle, in float dt )
	{
		//Rotate the wheel around the axle axis
		Spin = (Spin - MathX.RadianToDegree( angularVelocity ) * dt) % 360;

		WorldRotation = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier ).RotateAroundAxis( Vector3.Right, Spin );

	}

	public void DoPhysics( VeloXBase vehicle, ref Vector3 vehVel, ref Vector3 vehAngVel, ref Vector3 outLin, ref Vector3 outAng, in float dt )
	{

		var pos = vehicle.WorldTransform.PointToWorld( StartPos );

		var ang = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier );

		var fw = ang.Forward;
		var rt = ang.Right;
		var up = ang.Up;

		var maxLen = SuspensionLength;

		var endPos = pos + ang.Down * maxLen;
		Trace = Scene.Trace.IgnoreGameObjectHierarchy( vehicle.GameObject )
					.FromTo( pos, endPos )
					.Cylinder( Width, Radius )
					.Rotated( vehicle.WorldTransform.Rotation * CylinderOffset )
					.UseRenderMeshes( false )
					.UseHitPosition( false )
					.WithoutTags( vehicle.WheelIgnoredTags )
					.Run();

		var fraction = Trace.Fraction;

		var contactPos = pos - maxLen * fraction * up;

		LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );

		DoSuspensionSounds( vehicle, fraction - lastFraction );
		lastFraction = fraction;

		if ( !IsOnGround )
		{
			SideSlip = 0;
			ForwardSlip = 0;
			return;
		}
		var normal = Trace.Normal;

		var vel = vehicle.Body.GetVelocityAtPoint( pos );
		// Split that velocity among our local directions

		var velF = fw.Dot( vel );

		var velR = rt.Dot( vel );

		var velU = normal.Dot( vel );

		var absVelR = Math.Abs( velR );


		//Make forward forces be perpendicular to the surface normal

		fw = normal.Cross( rt );

		//Suspension spring force &damping

		var offset = maxLen - (fraction * maxLen);

		var springForce = (offset * SpringStrength);

		var damperForce = (lastSpringOffset - offset) * SpringDamper;
		lastSpringOffset = offset;
		var force = (springForce - damperForce) * up.Dot( normal ) * normal;

		//If the suspension spring is going to be fully compressed on the next frame...

		if ( velU < 0 && offset + Math.Abs( velU * dt ) > SuspensionLength )
		{
			var (linearVel, angularVel) = vehicle.Body.PhysicsBody.CalculateVelocityOffset( (-velU / dt) * normal, pos );
			vehVel += linearVel;
			vehAngVel += angularVel;
		}

		//Rolling resistance
		force += 0.05f * -velF * fw;

		//Brake and torque forces
		var surfaceGrip = 1;

		var maxTraction = ForwardTractionMax * surfaceGrip * 1;
		//Grip loss logic

		var brakeForce = MathX.Clamp( -velF, -Brake, Brake ) * BrakePowerMax * surfaceGrip;

		var forwardForce = Torque + brakeForce;

		var signForwardForce = forwardForce > 0 ? 1 : (forwardForce < 0 ? -1 : 0);

		// Given an amount of sideways slippage( up to the max.traction )
		// and the forward force, calculate how much grip we are losing.

		tractionCycle.x = Math.Min( absVelR, maxTraction );
		tractionCycle.y = forwardForce;

		var gripLoss = Math.Max( tractionCycle.Length - maxTraction, 0 );


		// Reduce the forward force by the amount of grip we lost,

		// but still allow some amount of brake force to apply regardless.

		forwardForce += -(gripLoss * signForwardForce) + MathX.Clamp( brakeForce * 0.5f, -maxTraction, maxTraction );

		force += fw * forwardForce;

		// Get how fast the wheel would be spinning if it had never lost grip

		var groundAngularVelocity = MathF.Tau * (velF / (Radius * MathF.Tau));

		// Add our grip loss to our spin velocity
		var _angvel = groundAngularVelocity + gripLoss * (Torque > 0 ? 1 : (Torque < 0 ? -1 : 0));

		// Smoothly match our current angular velocity to the angular velocity affected by grip loss

		angularVelocity = MathX.Approach( angularVelocity, _angvel, dt * 200 );
		ForwardSlip = groundAngularVelocity - angularVelocity;

		// Calculate side slip angle
		var slipAngle = MathF.Atan2( velR, MathF.Abs( velF ) ) / MathF.PI * 2;
		SideSlip = slipAngle * MathX.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 ) * 2;

		//Sideways traction ramp

		slipAngle = MathF.Abs( slipAngle * slipAngle );

		maxTraction = TractionRamp( slipAngle, SideTractionMaxAng, SideTractionMax, SideTractionMin );

		var sideForce = -rt.Dot( vel * SideTractionMultiplier );

		// Reduce sideways traction force as the wheel slips forward
		sideForce *= 1 - Math.Clamp( MathF.Abs( gripLoss ) * 0.1f, 0, 1 ) * 0.9f;

		// Apply sideways traction force
		force += Math.Clamp( sideForce, -maxTraction, maxTraction ) * surfaceGrip * rt;

		force += velR * SideTractionMultiplier * -0.1f * rt;
		//Apply the forces at the axle / ground contact position

		vehicle.Body.ApplyForceAt( pos, force / dt );


	}
}