291 lines
9.5 KiB
C#
291 lines
9.5 KiB
C#
using Sandbox;
|
|
using Sandbox.Rendering;
|
|
using Sandbox.Services;
|
|
using System;
|
|
using System.Collections.Specialized;
|
|
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 => sideFriction.Slip.MeterToInch();
|
|
public float ForwardSlip => forwardFriction.Slip.MeterToInch();
|
|
[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 Friction forwardFriction;
|
|
private Friction 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 ));
|
|
float lastload;
|
|
private float inertia;
|
|
|
|
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( false )
|
|
.WithoutTags( vehicle.WheelIgnoredTags )
|
|
.Run();
|
|
//forward = ang.Forward;
|
|
//right = ang.Right;
|
|
up = ang.Up;
|
|
|
|
forward = Vector3.VectorPlaneProject( ang.Forward, Trace.Normal ).Normal;
|
|
right = Vector3.VectorPlaneProject( ang.Right, Trace.Normal ).Normal;
|
|
|
|
var fraction = Trace.Fraction;
|
|
|
|
contactPos = pos - maxLen * fraction * up;
|
|
|
|
LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );
|
|
|
|
DoSuspensionSounds( vehicle, fraction - lastFraction );
|
|
lastFraction = fraction;
|
|
|
|
var normal = Trace.Normal;
|
|
|
|
var vel = vehicle.Body.GetVelocityAtPoint( contactPos );
|
|
|
|
var offset = maxLen - (fraction * maxLen);
|
|
var springForce = (offset * SpringStrength);
|
|
var damperForce = (lastSpringOffset - offset) * SpringDamper;
|
|
lastSpringOffset = offset;
|
|
|
|
// Vector3.CalculateVelocityOffset is broken (need fix)
|
|
var velU = normal.Dot( vel );
|
|
if ( velU < 0 && offset + Math.Abs( velU * Time.Delta ) > SuspensionLength )
|
|
{
|
|
var impulse = (-velU / Time.Delta) * normal;
|
|
var body = vehicle.Body.PhysicsBody;
|
|
Vector3 com = body.MassCenter;
|
|
Rotation bodyRot = body.Rotation;
|
|
|
|
Vector3 r = pos - com;
|
|
|
|
Vector3 torque = Vector3.Cross( r, impulse );
|
|
|
|
Vector3 torqueLocal = bodyRot.Inverse * torque;
|
|
Vector3 angularVelocityLocal = torqueLocal * body.Inertia.Inverse;
|
|
|
|
var centerAngularVelocity = bodyRot * angularVelocityLocal;
|
|
|
|
var centerVelocity = impulse * (1 / body.Mass);
|
|
|
|
vehicle.Body.Velocity += centerVelocity * 10;
|
|
vehicle.Body.AngularVelocity += centerAngularVelocity;
|
|
damperForce = 0;
|
|
}
|
|
|
|
force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal;
|
|
|
|
load = Math.Max( force.z, 0 ).InchToMeter();
|
|
|
|
float forwardSpeed = vel.Dot( forward ).InchToMeter();
|
|
float sideSpeed = vel.Dot( right ).InchToMeter();
|
|
|
|
float camber_rad = CamberAngle.DegreeToRadian();
|
|
float R = Radius.InchToMeter();
|
|
|
|
float linearSpeed = angularVelocity * Radius.InchToMeter();
|
|
|
|
float F_roll = TirePreset.GetRollingResistance( linearSpeed, 1.0f );
|
|
F_roll *= -Math.Clamp( linearSpeed * 0.25f, -1, 1 );
|
|
|
|
float T_brake = Brake * BrakePowerMax;
|
|
T_brake *= -Math.Clamp( linearSpeed * 0.25f, -1, 1 );
|
|
|
|
float Winit = angularVelocity;
|
|
|
|
angularVelocity += F_roll * 9.80665f / Inertia * Time.Delta;
|
|
|
|
angularVelocity += Torque / Inertia * Time.Delta;
|
|
|
|
|
|
angularVelocity += T_brake / Inertia * Time.Delta;
|
|
|
|
if ( IsOnGround )
|
|
{
|
|
TirePreset.ComputeSlip( forwardSpeed, sideSpeed, angularVelocity, R, out var slip, out var slip_ang );
|
|
var fx = TirePreset.Pacejka.PacejkaFx( slip, load, 1, out var maxTorque );
|
|
var fy = TirePreset.Pacejka.PacejkaFy( slip_ang * load, load, camber_rad, 1, out var _ );
|
|
|
|
maxTorque *= R;
|
|
var errorTorque = (angularVelocity - forwardSpeed / R) * BaseInertia / Time.Delta;
|
|
|
|
var surfaceTorque = Math.Clamp( errorTorque, -maxTorque, maxTorque );
|
|
|
|
angularVelocity -= surfaceTorque / Inertia * Time.Delta;
|
|
|
|
|
|
float deltaOmegaTorque = (angularVelocity - Winit) * Inertia / Time.Delta;
|
|
|
|
CounterTorque = -surfaceTorque - deltaOmegaTorque;
|
|
forwardFriction.Slip = slip;
|
|
forwardFriction.Force = -fx;
|
|
forwardFriction.Speed = forwardSpeed;
|
|
sideFriction.Slip = slip_ang;
|
|
sideFriction.Force = fy;
|
|
sideFriction.Speed = sideSpeed;
|
|
|
|
vehicle.Body.ApplyForceAt( pos, force / Time.Delta * ProjectSettings.Physics.SubSteps );
|
|
|
|
Vector3 frictionForce = forward * forwardFriction.Force + right * sideFriction.Force;
|
|
vehicle.Body.ApplyForceAt( pos, frictionForce * ProjectSettings.Physics.SubSteps );
|
|
}
|
|
else
|
|
{
|
|
forwardFriction = new();
|
|
sideFriction = new();
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
//todo
|
|
protected (float Mass, float Inertia) CalcMassAndInertia()
|
|
{
|
|
// section width in millimeters, measured from sidewall to sidewall
|
|
// ratio of sidewall height to section width in percent
|
|
// diameter of the wheel in inches
|
|
var tire_size = new Vector3( 215, 45, 17 );
|
|
|
|
float tire_width = tire_size[0] * 0.001f;
|
|
float tire_aspect_ratio = tire_size[1] * 0.01f;
|
|
float tire_radius = tire_size[2] * 0.5f * 0.0254f + tire_width * tire_aspect_ratio;
|
|
float tire_thickness = 0.02f;
|
|
float tire_density = 1000; // rubber
|
|
|
|
float rim_radius = tire_radius - tire_width * tire_aspect_ratio;
|
|
float rim_width = tire_width;
|
|
float rim_thickness = 0.01f;
|
|
float rim_density = 2700; // aluminium
|
|
|
|
float tire_volume = float.Pi * tire_width * tire_thickness * (2 * tire_radius - tire_thickness);
|
|
float rim_volume = float.Pi * rim_width * rim_thickness * (2 * rim_radius - rim_thickness);
|
|
float tire_mass = tire_density * tire_volume;
|
|
float rim_mass = rim_density * rim_volume;
|
|
float tire_inertia = tire_mass * tire_radius * tire_radius;
|
|
float rim_inertia = rim_mass * rim_radius * rim_radius;
|
|
|
|
float mass = tire_mass + rim_mass;
|
|
float inertia = tire_inertia + rim_inertia;
|
|
|
|
return (mass, inertia);
|
|
}
|
|
|
|
// debug
|
|
protected override void OnUpdate()
|
|
{
|
|
//DebugOverlay.Normal( contactPos, forward * forwardFriction.Force / 1000f, Color.Red, overlay: true );
|
|
//DebugOverlay.Normal( contactPos, right * sideFriction.Force / 1000f, Color.Green, overlay: true );
|
|
//DebugOverlay.Normal( contactPos, up * force / 1000f, Color.Blue, overlay: true );
|
|
}
|
|
}
|