317 lines
9.1 KiB
C#
317 lines
9.1 KiB
C#
using Sandbox;
|
|
using Sandbox.Rendering;
|
|
using Sandbox.Services;
|
|
using Sandbox.UI;
|
|
using System;
|
|
using System.Collections.Specialized;
|
|
using System.Diagnostics.Metrics;
|
|
using System.Numerics;
|
|
using System.Text.RegularExpressions;
|
|
using System.Threading;
|
|
using static Sandbox.CameraComponent;
|
|
using static Sandbox.Package;
|
|
using static Sandbox.SkinnedModelRenderer;
|
|
|
|
namespace VeloX;
|
|
|
|
[Group( "VeloX" )]
|
|
[Title( "VeloX - Wheel" )]
|
|
public partial class VeloXWheel : Component
|
|
{
|
|
|
|
[Property] public float Radius { get; set; } = 15;
|
|
[Property] public float Mass { get; set; } = 20;
|
|
[Property] public float RollingResistance { get; set; } = 20;
|
|
[Property] public float SlipCircleShape { get; set; } = 1.05f;
|
|
[Property] public TirePreset TirePreset { get; set; }
|
|
[Property] public float Width { get; set; } = 6;
|
|
public float SideSlip { get; private set; }
|
|
public float ForwardSlip { get; private set; }
|
|
[Sync] public float Torque { get; set; }
|
|
[Sync, Range( 0, 1 )] public float Brake { get; set; }
|
|
[Property] float BrakePowerMax { get; set; } = 3000;
|
|
[Property] public bool IsFront { get; protected set; }
|
|
[Property] public float SteerMultiplier { get; set; }
|
|
[Property] public float CasterAngle { get; set; } = 7; // todo
|
|
[Property] public float CamberAngle { get; set; } = -3;
|
|
[Property] public float ToeAngle { get; set; } = 0.5f;
|
|
[Property] public float Ackermann { get; set; } = 0;
|
|
|
|
[Property, Group( "Suspension" )] float SuspensionLength { get; set; } = 10;
|
|
[Property, Group( "Suspension" )] float SpringStrength { get; set; } = 800;
|
|
[Property, Group( "Suspension" )] float SpringDamper { get; set; } = 3000;
|
|
|
|
[Property] public bool AutoPhysics { get; set; } = true;
|
|
|
|
public float Spin { get; private set; }
|
|
|
|
public float RPM { get => angularVelocity * 30f / MathF.PI; set => angularVelocity = value / (30f / MathF.PI); }
|
|
public float AngularVelocity { get => angularVelocity; set => angularVelocity = value; }
|
|
|
|
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 float angularVelocity;
|
|
private float load;
|
|
private float lastFraction;
|
|
|
|
private Vector3 contactPos;
|
|
private Vector3 forward;
|
|
private Vector3 right;
|
|
private Vector3 up;
|
|
|
|
private float forwardFriction;
|
|
private float sideFriction;
|
|
private Vector3 force;
|
|
public float CounterTorque { get; private set; }
|
|
|
|
internal float BaseInertia => 0.5f * Mass * MathF.Pow( Radius.InchToMeter(), 2 );
|
|
public float Inertia
|
|
{
|
|
get => BaseInertia + inertia;
|
|
set => inertia = value;
|
|
}
|
|
|
|
|
|
protected override void OnAwake()
|
|
{
|
|
base.OnAwake();
|
|
if ( StartPos.IsNearZeroLength )
|
|
StartPos = LocalPosition;
|
|
}
|
|
|
|
internal void Update( VeloXBase vehicle, in float dt )
|
|
{
|
|
UpdateVisuals( vehicle, dt );
|
|
}
|
|
|
|
private void UpdateVisuals( VeloXBase vehicle, in float dt )
|
|
{
|
|
Spin -= angularVelocity.RadianToDegree() * dt;
|
|
WorldRotation = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) ) * Rotation.FromAxis( Vector3.Right, Spin );
|
|
}
|
|
|
|
private Rotation GetSteer( float steer )
|
|
{
|
|
|
|
float angle = (-steer * SteerMultiplier).DegreeToRadian();
|
|
|
|
float t = MathF.Tan( (MathF.PI / 2) - angle ) - Ackermann;
|
|
float steering_angle = MathF.CopySign( float.Pi / 2, t ) - MathF.Atan( t );
|
|
var steering_axis = Vector3.Up * MathF.Cos( -CasterAngle.DegreeToRadian() ) +
|
|
Vector3.Right * MathF.Sin( -CasterAngle.DegreeToRadian() );
|
|
|
|
return Rotation.FromAxis( Vector3.Forward, -CamberAngle ) * Rotation.FromAxis( steering_axis, steering_angle.RadianToDegree() );
|
|
}
|
|
|
|
|
|
private static float GetLongitudinalLoadCoefficient( float load ) => 11000 * (1 - MathF.Exp( -0.00014f * load ));
|
|
private static float GetLateralLoadCoefficient( float load ) => 18000 * (1 - MathF.Exp( -0.0001f * load ));
|
|
private float inertia;
|
|
|
|
|
|
private (float, float, float, float) StepLongitudinal( float Tm, float Tb, float Vx, float W, float Lc, float R, float I )
|
|
{
|
|
float wInit = W;
|
|
float vxAbs = Math.Abs( Vx );
|
|
float Sx;
|
|
if ( Lc < 0.01f )
|
|
{
|
|
Sx = 0;
|
|
}
|
|
else if ( vxAbs >= 0.01f )
|
|
{
|
|
Sx = (W * R - Vx) / vxAbs;
|
|
}
|
|
else
|
|
{
|
|
Sx = (W * R - Vx) * 0.6f;
|
|
}
|
|
|
|
Sx = Math.Clamp( Sx, -1, 1 );
|
|
|
|
W += Tm / I * Time.Delta;
|
|
|
|
Tb *= W > 0 ? -1 : 1;
|
|
|
|
float tbCap = Math.Abs( W ) * I / Time.Delta;
|
|
float tbr = Math.Abs( Tb ) - Math.Abs( tbCap );
|
|
tbr = Math.Max( tbr, 0 );
|
|
|
|
Tb = Math.Clamp( Tb, -tbCap, tbCap );
|
|
|
|
W += Tb / I * Time.Delta;
|
|
|
|
float maxTorque = TirePreset.Pacejka.PacejkaFx( Math.Abs( Sx ) ) * Lc * R;
|
|
|
|
float errorTorque = (W - Vx / R) * I / Time.Delta;
|
|
float surfaceTorque = Math.Clamp( errorTorque, -maxTorque, maxTorque );
|
|
|
|
W -= surfaceTorque / I * Time.Delta;
|
|
float Fx = surfaceTorque / R;
|
|
|
|
tbr *= (W > 0 ? -1 : 1);
|
|
float TbCap2 = Math.Abs( W ) * I / Time.Delta;
|
|
float Tb2 = Math.Clamp( tbr, -TbCap2, TbCap2 );
|
|
W += Tb2 / I * Time.Delta;
|
|
|
|
float deltaOmegaTorque = (W - wInit) * I / Time.Delta;
|
|
float Tcnt = -surfaceTorque + Tb + Tb2 - deltaOmegaTorque;
|
|
|
|
return (W, Sx, Fx, Tcnt);
|
|
}
|
|
|
|
private void StepLateral( float Vx, float Vy, float Lc, out float Sy, out float Fy )
|
|
{
|
|
float VxAbs = Math.Abs( Vx );
|
|
|
|
if ( Lc < 0.01f )
|
|
{
|
|
Sy = 0;
|
|
}
|
|
else if ( VxAbs > 0.1f )
|
|
{
|
|
|
|
Sy = MathX.RadianToDegree( MathF.Atan( Vy / VxAbs ) ) / 50;
|
|
}
|
|
else
|
|
{
|
|
|
|
Sy = Vy * (0.003f / Time.Delta);
|
|
}
|
|
|
|
Sy = Math.Clamp( Sy, -1, 1 );
|
|
float slipSign = Sy < 0 ? -1 : 1;
|
|
Fy = -slipSign * TirePreset.Pacejka.PacejkaFy( Math.Abs( Sy ) ) * Lc;
|
|
}
|
|
|
|
private void SlipCircle( float Sx, float Sy, float Fx, ref float Fy )
|
|
{
|
|
float SxAbs = Math.Abs( Sx );
|
|
if ( SxAbs > 0.01f )
|
|
{
|
|
float SxClamped = Math.Clamp( Sx, -1, 1 );
|
|
float SyClamped = Math.Clamp( Sy, -1, 1 );
|
|
Vector2 combinedSlip = new( SxClamped * 1.05f, SyClamped );
|
|
Vector2 slipDir = combinedSlip.Normal;
|
|
|
|
float F = MathF.Sqrt( Fx * Fx + Fy * Fy );
|
|
float absSlipDirY = Math.Abs( slipDir.y );
|
|
|
|
Fy = F * absSlipDirY * (Fy < 0 ? -1 : 1);
|
|
}
|
|
}
|
|
|
|
public void DoPhysics( VeloXBase vehicle )
|
|
{
|
|
var pos = vehicle.WorldTransform.PointToWorld( StartPos );
|
|
|
|
var ang = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) );
|
|
|
|
var maxLen = SuspensionLength;
|
|
|
|
var endPos = pos + ang.Down * maxLen;
|
|
Trace = Scene.Trace
|
|
.IgnoreGameObjectHierarchy( vehicle.GameObject )
|
|
.Cylinder( Width, Radius, pos, endPos )
|
|
.Rotated( vehicle.WorldTransform.Rotation * CylinderOffset )
|
|
.UseRenderMeshes( false )
|
|
.UseHitPosition( true )
|
|
.WithoutTags( vehicle.WheelIgnoredTags )
|
|
.Run();
|
|
|
|
forward = Vector3.VectorPlaneProject( ang.Forward, Trace.Normal );
|
|
right = Vector3.VectorPlaneProject( ang.Right, Trace.Normal );
|
|
|
|
var fraction = Trace.Fraction;
|
|
|
|
contactPos = pos - maxLen * fraction * ang.Up;
|
|
|
|
LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );
|
|
|
|
DoSuspensionSounds( vehicle, fraction - lastFraction );
|
|
lastFraction = fraction;
|
|
|
|
if ( !IsOnGround )
|
|
return;
|
|
|
|
var vel = vehicle.Body.GetVelocityAtPoint( contactPos );
|
|
|
|
var offset = maxLen - (fraction * maxLen);
|
|
var springForce = offset * SpringStrength;
|
|
var damperForce = (lastSpringOffset - offset) * SpringDamper;
|
|
|
|
lastSpringOffset = offset;
|
|
|
|
|
|
var velU = Trace.Normal.Dot( vel );
|
|
|
|
if ( velU < 0 && offset + Math.Abs( velU * Time.Delta ) > SuspensionLength )
|
|
{
|
|
vehicle.Body.CalculateVelocityOffset( -velU / Time.Delta * Trace.Normal, pos, out var linearImp, out var angularImp );
|
|
|
|
vehicle.Body.Velocity += linearImp;
|
|
vehicle.Body.AngularVelocity += angularImp;
|
|
vehicle.Body.CalculateVelocityOffset( Trace.HitPosition - (contactPos + Trace.Normal * velU * Time.Delta), pos, out var lin, out _ );
|
|
|
|
vehicle.WorldPosition += lin / Time.Delta;
|
|
damperForce = 0;
|
|
|
|
}
|
|
|
|
force = (springForce - damperForce) * Trace.Normal;
|
|
|
|
load = Math.Max( springForce - damperForce, 0 );
|
|
float R = Radius.InchToMeter();
|
|
|
|
float forwardSpeed = vel.Dot( forward ).InchToMeter();
|
|
float sideSpeed = vel.Dot( right ).InchToMeter();
|
|
|
|
float longitudinalLoadCoefficient = GetLongitudinalLoadCoefficient( load );
|
|
float lateralLoadCoefficient = GetLateralLoadCoefficient( load );
|
|
|
|
float F_roll = TirePreset.GetRollingResistance( angularVelocity * R, 1.0f ) * 10000;
|
|
|
|
( float W, float Sx, float Fx, float counterTq) = StepLongitudinal(
|
|
Torque,
|
|
Brake * BrakePowerMax + F_roll,
|
|
forwardSpeed,
|
|
angularVelocity,
|
|
longitudinalLoadCoefficient,
|
|
R,
|
|
Inertia
|
|
);
|
|
|
|
StepLateral( forwardSpeed, sideSpeed, lateralLoadCoefficient, out float Sy, out float Fy );
|
|
|
|
SlipCircle( Sx, Sy, Fx, ref Fy );
|
|
|
|
CounterTorque = counterTq;
|
|
angularVelocity = W;
|
|
|
|
force += forward * Fx;
|
|
force += right * Fy * Math.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 );
|
|
ForwardSlip = Sx;
|
|
SideSlip = Sy * Math.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 );
|
|
|
|
vehicle.Body.ApplyForceAt( pos, force / Time.Delta );
|
|
|
|
}
|
|
|
|
#if DEBUG
|
|
protected override void OnUpdate()
|
|
{
|
|
DebugOverlay.Normal( contactPos, forward * forwardFriction, Color.Red, overlay: true );
|
|
DebugOverlay.Normal( contactPos, right * sideFriction, Color.Green, overlay: true );
|
|
DebugOverlay.Normal( contactPos, up * force / 1000f, Color.Blue, overlay: true );
|
|
}
|
|
#endif
|
|
|
|
}
|