From 629ae6715c9fbddf6fddb5c28c578b1ccdfe36ef Mon Sep 17 00:00:00 2001 From: Valera <108022376+kekobka@users.noreply.github.com> Date: Sat, 14 Jun 2025 18:16:26 +0700 Subject: [PATCH] new pacejka implementation (car not working) --- .../High-Performance Road Tire.whfric | 22 -- Assets/frictions/aboba.whfric | 22 -- Assets/frictions/default.tire | 7 + .../Differential/BaseDifferential.cs | 2 +- Code/Base/Powertrain/Engine.cs | 14 +- Code/Base/Wheel/FrictionPreset.cs | 16 - Code/Base/Wheel/Pacejka.cs | 347 ++++++++++++++++++ Code/Base/Wheel/TirePreset.cs | 97 ++++- Code/Base/Wheel/VeloXWheel.cs | 234 ++++-------- Code/Car/VeloXCar.Steering.cs | 2 +- Editor/EngineStreamInspector.cs | 55 --- Editor/Wheel/PacejkaWidget.cs | 90 +++++ Editor/Wheel/TirePresetEditor.cs | 70 ++++ Editor/Wheel/TirePresetPreview.cs | 188 ++++++++++ 14 files changed, 883 insertions(+), 283 deletions(-) delete mode 100644 Assets/frictions/High-Performance Road Tire.whfric delete mode 100644 Assets/frictions/aboba.whfric create mode 100644 Assets/frictions/default.tire delete mode 100644 Code/Base/Wheel/FrictionPreset.cs create mode 100644 Code/Base/Wheel/Pacejka.cs delete mode 100644 Editor/EngineStreamInspector.cs create mode 100644 Editor/Wheel/PacejkaWidget.cs create mode 100644 Editor/Wheel/TirePresetEditor.cs create mode 100644 Editor/Wheel/TirePresetPreview.cs diff --git a/Assets/frictions/High-Performance Road Tire.whfric b/Assets/frictions/High-Performance Road Tire.whfric deleted file mode 100644 index d1b93e8..0000000 --- a/Assets/frictions/High-Performance Road Tire.whfric +++ /dev/null @@ -1,22 +0,0 @@ -{ - "Longitudinal": { - "B": 18, - "C": 1.5, - "D": 1.5, - "E": 0.3 - }, - "Lateral": { - "B": 12, - "C": 1.3, - "D": 1.8, - "E": -1.8 - }, - "Aligning": { - "B": 2.8, - "C": 2.1, - "D": 0.1, - "E": -2.5 - }, - "__references": [], - "__version": 0 -} \ No newline at end of file diff --git a/Assets/frictions/aboba.whfric b/Assets/frictions/aboba.whfric deleted file mode 100644 index 812f6dd..0000000 --- a/Assets/frictions/aboba.whfric +++ /dev/null @@ -1,22 +0,0 @@ -{ - "Longitudinal": { - "B": 0, - "C": 1, - "D": 1, - "E": 0.3 - }, - "Lateral": { - "B": 1, - "C": 1, - "D": 1, - "E": 0.3 - }, - "Aligning": { - "B": 2.8, - "C": 2.1, - "D": 0.1, - "E": -2.5 - }, - "__references": [], - "__version": 0 -} \ No newline at end of file diff --git a/Assets/frictions/default.tire b/Assets/frictions/default.tire new file mode 100644 index 0000000..e2532c9 --- /dev/null +++ b/Assets/frictions/default.tire @@ -0,0 +1,7 @@ +{ + "Pacejka": {}, + "RollResistanceLin": 0.001, + "RollResistanceQuad": 1E-06, + "__references": [], + "__version": 0 +} \ No newline at end of file diff --git a/Code/Base/Powertrain/Differential/BaseDifferential.cs b/Code/Base/Powertrain/Differential/BaseDifferential.cs index 1eaa75c..296ce82 100644 --- a/Code/Base/Powertrain/Differential/BaseDifferential.cs +++ b/Code/Base/Powertrain/Differential/BaseDifferential.cs @@ -3,7 +3,7 @@ using System; namespace VeloX.Powertrain; -[Category( "VeloX/Powertrain/Gearbox" )] +[Category( "VeloX/Powertrain/Differential" )] public abstract class BaseDifferential : PowertrainComponent { [Property] public float FinalDrive { get; set; } = 3.392f; diff --git a/Code/Base/Powertrain/Engine.cs b/Code/Base/Powertrain/Engine.cs index 5c95861..400f67e 100644 --- a/Code/Base/Powertrain/Engine.cs +++ b/Code/Base/Powertrain/Engine.cs @@ -8,8 +8,6 @@ public class Engine : PowertrainComponent [Property, Group( "Settings" )] public float IdleRPM { get; set; } = 900f; [Property, Group( "Settings" )] public float MaxRPM { get; set; } = 7000f; [Property, Group( "Settings" )] public override float Inertia { get; set; } = 0.151f; - [Property, Group( "Settings" )] public float StartFriction { get; set; } = 50f; - [Property, Group( "Settings" )] public float FrictionCoeff { get; set; } = 0.02f; [Property, Group( "Settings" )] public float LimiterDuration { get; set; } = 0.05f; [Property, Group( "Settings" )] public Curve TorqueMap { get; set; } [Property, Group( "Settings" )] public EngineStream Stream { get; set; } @@ -24,6 +22,7 @@ public class Engine : PowertrainComponent private float finalTorque; private EngineStreamPlayer StreamPlayer; + public float[] friction = [15.438f, 2.387f, 0.7958f]; protected override void OnStart() { @@ -32,11 +31,19 @@ public class Engine : PowertrainComponent StreamPlayer = new( Stream ); } + public float GetFrictionTorque( float throttle, float rpm ) + { + float s = rpm < 0 ? -1f : 1f; + float r = s * rpm * 0.001f; + float f = friction[0] + friction[1] * r + friction[2] * r * r; + return -s * f * (1 - throttle); + } private float GenerateTorque() { float throttle = Throttle; float rpm = RPM; - float friction = StartFriction - rpm * FrictionCoeff; + float friction = GetFrictionTorque( throttle, rpm ); + float maxInitialTorque = TorqueMap.Evaluate( RPMPercent ) - friction; float idleFadeStart = Math.Clamp( MathX.Remap( rpm, IdleRPM - 300, IdleRPM, 1, 0 ), 0, 1 ); float idleFadeEnd = Math.Clamp( MathX.Remap( rpm, IdleRPM, IdleRPM + 600, 1, 0 ), 0, 1 ); @@ -70,6 +77,7 @@ public class Engine : PowertrainComponent float outputInertia = Output.QueryInertia(); float inertiaSum = Inertia + outputInertia; float outputW = Output.QueryAngularVelocity( angularVelocity ); + float targetW = Inertia / inertiaSum * angularVelocity + outputInertia / inertiaSum * outputW; float generatedTorque = GenerateTorque(); float reactTorque = (targetW - angularVelocity) * Inertia / Time.Delta; diff --git a/Code/Base/Wheel/FrictionPreset.cs b/Code/Base/Wheel/FrictionPreset.cs deleted file mode 100644 index 55909b3..0000000 --- a/Code/Base/Wheel/FrictionPreset.cs +++ /dev/null @@ -1,16 +0,0 @@ -using System; - -namespace VeloX; -public class FrictionPreset -{ - public float B { get; set; } = 10.86f; - public float C { get; set; } = 2.15f; - public float D { get; set; } = 0.933f; - public float E { get; set; } = 0.992f; - public float Evaluate( float slip ) - { - var t = Math.Abs( slip ); - - return D * MathF.Sin( C * MathF.Atan( B * t - E * (B * t - MathF.Atan( B * t )) ) ); - } -} diff --git a/Code/Base/Wheel/Pacejka.cs b/Code/Base/Wheel/Pacejka.cs new file mode 100644 index 0000000..7da7570 --- /dev/null +++ b/Code/Base/Wheel/Pacejka.cs @@ -0,0 +1,347 @@ +using Sandbox; +using Sandbox.Services; +using System; + +namespace VeloX; +public class Pacejka +{ + public struct LateralForce() + { + [Description( "Shape factor" )] + [Range( 1, 3 )] public float a0 = 1.4f; // 0 + + [Description( "Load infl on lat friction coeff (*1000) (1/kN)" )] + [Range( -100, 100 )] public float a1 = -0f; // 1 + + [Description( "Lateral friction coefficient at load = 0 (*1000)" )] + [Range( 1, 2500 )] public float a2 = 1688f; // 2 + + [Description( "Maximum stiffness (N/deg)" )] + [Range( 1, 5000 )] public float a3 = 2400f; // 3 + + [Description( "Load at maximum stiffness (kN)" )] + [Range( -100, 100 )] public float a4 = 6.026f; // 4 + + [Description( "Camber infiuence on stiffness (%/deg/100)" )] + [Range( -10, 10 )] public float a5 = 0f; // 5 + + [Description( "Curvature change with load" )] + [Range( -10, 10 )] public float a6 = -0.359f; // 6 + + [Description( "Curvature at load = 0" )] + [Range( -10, 10 )] public float a7 = 1.0f; // 7 + + [Description( "Horizontal shift because of camber (deg/deg)" )] + [Range( -10, 10 )] public float a8 = 0f; // 8 + + [Description( "Load influence on horizontal shift (deg/kN)" )] + [Range( -10, 10 )] public float a9 = -0.00611f;// 9 + + [Description( "Horizontal shift at load = 0 (deg)" )] + [Range( -10, 10 )] public float a10 = -0.0322f;// 10 + + [Description( "Camber influence on vertical shift (N/deg/kN)" )] + [Range( -10, 100 )] public float a111 = 0f; // 11 + + [Description( "Camber influence on vertical shift (N/deg/kN**2" )] + [Range( -10, 10 )] public float a112 = 0f; // 12 + + [Description( "Load influence on vertical shift (N/kN)" )] + [Range( -100, 100 )] public float a12 = 0f; // 13 + + [Description( "Vertical shift at load = 0 (N)" )] + [Range( -10, 10 )] public float a13 = 0f; // 14 + } + + public struct LongitudinalForce() + { + [Description( "Shape factor" )] + [Range( 1, 3 )] public float b0 = 1.65f; // 0 + + [Description( "Load infl on long friction coeff (*1000) (1/kN)" )] + [Range( -300, 300 )] public float b1 = 0f; // 1 + + [Description( "Longitudinal friction coefficient at load = 0 (*1000)" )] + [Range( -10, 10 )] public float b2 = 1690f; // 2 + + [Description( "Curvature factor of stiffness (N/%/kN**2)" )] + [Range( -100, 100 )] public float b3 = 0f; // 3 + + [Description( "Change of stiffness with load at load = 0 (N/%/kN)" )] + [Range( -1000, 1000 )] public float b4 = 229f; // 4 + + [Description( "Change of progressivity of stiffness/load (1/kN)" )] + [Range( -10, 10 )] public float b5 = 0f; // 5 + + [Description( "Curvature change with load" )] + [Range( -10, 10 )] public float b6 = 0f; // 6 + + [Description( "Curvature change with load" )] + [Range( -10, 10 )] public float b7 = 0f; // 7 + + [Description( "Curvature at load = 0" )] + [Range( -10, 10 )] public float b8 = -10f; // 7 + + [Description( "Load influence on horizontal shift (%/kN)" )] + [Range( -10, 10 )] public float b9 = 0f; // 9 + + [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 + } + + + public struct AligningMoment() + { + [Description( "Shape factor" )] + [Range( 1, 7 )] public float c0 = 2.0f; // 0 + + [Description( "Load influence of peak value (Nm/kN**2)" )] + [Range( -10, 10 )] public float c1 = -3.8f; // 1 + + [Description( "Load influence of peak value (Nm/kN)" )] + [Range( -10, 10 )] public float c2 = -3.14f; // 2 + + [Description( "Curvature factor of stiffness (Nm/deg/kN**2" )] + [Range( -10, 10 )] public float c3 = -1.16f; // 3 + + [Description( "Change of stiffness with load at load = 0 (Nm/deg/kN)" )] + [Range( -100, 100 )] public float c4 = -7.2f; // 4 + + [Description( "Change of progressivity of stiffness/load (1/kN)" )] + [Range( -10, 10 )] public float c5 = 0.0f; // 5 + + [Description( "Camber influence on stiffness (%/deg/100)" )] + [Range( -10, 10 )] public float c6 = 0.0f; // 6 + + [Description( "Curvature change with load" )] + [Range( -10, 10 )] public float c7 = 0.044f; // 7 + + [Description( "Curvature change with load" )] + [Range( -10, 10 )] public float c8 = -0.58f; // 8 + + [Description( "Curvature at load = 0" )] + [Range( -10, 10 )] public float c9 = 0.18f; // 9 + + [Description( "Camber influence of stiffness" )] + [Range( -10, 10 )] public float c10 = 0.0f; // 10 + + [Description( "Camber influence on horizontal shift (deg/deg)" )] + [Range( -10, 10 )] public float c11 = 0.0f; // 11 + + [Description( "Load influence on horizontal shift (deg/kN)" )] + [Range( -10, 10 )] public float c12 = 0.0f; // 12 + + [Description( "Horizontal shift at load = 0 (deg)" )] + [Range( -10, 10 )] public float c13 = 0.0f; // 13 + + [Description( "Camber influence on vertical shift (Nm/deg/kN**2" )] + [Range( -10, 10 )] public float c14 = 0.14f; // 14 + + [Description( "Camber influence on vertical shift (Nm/deg/kN)" )] + [Range( -10, 10 )] public float c15 = -1.029f; // 15 + + [Description( "Load influence on vertical shift (Nm/kN)" )] + [Range( -10, 10 )] public float c16 = 0.0f; // 16 + + [Description( "Vertical shift at load = 0 (Nm)" )] + [Range( -10, 10 )] public float c17 = 0.0f; // 17 + } + public struct CombiningForce + { + public float gy1 = 1; // 0 + public float gy2 = 1; // 1 + public float gx1 = 1; // 2 + public float gx2 = 1f; // 3 + + public CombiningForce() + { + } + } + + + public LateralForce Lateral = new(); + public LongitudinalForce Longitudinal = new(); + public AligningMoment Aligning = new(); + public CombiningForce Combining = new(); + + + + /// pacejka magic formula for longitudinal force + public float PacejkaFx( float sigma, float Fz, float friction_coeff ) + { + var b = Longitudinal; + + // shape factor + float C = b.b0; + + // peak factor + float D = (b.b1 * Fz + b.b2) * Fz; + + // stiffness at sigma = 0 + float BCD = (b.b3 * Fz + b.b4) * Fz * MathF.Exp( -b.b5 * Fz ); + + // stiffness factor + float B = BCD / (C * D); + + // curvature factor + float E = (b.b6 * Fz + b.b7) * Fz + b.b8; + + // horizontal shift + float Sh = b.b9 * Fz + b.b10; + + // composite + float S = 100 * sigma + Sh; + + // longitudinal force + float BS = B * S; + float Fx = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ); + + // scale by surface friction + Fx *= friction_coeff; + + return Fx; + } + + /// pacejka magic formula for lateral force + public float PacejkaFy( float alpha, float Fz, float gamma, float friction_coeff, out float camber_alpha ) + { + var a = Lateral; + + // shape factor + float C = a.a0; + + // peak factor + 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 )); + + // stiffness factor + float B = BCD / (C * D); + + // curvature factor + float E = a.a6 * Fz + a.a7; + + // horizontal shift + float Sh = a.a8 * gamma + a.a9 * Fz + a.a10; + + // vertical shift + float Sv = ((a.a111 * Fz + a.a112) * gamma + a.a12) * Fz + a.a13; + + // composite slip angle + float S = alpha + Sh; + + // lateral force + float BS = B * S; + float Fy = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; + + // scale by surface friction + Fy *= friction_coeff; + camber_alpha = Sh + Sv / BCD * friction_coeff; + + return Fy; + } + + /// pacejka magic formula for aligning torque + public float PacejkaMz( float alpha, float Fz, float gamma, float friction_coeff ) + { + var c = Aligning; + + // shape factor + float C = c.c0; + + // peak factor + float D = (c.c1 * Fz + c.c2) * Fz; + + // stiffness at alpha = 0 + float BCD = (c.c3 * Fz + c.c4) * Fz * (1 - c.c6 * MathF.Abs( gamma )) * MathF.Exp( -c.c5 * Fz ); + + // stiffness factor + float B = BCD / (C * D); + + // curvature factor + float E = (c.c7 * Fz * Fz + c.c8 * Fz + c.c9) * (1 - c.c10 * MathF.Abs( gamma )); + + // horizontal shift + float Sh = c.c11 * gamma + c.c12 * Fz + c.c13; + + // composite slip angle + float S = alpha + Sh; + + // vertical shift + float Sv = (c.c14 * Fz * Fz + c.c15 * Fz) * gamma + c.c16 * Fz + c.c17; + + // self-aligning torque + float BS = B * S; + float Mz = D * Sin3Pi2( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv; + + // scale by surface friction + Mz *= friction_coeff; + + return Mz; + } + + /// pacejka magic formula for the longitudinal combining factor + public float PacejkaGx( float sigma, float alpha ) + { + var p = Combining; + float a = p.gx2 * sigma; + float b = p.gx1 * alpha; + float c = a * a + 1; + return MathF.Sqrt( c / (c + b * b) ); + } + + /// pacejka magic formula for the lateral combining factor + public float PacejkaGy( float sigma, float alpha ) + { + var p = Combining; + float a = p.gy2 * alpha; + float b = p.gy1 * sigma; + 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/TirePreset.cs b/Code/Base/Wheel/TirePreset.cs index 59e7013..ecfa529 100644 --- a/Code/Base/Wheel/TirePreset.cs +++ b/Code/Base/Wheel/TirePreset.cs @@ -1,12 +1,101 @@ using Sandbox; +using System; namespace VeloX; -[GameResource( "Wheel Friction", "whfric", "Wheel Friction", Category = "VeloX", Icon = "radio_button_checked" )] +[GameResource( "Wheel Friction", "tire", "Wheel Friction", Category = "VeloX", Icon = "radio_button_checked" )] public class TirePreset : GameResource { - public FrictionPreset Longitudinal { get; set; } - public FrictionPreset Lateral { get; set; } - public FrictionPreset Aligning { get; set; } + + [Property] public Pacejka Pacejka { get; set; } + + public float RollResistanceLin { get; set; } = 1E-3f; + public float RollResistanceQuad { get; set; } = 1E-6f; + + public float GetRollingResistance( float velocity, float resistance_factor ) + { // surface influence on rolling resistance + float resistance = resistance_factor * RollResistanceLin; + + // heat due to tire deformation increases rolling resistance + // approximate by quadratic function + resistance += velocity * velocity * RollResistanceQuad; + + return resistance; + } + + public void ComputeSlip( float vlon, float vlat, float vrot, out float slip_ratio, out float slip_angle ) + { + float rvlon = 1 / MathF.Max( MathF.Abs( vlon ), 1E-3f ); + 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 + public static float ComputeCamberAngle( float sin_camber ) + { + float sc = Math.Clamp( sin_camber, -0.3f, 0.3f ); + return ((1 / 6.0f) * (sc * sc) + 1) * sc; + } + + 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 + + public TireState() + { + } + }; + + public void ComputeState( + float normal_force, + float rot_velocity, + float lon_velocity, + float lat_velocity, + float camber_angle, + out TireState s + ) + { + s = new TireState + { + camber = camber_angle, + friction = 1.0f + }; + + if ( normal_force * s.friction < 1E-6f ) + { + s.slip = s.slip_angle = 0; + s.fx = s.fy = s.mz = 0; + return; + } + + float Fz = Math.Min( normal_force * 1E-3f, 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 ); + s.vcam = ComputeCamberVelocity( camber_alpha.DegreeToRadian(), lon_velocity ); + s.slip = slip; + s.slip_angle = slip_angle; + s.fx = Fx; + s.fy = Fy; + } + + public static float ComputeCamberVelocity( float sa, float vx ) + { + float tansa = (1 / 3.0f * (sa * sa) + 1) * sa; + return tansa * vx; + } } diff --git a/Code/Base/Wheel/VeloXWheel.cs b/Code/Base/Wheel/VeloXWheel.cs index d0a00ed..4a53016 100644 --- a/Code/Base/Wheel/VeloXWheel.cs +++ b/Code/Base/Wheel/VeloXWheel.cs @@ -1,5 +1,8 @@ using Sandbox; +using Sandbox.Services; using System; +using System.Collections.Specialized; +using System.Numerics; using System.Text.RegularExpressions; namespace VeloX; @@ -13,15 +16,10 @@ public partial class VeloXWheel : Component [Property] public float Mass { get; set; } = 20; [Property] public float RollingResistance { get; set; } = 20; [Property] public float SlipCircleShape { get; set; } = 1.05f; - - public FrictionPreset LongitudinalFrictionPreset => WheelFriction.Longitudinal; - public FrictionPreset LateralFrictionPreset => WheelFriction.Lateral; - public FrictionPreset AligningFrictionPreset => WheelFriction.Aligning; - - [Property] public TirePreset WheelFriction { get; set; } + [Property] public TirePreset TirePreset { get; set; } [Property] public float Width { get; set; } = 6; - [Sync] public float SideSlip { get; private set; } - [Sync] public float ForwardSlip { get; private set; } + 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; @@ -30,6 +28,7 @@ public partial class VeloXWheel : Component [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; @@ -107,115 +106,21 @@ public partial class VeloXWheel : Component Spin -= angularVelocity.MeterToInch() * dt; - - var steerRotated = entityAngles.RotateAroundAxis( Vector3.Up, vehicle.SteerAngle.yaw * SteerMultiplier + ToeAngle ); - var camberRotated = steerRotated.RotateAroundAxis( Vector3.Forward, -CamberAngle ); - var angularVelocityRotated = camberRotated.RotateAroundAxis( Vector3.Right, Spin ); - - WorldRotation = angularVelocityRotated; + WorldRotation = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) ) * Rotation.FromAxis( Vector3.Right, Spin ); } - private (float, float, float, float) StepLongitudinal( float Vx, float Lc, float kFx, float kSx) + private Rotation GetSteer( float steer ) { - float Tm = Torque; - float Tb = Brake * BrakePowerMax + RollingResistance; - float R = Radius.InchToMeter(); - float I = Inertia; - float Winit = angularVelocity; - float W = angularVelocity; + float angle = (steer * SteerMultiplier).DegreeToRadian(); - float VxAbs = MathF.Abs( Vx ); - float Sx; - if ( VxAbs >= 0.1f ) - Sx = (Vx - W * R) / VxAbs; + 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() ); - else - Sx = (Vx - W * R) * 0.6f; - - Sx = Math.Clamp( Sx * kSx, -1, 1 ); - - W += Tm / I * Time.Delta; - - Tb *= W > 0 ? -1 : 1; - - float TbCap = MathF.Abs( W ) * I / Time.Delta; - float Tbr = MathF.Abs( Tb ) - MathF.Abs( TbCap ); - Tbr = MathF.Max( Tbr, 0 ); - Tb = Math.Clamp( Tb, -TbCap, TbCap ); - W += Tb / I * Time.Delta; - - float maxTorque = LongitudinalFrictionPreset.Evaluate( Sx ) * Lc * kFx; - - float errorTorque = (W - Vx / R) * I / Time.Delta; - - float surfaceTorque = MathX.Clamp( errorTorque, -maxTorque, maxTorque ); - - W -= surfaceTorque / I * Time.Delta; - - float Fx = surfaceTorque / R; - - - Tbr *= W > 0 ? -1 : 1; - float TbCap2 = MathF.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; - if ( Lc < 0.001f ) - Sx = 0; - - return (W, Sx, Fx, Tcnt); - } - - private (float, float) StepLateral( float Vx, float Vy, float Lc, float kFy, float kSy) - { - float VxAbs = MathF.Abs( Vx ); - float Sy; - - if ( VxAbs > 0.1f ) - Sy = MathF.Atan( Vy / VxAbs ).RadianToDegree() * 0.01111f; - else - Sy = Vy * (0.003f / Time.Delta); - - - Sy *= kSy * 0.95f; - Sy = Math.Clamp( Sy * kSy, -1, 1 ); - float Fy = -MathF.Sign( Sy ) * LateralFrictionPreset.Evaluate( Sy ) * Lc * kFy; - if ( Lc < 0.0001f ) - Sy = 0; - - return (Sy, Fy); - } - - 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 * SlipCircleShape, - SyClamped - ); - - Vector2 slipDir = combinedSlip.Normal; - - float F = MathF.Sqrt( Fx * Fx + Fy * Fy ); - - float absSlipDirY = MathF.Abs( slipDir.y ); - - Fy = F * absSlipDirY * MathF.Sign( Fy ); - } + return Rotation.FromAxis( Vector3.Forward, -CamberAngle ) * Rotation.FromAxis( steering_axis, steering_angle.RadianToDegree() ); } @@ -226,7 +131,7 @@ public partial class VeloXWheel : Component { var pos = vehicle.WorldTransform.PointToWorld( StartPos ); - var ang = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier ); + var ang = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) ); forward = ang.Forward; right = ang.Right; @@ -263,8 +168,8 @@ public partial class VeloXWheel : Component if ( !IsOnGround ) { - SideSlip = 0; - ForwardSlip = 0; + forwardFriction = new Friction(); + sideFriction = new Friction(); return; } @@ -283,60 +188,71 @@ public partial class VeloXWheel : Component // vehicle.Body.AngularVelocity += angularVel; //} - - load = springForce - damperForce; - load = Math.Max( load, 0 ); - - var longitudinalLoadCoefficient = GetLongitudinalLoadCoefficient( load ); - var lateralLoadCoefficient = GetLateralLoadCoefficient( load ); - - float forwardSpeed = 0; - float sideSpeed = 0; - + load = Math.Max( force.z.InchToMeter(), 0 ); if ( IsOnGround ) { - forwardSpeed = vel.Dot( forward ); - sideSpeed = vel.Dot( right ); + float forwardSpeed = vel.Dot( forward ); + float sideSpeed = vel.Dot( right ); + + float camber_rad = CamberAngle.DegreeToRadian(); + + TirePreset.ComputeState( + load, + angularVelocity, + forwardSpeed, + sideSpeed, + camber_rad, + out var tireState + ); + + float linearSpeed = angularVelocity * Radius.InchToMeter(); + float F_roll = TirePreset.GetRollingResistance( linearSpeed, 1.0f ); + F_roll = -MathF.Sign( forwardSpeed ) * F_roll; + + 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; + + forwardFriction = new Friction() + { + Slip = tireState.slip, + Force = Fx_total.MeterToInch(), + Speed = forwardSpeed + }; + + sideFriction = new Friction() + { + Slip = tireState.slip_angle, + Force = tireState.fy.MeterToInch(), + Speed = sideSpeed + }; + Vector3 frictionForce = forward * forwardFriction.Force + right * sideFriction.Force; + vehicle.Body.ApplyForceAt( contactPos, force + frictionForce ); + } - (float W, float Sx, float Fx, float Tcnt) = StepLongitudinal( - forwardSpeed, - longitudinalLoadCoefficient, - 0.95f, - 0.9f - ); - - (float Sy, float Fy) = StepLateral( - forwardSpeed, - sideSpeed, - lateralLoadCoefficient, - 0.95f, - 0.9f - ); - - SlipCircle( Sx, Sy, Fx, ref Fy ); - - angularVelocity = W; - CounterTorque = Tcnt; - - forwardFriction = new Friction() + else { - Slip = Sx, - Force = Fx.MeterToInch(), - Speed = forwardSpeed - }; + // Колесо в воздухе: сбрасываем силы + forwardFriction = new Friction(); + sideFriction = new Friction(); - sideFriction = new Friction() - { - Slip = Sy, - Force = Fy.MeterToInch(), - Speed = sideSpeed - }; + // Обновление угловой скорости только от мотора/тормозов + float T_brake = Brake * BrakePowerMax; + if ( angularVelocity > 0 ) T_brake = -T_brake; + else T_brake = angularVelocity < 0 ? T_brake : -MathF.Sign( Torque ) * T_brake; + angularVelocity += (Torque + T_brake) / Inertia * Time.Delta; + } - var frictionforce = right * sideFriction.Force + forward * forwardFriction.Force; - - vehicle.Body.ApplyForceAt( contactPos, force + frictionforce ); } diff --git a/Code/Car/VeloXCar.Steering.cs b/Code/Car/VeloXCar.Steering.cs index 2088a0a..818c588 100644 --- a/Code/Car/VeloXCar.Steering.cs +++ b/Code/Car/VeloXCar.Steering.cs @@ -22,7 +22,7 @@ public partial class VeloXCar var inputSteer = Input.AnalogMove.y; var absInputSteer = Math.Abs( inputSteer ); - var sideSlip = Math.Clamp( 0, -1, 1 ); + var sideSlip = Math.Clamp( avgSideSlip, -1, 1 ); var steerConeFactor = Math.Clamp( TotalSpeed / SteerConeMaxSpeed, 0, 1 ); var steerCone = 1 - steerConeFactor * (1 - SteerConeMaxAngle); diff --git a/Editor/EngineStreamInspector.cs b/Editor/EngineStreamInspector.cs deleted file mode 100644 index 036961b..0000000 --- a/Editor/EngineStreamInspector.cs +++ /dev/null @@ -1,55 +0,0 @@ -//using Editor; -//using Editor.Assets; -//using Sandbox; -//using VeloX; -//using static Editor.Inspectors.AssetInspector; - -//[CanEdit( "asset:engstr" )] -//public class EngineStreamInspector : Widget, IAssetInspector -//{ -// EngineStream EngineStream; -// ControlSheet MainSheet; - -// public EngineStreamInspector( Widget parent ) : base( parent ) -// { -// Layout = Layout.Column(); -// Layout.Margin = 12; -// Layout.Spacing = 12; - -// MainSheet = new ControlSheet(); - -// Layout.Add( MainSheet, 1 ); - -// } - -// [EditorEvent.Hotload] -// void RebuildSheet() -// { -// if ( EngineStream is null || MainSheet is null ) -// return; - -// Layout.Clear( true ); -// var text = Layout.Add( new Editor.TextEdit() ); -// var but = Layout.Add( new Editor.Button( "Load JSON" ) ); -// but.Clicked += () => -// { -// EngineStream.LoadFromJson( text.PlainText ); -// }; - -// var so = EngineStream.GetSerialized(); - - -// so.OnPropertyChanged += _ => -// { -// EngineStream.StateHasChanged(); -// }; -// Layout.Add( ControlWidget.Create( so.GetProperty( nameof( EngineStream.Layers ) ) ) ); -// Layout.Add( ControlWidget.Create( so.GetProperty( nameof( EngineStream.Parameters ) ) ) ); -// } - -// public void SetAsset( Asset asset ) -// { -// EngineStream = asset.LoadResource(); -// RebuildSheet(); -// } -//} diff --git a/Editor/Wheel/PacejkaWidget.cs b/Editor/Wheel/PacejkaWidget.cs new file mode 100644 index 0000000..78d194c --- /dev/null +++ b/Editor/Wheel/PacejkaWidget.cs @@ -0,0 +1,90 @@ +using Editor; +using Sandbox; + +namespace VeloX; + +[CustomEditor( typeof( Pacejka ) )] +public class PacejkaWidget : ControlObjectWidget +{ + public override bool SupportsMultiEdit => false; + public override bool IncludeLabel => false; + + [CustomEditor( typeof( Pacejka.LateralForce ) )] + private class LateralForceWidget : ControlObjectWidget + { + public LateralForceWidget( SerializedProperty property ) : base( property, true ) + { + Layout = Layout.Column(); + Layout.Margin = 8f; + Layout.Spacing = 8; + foreach ( var item in TypeLibrary.GetType().Fields ) + { + var row = Layout.AddRow(); + row.Spacing = 8; + var propetry = SerializedObject.GetProperty( item.Name ); + row.Add( new Label( propetry.Name ) ); + row.Add( Create( propetry ) ); + } + } + } + + [CustomEditor( typeof( Pacejka.LongitudinalForce ) )] + private class LongitudinalForceWidget : ControlObjectWidget + { + public LongitudinalForceWidget( SerializedProperty property ) : base( property, true ) + { + Layout = Layout.Column(); + Layout.Margin = 8f; + Layout.Spacing = 8; + foreach ( var item in TypeLibrary.GetType().Fields ) + { + var row = Layout.AddRow(); + row.Spacing = 8; + var propetry = SerializedObject.GetProperty( item.Name ); + row.Add( new Label( propetry.Name ) ); + row.Add( Create( propetry ) ); + } + } + } + + [CustomEditor( typeof( Pacejka.AligningMoment ) )] + private class AligningMomentWidget : ControlObjectWidget + { + public AligningMomentWidget( SerializedProperty property ) : base( property, true ) + { + Layout = Layout.Column(); + Layout.Margin = 8f; + Layout.Spacing = 8; + foreach ( var item in TypeLibrary.GetType().Fields ) + { + var row = Layout.AddRow(); + row.Spacing = 8; + var propetry = SerializedObject.GetProperty( item.Name ); + row.Add( new Label( propetry.Name ) ); + row.Add( Create( propetry ) ); + } + } + } + + private Pacejka Pacejka; + public PacejkaWidget( SerializedProperty property ) : base( property, true ) + { + + var obj = SerializedObject; + Pacejka = obj.ParentProperty.GetValue(); + + Layout = Layout.Column(); + Layout.Margin = 8f; + Layout.Add( new Label.Body( $" {ToolTip}" ) { Color = Color.White } ); + var tabs = Layout.Add( new TabWidget( null ) ); + tabs.AddPage( nameof( Pacejka.Lateral ), null, + Layout.Add( Create( obj.GetProperty( nameof( Pacejka.Lateral ) ) ) ) + ); + tabs.AddPage( nameof( Pacejka.Longitudinal ), null, + Layout.Add( Create( obj.GetProperty( nameof( Pacejka.Longitudinal ) ) ) ) + ); + tabs.AddPage( nameof( Pacejka.Aligning ), null, + Layout.Add( Create( obj.GetProperty( nameof( Pacejka.Aligning ) ) ) ) + ); + } +} diff --git a/Editor/Wheel/TirePresetEditor.cs b/Editor/Wheel/TirePresetEditor.cs new file mode 100644 index 0000000..695161d --- /dev/null +++ b/Editor/Wheel/TirePresetEditor.cs @@ -0,0 +1,70 @@ +using Editor; +using Editor.Assets; +using Editor.Inspectors; +using Sandbox; +using static Editor.Inspectors.AssetInspector; + +namespace VeloX; + +[CanEdit( "asset:tire" )] +public class TirePresetEditor : Widget, IAssetInspector +{ + TirePreset Tire; + ControlSheet MainSheet; + TirePresetPreview TirePreview; + public TirePresetEditor( Widget parent ) : base( parent ) + { + Layout = Layout.Column(); + Layout.Margin = 4; + Layout.Spacing = 4; + + // Create a ontrolSheet that will display all our Properties + MainSheet = new ControlSheet(); + Layout.Add( MainSheet ); + + //// Add a randomize button below the ControlSheet + //var button = Layout.Add( new Button( "Randomize", "casino", this ) ); + //button.Clicked += () => + //{ + // foreach ( var prop in Test.GetSerialized() ) + // { + // // Randomize all the float values from 0-100 + // if ( prop.PropertyType != typeof( float ) ) continue; + // prop.SetValue( Random.Shared.Float( 0, 100 ) ); + // } + //}; + + Layout.AddStretchCell(); + + RebuildSheet(); + Focus(); + + } + + [EditorEvent.Hotload] + void RebuildSheet() + { + if ( Tire is null ) return; + if ( MainSheet is null ) return; + MainSheet.Clear( true ); + + var so = Tire.GetSerialized(); + so.OnPropertyChanged += x => + { + Tire.StateHasChanged(); + TirePreview.Widget.UpdatePixmap(); + }; + MainSheet.AddObject( so ); + } + + void IAssetInspector.SetAssetPreview( AssetPreview preview ) + { + TirePreview = preview as TirePresetPreview; + } + + public void SetAsset( Asset asset ) + { + Tire = asset.LoadResource(); + RebuildSheet(); + } +} diff --git a/Editor/Wheel/TirePresetPreview.cs b/Editor/Wheel/TirePresetPreview.cs new file mode 100644 index 0000000..2e08a9d --- /dev/null +++ b/Editor/Wheel/TirePresetPreview.cs @@ -0,0 +1,188 @@ +using Editor; +using Editor.Assets; +using Editor.Inspectors; +using Editor.ShaderGraph.Nodes; +using Sandbox; +using System; +using System.Collections.Generic; +using System.Threading.Tasks; + +namespace VeloX; + +[AssetPreview( "tire" )] +class TirePresetPreview( Asset asset ) : PixmapAssetPreview( asset ) +{ + + private TirePreset Tire = asset.LoadResource(); + public AssetPreviewWidget Widget { get; private set; } + [Range( 100, 10000 )] private float Load { get; set; } = 2500f; + [Range( 0, 1 )] private float Zoom { get; set; } = 0; + [Range( -10, 10 )] private float Camber { get; set; } = 0; + + public override Widget CreateWidget( Widget parent ) + { + Widget = parent as AssetPreviewWidget; + + Task.Run( async () => + { + while ( Widget != null ) + { + await MainThread.Wait(); + await Widget.UpdatePixmap(); + await Task.Delay( 100 ); + } + } ); + + return null; + } + public override Widget CreateToolbar() + { + var info = new IconButton( "settings" ); + info.Layout = Layout.Row(); + info.MinimumSize = 16; + info.MouseLeftPress = () => OpenSettings( info ); + return info; + } + static List pointCache = []; + private void DrawPacejka() + { + + float load = Load * 0.001f; + float zoom = (1.0f - Zoom) * 4.0f + 0.1f; + var tire = Tire.Pacejka; + var width = Paint.LocalRect.Width; + var height = Paint.LocalRect.Height; + + + { // draw lateral line + pointCache.Clear(); + + Paint.SetPen( Color.Red, 1 ); + float x0 = -zoom * 0.5f * 20.0f; + float xn = zoom * 0.5f * 20.0f; + float ymin = -1000.0f; + float ymax = 1000.0f; + int points = 500; + + + for ( float x = x0; x <= xn; x += (xn - x0) / points ) + { + + float yval = tire.PacejkaFy( x, load, Camber, 1.0f, out float maxforce ) / load; + float xval = width * (x - x0) / (xn - x0); + yval /= ymax - ymin; + yval = (yval + 1.0f) * 0.5f; + yval = 1.0f - yval; + yval *= height; + if ( x == x0 ) + pointCache.Add( new( xval, yval ) ); + else + pointCache.Add( new( xval, yval ) ); + } + + Paint.DrawLine( pointCache ); + } + + { // draw longitudinal line + pointCache.Clear(); + + Paint.SetPen( Color.Blue, 1 ); + float x0 = -zoom * 0.5f * 10.0f; + float xn = zoom * 0.5f * 10.0f; + float ymin = -60.0f; + float ymax = 60.0f; + int points = 500; + + for ( float x = x0; x <= xn; x += (xn - x0) / points ) + { + float yval = tire.PacejkaMz( x, load, Camber * (180.0f / MathF.PI), 1.0f ) / load; + float xval = width * (x - x0) / (xn - x0); + yval /= ymax - ymin; + yval = (yval + 1.0f) * 0.5f; + yval = 1.0f - yval; + yval *= height; + if ( x == x0 ) + pointCache.Add( new( xval, yval ) ); + else + pointCache.Add( new( xval, yval ) ); + } + Paint.DrawLine( pointCache ); + } + + { // draw aligning line + pointCache.Clear(); + + Paint.SetPen( Color.Green, 1 ); + float x0 = -zoom * 0.5f; + float xn = zoom * 0.5f; + float ymin = -1000.0f; + float ymax = 1000.0f; + int points = 500; + + for ( float x = x0; x <= xn; x += (xn - x0) / points ) + { + float yval = tire.PacejkaFx( x, load, 1.0f ) / load; + float xval = width * (x - x0) / (xn - x0); + yval /= ymax - ymin; + yval = (yval + 1.0f) * 0.5f; + yval = 1.0f - yval; + yval *= height; + if ( x == x0 ) + pointCache.Add( new( xval, yval ) ); + else + pointCache.Add( new( xval, yval ) ); + } + Paint.DrawLine( pointCache ); + } + + pointCache.Clear(); + } + + public override Task RenderToPixmap( Pixmap pixmap ) + { + Paint.ToPixmap( pixmap ); + Paint.Antialiasing = true; + + Paint.SetBrush( Color.Gray ); + Paint.DrawRect( Paint.LocalRect ); + Paint.ClearBrush(); + + Paint.SetPen( Color.Black, 1 ); + + var width = Paint.LocalRect.Width; + var height = Paint.LocalRect.Height; + float xc = width / 2; + float yc = height / 2; + + Paint.DrawLine( new( xc, 0 ), new( xc, yc * 2 ) ); + Paint.DrawLine( new( 0, yc ), new( xc * 2, yc ) ); + + DrawPacejka(); + + return Task.CompletedTask; + } + + public void OpenSettings( Widget parent ) + { + var popup = new PopupWidget( parent ) + { + IsPopup = true, + Layout = Layout.Column() + }; + + popup.Layout.Margin = 16; + + var ps = new ControlSheet(); + + ps.AddProperty( this, x => x.Load ); + ps.AddProperty( this, x => x.Zoom ); + ps.AddProperty( this, x => x.Camber ); + + popup.Layout.Add( ps ); + popup.MaximumWidth = 300; + popup.Show(); + popup.Position = parent.ScreenRect.TopRight - popup.Size; + popup.ConstrainToScreen(); + + } +}