From 4899a38265bdf7cc3362bba19b98013b2b95f399 Mon Sep 17 00:00:00 2001 From: Valera <108022376+kekobka@users.noreply.github.com> Date: Sun, 15 Jun 2025 03:23:47 +0700 Subject: [PATCH] not work --- Code/Base/Powertrain/Engine.cs | 2 +- Code/Base/VeloXBase.cs | 2 +- Code/Base/Wheel/Pacejka.cs | 55 +--------- Code/Base/Wheel/Suspension/BasicSuspension.cs | 18 +++ Code/Base/Wheel/Suspension/Hinge.cs | 26 +++++ .../Wheel/Suspension/MacPhersonSuspension.cs | 32 ++++++ Code/Base/Wheel/TirePreset.cs | 79 +++++++++++--- Code/Base/Wheel/VeloXWheel.Sound.cs | 28 +++++ Code/Base/Wheel/VeloXWheel.cs | 103 +++++++++--------- Code/Car/VeloXCar.Steering.cs | 2 +- 10 files changed, 225 insertions(+), 122 deletions(-) create mode 100644 Code/Base/Wheel/Suspension/BasicSuspension.cs create mode 100644 Code/Base/Wheel/Suspension/Hinge.cs create mode 100644 Code/Base/Wheel/Suspension/MacPhersonSuspension.cs create mode 100644 Code/Base/Wheel/VeloXWheel.Sound.cs diff --git a/Code/Base/Powertrain/Engine.cs b/Code/Base/Powertrain/Engine.cs index 400f67e..54756a4 100644 --- a/Code/Base/Powertrain/Engine.cs +++ b/Code/Base/Powertrain/Engine.cs @@ -87,7 +87,7 @@ public class Engine : PowertrainComponent angularVelocity += finalTorque / inertiaSum * Time.Delta; angularVelocity = Math.Max( angularVelocity, 0 ); - UpdateStream(); + //UpdateStream(); return finalTorque; } diff --git a/Code/Base/VeloXBase.cs b/Code/Base/VeloXBase.cs index b408534..6215466 100644 --- a/Code/Base/VeloXBase.cs +++ b/Code/Base/VeloXBase.cs @@ -15,7 +15,7 @@ public abstract partial class VeloXBase : Component [Property, Group( "Components" )] public Rigidbody Body { get; protected set; } [Property, Group( "Components" )] public Collider Collider { get; protected set; } - [Sync] public Angles SteerAngle { get; set; } + [Sync( SyncFlags.Interpolate )] public Angles SteerAngle { get; set; } public Vector3 LocalVelocity; public float ForwardSpeed; diff --git a/Code/Base/Wheel/Pacejka.cs b/Code/Base/Wheel/Pacejka.cs index 7da7570..6dac58e 100644 --- a/Code/Base/Wheel/Pacejka.cs +++ b/Code/Base/Wheel/Pacejka.cs @@ -62,7 +62,7 @@ public class Pacejka [Range( -300, 300 )] public float b1 = 0f; // 1 [Description( "Longitudinal friction coefficient at load = 0 (*1000)" )] - [Range( -10, 10 )] public float b2 = 1690f; // 2 + [Range( 0, 10000 )] public float b2 = 1690f; // 2 [Description( "Curvature factor of stiffness (N/%/kN**2)" )] [Range( -100, 100 )] public float b3 = 0f; // 3 @@ -87,12 +87,6 @@ public class Pacejka [Description( "Horizontal shift at load = 0 (%)" )] [Range( -10, 10 )] public float b10 = 0f; // 10 - - [Description( "Load influence on vertical shift (N/kN)" )] - [Range( -10, 10 )] public float b11 = 0f; // 10 - - [Description( "Vertical shift at load = 0 (N)" )] - [Range( -10, 10 )] public float b12 = 0f; // 10 } @@ -200,7 +194,7 @@ public class Pacejka // longitudinal force float BS = B * S; - float Fx = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ); + float Fx = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ); // scale by surface friction Fx *= friction_coeff; @@ -220,8 +214,7 @@ public class Pacejka float D = (a.a1 * Fz + a.a2) * Fz; // stiffness at alpha = 0 - float BCD = a.a3 * Sin2Atan( Fz, a.a4 ) * (1 - a.a5 * MathF.Abs( gamma )); - + float BCD = a.a3 * MathF.Atan2( Fz, a.a4 ) * (1 - a.a5 * MathF.Abs( gamma )); // stiffness factor float B = BCD / (C * D); @@ -239,7 +232,7 @@ public class Pacejka // lateral force float BS = B * S; - float Fy = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; + float Fy = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; // scale by surface friction Fy *= friction_coeff; @@ -279,7 +272,7 @@ public class Pacejka // self-aligning torque float BS = B * S; - float Mz = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; + float Mz = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; // scale by surface friction Mz *= friction_coeff; @@ -306,42 +299,4 @@ public class Pacejka float c = a * a + 1; return MathF.Sqrt( c / (c + b * b) ); } - - public static float SinPi2( float x ) - { - float s = x * x; - float p = -1.8447486103462252e-04f; - p = 8.3109378830028557e-03f + p * s; - p = -1.6665578084732124e-01f + p * s; - p = 1.0f + p * s; - return p * x; - } - - // Вычисление sin(x) для |x| <= 3π/2 - public static float Sin3Pi2( float x ) - { - // Приведение x к интервалу [-π, π] - if ( x < -MathF.PI ) - x += 2 * MathF.PI; - else if ( x > MathF.PI ) - x -= 2 * MathF.PI; - - // Отражение в интервал [-π/2, π/2] с использованием симметрии - if ( x < -MathF.PI / 2 ) - x = -MathF.PI - x; - else if ( x > MathF.PI / 2 ) - x = MathF.PI - x; - - return SinPi2( x ); - } - - public static float Sin2Atan( float x ) - { - return 2 * x / (x * x + 1); - } - - public static float Sin2Atan( float y, float x ) - { - return 2 * x * y / (x * x + y * y); - } } diff --git a/Code/Base/Wheel/Suspension/BasicSuspension.cs b/Code/Base/Wheel/Suspension/BasicSuspension.cs new file mode 100644 index 0000000..bed19d6 --- /dev/null +++ b/Code/Base/Wheel/Suspension/BasicSuspension.cs @@ -0,0 +1,18 @@ +namespace VeloX; + +public class BasicSuspension +{ + private readonly Hinge Hinge; + + public BasicSuspension( Vector3 wheel, Vector3 hingeBody, Vector3 hingeWheel ) + { + Vector3 hingePoint = wheel - (hingeWheel - hingeBody); + Hinge = new Hinge( hingePoint, hingeWheel - hingeBody ); + } + + public virtual void GetWheelTransform( float travel, out Rotation rotation, out Vector3 position ) + { + rotation = Rotation.Identity; + position = Hinge.Rotate( travel ); + } +} diff --git a/Code/Base/Wheel/Suspension/Hinge.cs b/Code/Base/Wheel/Suspension/Hinge.cs new file mode 100644 index 0000000..db8d1d9 --- /dev/null +++ b/Code/Base/Wheel/Suspension/Hinge.cs @@ -0,0 +1,26 @@ +using System; + +namespace VeloX; + +internal readonly struct Hinge( Vector3 hinge_anchor, Vector3 hinge_arm ) +{ + [Description( "the point that the wheels are rotated around as the suspension compresses" )] + public readonly Vector3 Anchor = hinge_anchor; + + [Description( "anchor to wheel vector" )] + public readonly Vector3 Arm = hinge_arm; + + [Description( "arm length squared" )] + public readonly float LengthSquared = hinge_arm.Dot( hinge_arm ); + + [Description( "1 / arm length in hinge axis normal plane" )] + public readonly float NormXY = 1 / MathF.Sqrt( hinge_arm.x * hinge_arm.x + hinge_arm.y * hinge_arm.y ); + + public readonly Vector3 Rotate( float travel ) + { + float z = Arm.z + travel; + float lengthSq = MathF.Max( LengthSquared - z * z, 0.0f ); + float nxy = NormXY * MathF.Sqrt( lengthSq ); + return Anchor + new Vector3( Arm.x * nxy, Arm.y * nxy, z ); + } +} diff --git a/Code/Base/Wheel/Suspension/MacPhersonSuspension.cs b/Code/Base/Wheel/Suspension/MacPhersonSuspension.cs new file mode 100644 index 0000000..6cd1044 --- /dev/null +++ b/Code/Base/Wheel/Suspension/MacPhersonSuspension.cs @@ -0,0 +1,32 @@ +using Sandbox; +using System; + +namespace VeloX; +public class MacPhersonSuspension +{ + public readonly Vector3 WheelOffset; + public readonly Vector3 UprightTop; + public readonly Vector3 UprightAxis; + private readonly Hinge Hinge; + + public MacPhersonSuspension( Vector3 wheel, Vector3 strutBody, Vector3 strutWheel, Vector3 hingeBody ) + { + WheelOffset = wheel - strutWheel; + UprightTop = strutBody; + UprightAxis = (strutBody - strutWheel).Normal; + Hinge = new( hingeBody, strutWheel - hingeBody ); + } + + public void GetWheelTransform( float travel, out Rotation rotation, out Vector3 position ) + { + Vector3 hingeEnd = Hinge.Rotate( travel ); + Vector3 uprightAxisNew = (UprightTop - hingeEnd).Normal; + + rotation = Rotation.FromAxis( + Vector3.Cross( UprightAxis, uprightAxisNew ), + MathF.Acos( Vector3.Dot( UprightAxis, uprightAxisNew ) ).RadianToDegree() + ); + + position = hingeEnd + WheelOffset.Transform( rotation ); + } +} diff --git a/Code/Base/Wheel/TirePreset.cs b/Code/Base/Wheel/TirePreset.cs index ecfa529..eee29ad 100644 --- a/Code/Base/Wheel/TirePreset.cs +++ b/Code/Base/Wheel/TirePreset.cs @@ -30,6 +30,7 @@ public class TirePreset : GameResource float vslip = vrot - vlon; slip_ratio = vslip * rvlon; slip_angle = -MathF.Atan( vlat * rvlon ); + } /// approximate asin(x) = x + x^3/6 for +-18 deg range @@ -39,22 +40,57 @@ public class TirePreset : GameResource return ((1 / 6.0f) * (sc * sc) + 1) * sc; } - public struct TireState + public struct TireState() { - public float friction = 0; // surface friction coefficient - public float camber = 0; // tire camber angle relative to track surface - public float vcam = 0; // camber thrust induced lateral slip velocity - public float slip = 0; // ratio of tire contact patch speed to road speed - public float slip_angle = 0; // the angle between the wheel heading and the wheel velocity - public float ideal_slip = 0; // peak force slip ratio - public float ideal_slip_angle = 0; // peak force slip angle - public float fx = 0; // positive during traction - public float fy = 0; // positive in a right turn - public float mz = 0; // positive in a left turn + /// + /// surface friction coefficient + /// + public float friction = 0; - public TireState() - { - } + /// + /// tire camber angle relative to track surface + /// + public float camber = 0; + + /// + /// camber thrust induced lateral slip velocity + /// + public float vcam = 0; + + /// + /// ratio of tire contact patch speed to road speed + /// + public float slip = 0; + + /// + /// the angle between the wheel heading and the wheel velocity + /// + public float slip_angle = 0; + + /// + /// peak force slip ratio + /// + public float ideal_slip = 0; + + /// + /// peak force slip angle + /// + public float ideal_slip_angle = 0; + + /// + /// positive during traction + /// + public float fx = 0; + + /// + /// positive during traction in a right turn + /// + public float fy = 0; + + /// + /// positive during traction in a left turn + /// + public float mz = 0; }; public void ComputeState( @@ -79,13 +115,22 @@ public class TirePreset : GameResource return; } - float Fz = Math.Min( normal_force * 1E-3f, 30f ); + float Fz = MathF.Min( normal_force * 0.001f, 30f ); + ComputeSlip( lon_velocity, lat_velocity, rot_velocity, out float slip, out float slip_angle ); float sigma = slip; float alpha = slip_angle.RadianToDegree(); float gamma = s.camber.RadianToDegree(); - float Fx = Pacejka.PacejkaFx( sigma, Fz, s.friction ); - float Fy = Pacejka.PacejkaFy( alpha, Fz, gamma, s.friction, out float camber_alpha ); + + float Fx0 = Pacejka.PacejkaFx( sigma, Fz, s.friction ); + float Fy0 = Pacejka.PacejkaFy( alpha, Fz, gamma, s.friction, out float camber_alpha ); + + // combined slip + float Gx = Pacejka.PacejkaGx( slip, slip_angle ); + float Gy = Pacejka.PacejkaGy( slip, slip_angle ); + float Fx = Gx * Fx0; + float Fy = Gy * Fy0; + s.vcam = ComputeCamberVelocity( camber_alpha.DegreeToRadian(), lon_velocity ); s.slip = slip; s.slip_angle = slip_angle; diff --git a/Code/Base/Wheel/VeloXWheel.Sound.cs b/Code/Base/Wheel/VeloXWheel.Sound.cs new file mode 100644 index 0000000..ff71b2b --- /dev/null +++ b/Code/Base/Wheel/VeloXWheel.Sound.cs @@ -0,0 +1,28 @@ +using Sandbox; +using System; + +namespace VeloX; + + +public partial class VeloXWheel : Component +{ + private RealTimeUntil expandSoundCD; + private RealTimeUntil contractSoundCD; + 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 ); + } + } +} diff --git a/Code/Base/Wheel/VeloXWheel.cs b/Code/Base/Wheel/VeloXWheel.cs index 4a53016..9651ede 100644 --- a/Code/Base/Wheel/VeloXWheel.cs +++ b/Code/Base/Wheel/VeloXWheel.cs @@ -4,6 +4,7 @@ using System; using System.Collections.Specialized; using System.Numerics; using System.Text.RegularExpressions; +using static Sandbox.CameraComponent; namespace VeloX; @@ -38,7 +39,7 @@ public partial class VeloXWheel : Component public float Spin { get; private set; } - public float RPM { get => angularVelocity * 60f / MathF.Tau; set => angularVelocity = value / (60 / MathF.Tau); } + public float RPM { get => angularVelocity * 30f / MathF.PI; set => angularVelocity = value / (30f / MathF.PI); } public float AngularVelocity => angularVelocity; internal float DistributionFactor { get; set; } @@ -53,8 +54,6 @@ public partial class VeloXWheel : Component private float angularVelocity; private float load; private float lastFraction; - private RealTimeUntil expandSoundCD; - private RealTimeUntil contractSoundCD; private Vector3 contactPos; private Vector3 forward; @@ -66,7 +65,7 @@ public partial class VeloXWheel : Component private Vector3 force; public float CounterTorque { get; private set; } - private float BaseInertia => 0.5f * Mass * MathF.Pow( Radius.InchToMeter(), 2 ); + private float BaseInertia => Mass * MathF.Pow( Radius.InchToMeter(), 2 ); public float Inertia => BaseInertia; @@ -77,24 +76,6 @@ public partial class VeloXWheel : Component StartPos = LocalPosition; } - 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 ); @@ -113,7 +94,7 @@ public partial class VeloXWheel : Component private Rotation GetSteer( float steer ) { - float angle = (steer * SteerMultiplier).DegreeToRadian(); + 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 ); @@ -126,7 +107,7 @@ public partial class VeloXWheel : Component 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; public void DoPhysics( VeloXBase vehicle ) { var pos = vehicle.WorldTransform.PointToWorld( StartPos ); @@ -162,10 +143,6 @@ public partial class VeloXWheel : Component var vel = vehicle.Body.GetVelocityAtPoint( pos ); - vel.x = vel.x.InchToMeter(); - vel.y = vel.y.InchToMeter(); - vel.z = vel.z.InchToMeter(); - if ( !IsOnGround ) { forwardFriction = new Friction(); @@ -177,29 +154,46 @@ public partial class VeloXWheel : Component var springForce = (offset * SpringStrength); var damperForce = (lastSpringOffset - offset) * SpringDamper; lastSpringOffset = offset; - force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal / Time.Delta; // Vector3.CalculateVelocityOffset is broken (need fix) - //var velU = normal.Dot( vel ).MeterToInch(); - //if ( velU < 0 && offset + Math.Abs( velU * dt ) > SuspensionLength ) - //{ - // var (linearVel, angularVel) = vehicle.Body.PhysicsBody.CalculateVelocityOffset( (-velU.InchToMeter() / dt) * normal, pos ); - // vehicle.Body.Velocity += linearVel; - // vehicle.Body.AngularVelocity += angularVel; - //} + 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; - load = Math.Max( force.z.InchToMeter(), 0 ); + 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(); if ( IsOnGround ) { - float forwardSpeed = vel.Dot( forward ); - float sideSpeed = vel.Dot( right ); + float forwardSpeed = vel.Dot( forward ).InchToMeter(); + float sideSpeed = vel.Dot( right ).InchToMeter(); float camber_rad = CamberAngle.DegreeToRadian(); - + float R = Radius.InchToMeter(); TirePreset.ComputeState( - load, - angularVelocity, + 2500, + angularVelocity * R, forwardSpeed, sideSpeed, camber_rad, @@ -212,31 +206,36 @@ public partial class VeloXWheel : Component float Fx_total = tireState.fx + F_roll; - float R = Radius.InchToMeter(); - float I = Inertia; float T_brake = Brake * BrakePowerMax; if ( angularVelocity > 0 ) T_brake = -T_brake; else T_brake = angularVelocity < 0 ? T_brake : -MathF.Sign( Torque ) * T_brake; - float totalTorque = Torque + T_brake - Fx_total * R; - angularVelocity += totalTorque / I * Time.Delta; + + // not work + Vector3 c = pos.Cross( forward ); + Vector3 v = (c * vehicle.Body.PhysicsBody.Inertia).Cross( pos ); + var m = 1 / (1 / vehicle.Body.Mass + forward.Dot( v )); + m *= Inertia / (m * R * R + Inertia); + float ve = forward.Dot( vel ).InchToMeter() - angularVelocity; + + angularVelocity += ve * m * Radius * (1 / Inertia);// totalTorque * (1 / Inertia) * Time.Delta; forwardFriction = new Friction() { Slip = tireState.slip, - Force = Fx_total.MeterToInch(), + Force = Fx_total, Speed = forwardSpeed }; sideFriction = new Friction() { Slip = tireState.slip_angle, - Force = tireState.fy.MeterToInch(), + Force = tireState.fy, Speed = sideSpeed }; Vector3 frictionForce = forward * forwardFriction.Force + right * sideFriction.Force; - vehicle.Body.ApplyForceAt( contactPos, force + frictionForce ); + vehicle.Body.ApplyForceAt( contactPos, (force + frictionForce) / Time.Delta ); } else @@ -260,8 +259,8 @@ public partial class VeloXWheel : Component // debug protected override void OnUpdate() { - DebugOverlay.Normal( contactPos, forward * forwardFriction.Force / 10000f, Color.Red, overlay: true ); - DebugOverlay.Normal( contactPos, right * sideFriction.Force / 10000f, Color.Green, overlay: true ); - DebugOverlay.Normal( contactPos, up * force / 50000f, Color.Blue, overlay: true ); + DebugOverlay.Normal( contactPos, forward * forwardFriction.Force / 100f, Color.Red, overlay: true ); + DebugOverlay.Normal( contactPos, right * sideFriction.Force / 100f, Color.Green, overlay: true ); + DebugOverlay.Normal( contactPos, up * force / 1000f, Color.Blue, overlay: true ); } } diff --git a/Code/Car/VeloXCar.Steering.cs b/Code/Car/VeloXCar.Steering.cs index 818c588..52c2dbd 100644 --- a/Code/Car/VeloXCar.Steering.cs +++ b/Code/Car/VeloXCar.Steering.cs @@ -36,7 +36,7 @@ public partial class VeloXCar inputSteer = Math.Clamp( inputSteer + counterSteer, -1, 1 ); Steering = inputSteer; - SteerAngle = new( 0, inputSteer * MaxSteerAngle, 0 ); + SteerAngle = new( 0, -inputSteer * MaxSteerAngle, 0 ); if ( ForwardSpeed < -100 ) jTurnMultiplier = 0.5f;