update
This commit is contained in:
Valera
@@ -1,4 +1,7 @@
|
||||
namespace VeloX;
|
||||
using Sandbox;
|
||||
using System;
|
||||
|
||||
namespace VeloX;
|
||||
|
||||
public abstract partial class VeloXBase
|
||||
{
|
||||
@@ -19,6 +22,20 @@ public abstract partial class VeloXBase
|
||||
foreach ( var v in Wheels )
|
||||
if ( v.AutoPhysics ) v.DoPhysics( this );
|
||||
|
||||
var totalSpeed = TotalSpeed + Math.Abs( Body.AngularVelocity.z );
|
||||
|
||||
var factor = 1 - Math.Clamp( totalSpeed / 30, 0, 1 );
|
||||
|
||||
if ( factor > 0.1f )
|
||||
{
|
||||
var vel = Body.Velocity;
|
||||
|
||||
var rt = WorldRotation.Right;
|
||||
|
||||
var force = rt.Dot( vel ) / Time.Delta * mass * factor * rt;
|
||||
Body.ApplyForce( -force );
|
||||
}
|
||||
|
||||
Body.ApplyTorque( angForce );
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,303 +1,43 @@
|
||||
using Sandbox;
|
||||
using Sandbox.Services;
|
||||
using System;
|
||||
using System.Threading;
|
||||
|
||||
namespace VeloX;
|
||||
public class Pacejka
|
||||
{
|
||||
public struct LateralForce()
|
||||
public class PacejkaPreset
|
||||
{
|
||||
[Description( "Shape factor" )]
|
||||
[Range( 1, 3 )] public float a0 { get; set; } = 1.4f; // 0
|
||||
[KeyProperty] public float B { get; set; } = 10.86f;
|
||||
[KeyProperty] public float C { get; set; } = 2.15f;
|
||||
[KeyProperty] public float D { get; set; } = 0.933f;
|
||||
[KeyProperty] public float E { get; set; } = 0.992f;
|
||||
|
||||
[Description( "Load infl on lat friction coeff (*1000) (1/kN)" )]
|
||||
[Range( -100, 100 )] public float a1 { get; set; } = -0f; // 1
|
||||
public float Evaluate( float slip ) => D * MathF.Sin( C * MathF.Atan( B * slip - E * (B * slip - MathF.Atan( B * slip )) ) );
|
||||
|
||||
[Description( "Lateral friction coefficient at load = 0 (*1000)" )]
|
||||
[Range( 1, 2500 )] public float a2 { get; set; } = 1688f; // 2
|
||||
|
||||
[Description( "Maximum stiffness (N/deg)" )]
|
||||
[Range( 1, 5000 )] public float a3 { get; set; } = 2400f; // 3
|
||||
|
||||
[Description( "Load at maximum stiffness (kN)" )]
|
||||
[Range( -100, 100 )] public float a4 { get; set; } = 6.026f; // 4
|
||||
|
||||
[Description( "Camber infiuence on stiffness (%/deg/100)" )]
|
||||
[Range( -10, 10 )] public float a5 { get; set; } = 0f; // 5
|
||||
|
||||
[Description( "Curvature change with load" )]
|
||||
[Range( -10, 10 )] public float a6 { get; set; } = -0.359f; // 6
|
||||
|
||||
[Description( "Curvature at load = 0" )]
|
||||
[Range( -10, 10 )] public float a7 { get; set; } = 1.0f; // 7
|
||||
|
||||
[Description( "Horizontal shift because of camber (deg/deg)" )]
|
||||
[Range( -10, 10 )] public float a8 { get; set; } = 0f; // 8
|
||||
|
||||
[Description( "Load influence on horizontal shift (deg/kN)" )]
|
||||
[Range( -10, 10 )] public float a9 { get; set; } = 0;// 9
|
||||
|
||||
[Description( "Horizontal shift at load = 0 (deg)" )]
|
||||
[Range( -10, 10 )] public float a10 { get; set; } = 0;// 10
|
||||
|
||||
[Description( "Camber influence on vertical shift (N/deg/kN)" )]
|
||||
[Range( -10, 100 )] public float a111 { get; set; } = 0f; // 11
|
||||
|
||||
[Description( "Camber influence on vertical shift (N/deg/kN**2" )]
|
||||
[Range( -10, 10 )] public float a112 { get; set; } = 0f; // 12
|
||||
|
||||
[Description( "Load influence on vertical shift (N/kN)" )]
|
||||
[Range( -100, 100 )] public float a12 { get; set; } = 0f; // 13
|
||||
|
||||
[Description( "Vertical shift at load = 0 (N)" )]
|
||||
[Range( -10, 10 )] public float a13 { get; set; } = 0f; // 14
|
||||
}
|
||||
|
||||
public struct LongitudinalForce()
|
||||
{
|
||||
[Description( "Shape factor" )]
|
||||
[Range( 1, 3 )] public float b0 { get; set; } = 1.65f; // 0
|
||||
|
||||
[Description( "Load infl on long friction coeff (*1000) (1/kN)" )]
|
||||
[Range( -300, 300 )] public float b1 { get; set; } = 0f; // 1
|
||||
|
||||
[Description( "Longitudinal friction coefficient at load = 0 (*1000)" )]
|
||||
[Range( 0, 10000 )] public float b2 { get; set; } = 1690f; // 2
|
||||
|
||||
[Description( "Curvature factor of stiffness (N/%/kN**2)" )]
|
||||
[Range( -100, 100 )] public float b3 { get; set; } = 0f; // 3
|
||||
|
||||
[Description( "Change of stiffness with load at load = 0 (N/%/kN)" )]
|
||||
[Range( -1000, 1000 )] public float b4 { get; set; } = 229f; // 4
|
||||
|
||||
[Description( "Change of progressivity of stiffness/load (1/kN)" )]
|
||||
[Range( -10, 10 )] public float b5 { get; set; } = 0f; // 5
|
||||
|
||||
[Description( "Curvature change with load" )]
|
||||
[Range( -10, 10 )] public float b6 { get; set; } = 0f; // 6
|
||||
|
||||
[Description( "Curvature change with load" )]
|
||||
[Range( -10, 10 )] public float b7 { get; set; } = 0f; // 7
|
||||
|
||||
[Description( "Curvature at load = 0" )]
|
||||
[Range( -10, 10 )] public float b8 { get; set; } = -10f; // 7
|
||||
|
||||
[Description( "Load influence on horizontal shift (%/kN)" )]
|
||||
[Range( -10, 10 )] public float b9 { get; set; } = 0f; // 9
|
||||
|
||||
[Description( "Horizontal shift at load = 0 (%)" )]
|
||||
[Range( -10, 10 )] public float b10 { get; set; } = 0f; // 10
|
||||
}
|
||||
|
||||
|
||||
public struct AligningMoment()
|
||||
{
|
||||
[Description( "Shape factor" )]
|
||||
[Range( 1, 7 )] public float c0 { get; set; } = 2.0f; // 0
|
||||
|
||||
[Description( "Load influence of peak value (Nm/kN**2)" )]
|
||||
[Range( -10, 10 )] public float c1 { get; set; } = -3.8f; // 1
|
||||
|
||||
[Description( "Load influence of peak value (Nm/kN)" )]
|
||||
[Range( -10, 10 )] public float c2 { get; set; } = -3.14f; // 2
|
||||
|
||||
[Description( "Curvature factor of stiffness (Nm/deg/kN**2" )]
|
||||
[Range( -10, 10 )] public float c3 { get; set; } = -1.16f; // 3
|
||||
|
||||
[Description( "Change of stiffness with load at load = 0 (Nm/deg/kN)" )]
|
||||
[Range( -100, 100 )] public float c4 { get; set; } = -7.2f; // 4
|
||||
|
||||
[Description( "Change of progressivity of stiffness/load (1/kN)" )]
|
||||
[Range( -10, 10 )] public float c5 { get; set; } = 0.0f; // 5
|
||||
|
||||
[Description( "Camber influence on stiffness (%/deg/100)" )]
|
||||
[Range( -10, 10 )] public float c6 { get; set; } = 0.0f; // 6
|
||||
|
||||
[Description( "Curvature change with load" )]
|
||||
[Range( -10, 10 )] public float c7 { get; set; } = 0.044f; // 7
|
||||
|
||||
[Description( "Curvature change with load" )]
|
||||
[Range( -10, 10 )] public float c8 { get; set; } = -0.58f; // 8
|
||||
|
||||
[Description( "Curvature at load = 0" )]
|
||||
[Range( -10, 10 )] public float c9 { get; set; } = 0.18f; // 9
|
||||
|
||||
[Description( "Camber influence of stiffness" )]
|
||||
[Range( -10, 10 )] public float c10 { get; set; } = 0.0f; // 10
|
||||
|
||||
[Description( "Camber influence on horizontal shift (deg/deg)" )]
|
||||
[Range( -10, 10 )] public float c11 { get; set; } = 0.0f; // 11
|
||||
|
||||
[Description( "Load influence on horizontal shift (deg/kN)" )]
|
||||
[Range( -10, 10 )] public float c12 { get; set; } = 0.0f; // 12
|
||||
|
||||
[Description( "Horizontal shift at load = 0 (deg)" )]
|
||||
[Range( -10, 10 )] public float c13 { get; set; } = 0.0f; // 13
|
||||
|
||||
[Description( "Camber influence on vertical shift (Nm/deg/kN**2" )]
|
||||
[Range( -10, 10 )] public float c14 { get; set; } = 0.14f; // 14
|
||||
|
||||
[Description( "Camber influence on vertical shift (Nm/deg/kN)" )]
|
||||
[Range( -10, 10 )] public float c15 { get; set; } = -1.029f; // 15
|
||||
|
||||
[Description( "Load influence on vertical shift (Nm/kN)" )]
|
||||
[Range( -10, 10 )] public float c16 { get; set; } = 0.0f; // 16
|
||||
|
||||
[Description( "Vertical shift at load = 0 (Nm)" )]
|
||||
[Range( -10, 10 )] public float c17 { get; set; } = 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 float GetPeakSlip()
|
||||
{
|
||||
float peakSlip = -1;
|
||||
float yMax = 0;
|
||||
|
||||
for ( float i = 0; i < 1f; i += 0.01f )
|
||||
{
|
||||
float y = Evaluate( i );
|
||||
if ( y > yMax )
|
||||
{
|
||||
yMax = y;
|
||||
peakSlip = i;
|
||||
}
|
||||
}
|
||||
|
||||
return peakSlip;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public PacejkaPreset Lateral { get; set; } = new();
|
||||
public PacejkaPreset Longitudinal { get; set; } = new();
|
||||
|
||||
public LateralForce Lateral { get; set; } = new();
|
||||
public LongitudinalForce Longitudinal { get; set; } = new();
|
||||
public AligningMoment Aligning { get; set; } = new();
|
||||
public CombiningForce Combining = new();
|
||||
|
||||
|
||||
|
||||
/// pacejka magic formula for longitudinal force
|
||||
public float PacejkaFx( float sigma, float Fz, float friction_coeff, out float maxforce_output )
|
||||
{
|
||||
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 * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) );
|
||||
|
||||
// scale by surface friction
|
||||
Fx *= friction_coeff;
|
||||
maxforce_output = D;
|
||||
|
||||
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 * MathF.Atan2( 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 * MathF.Sin( 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 * MathF.Sin( 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 float PacejkaFx( float slip ) => Longitudinal.Evaluate( slip );
|
||||
public float PacejkaFy( float slip ) => Lateral.Evaluate( slip );
|
||||
}
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
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;
|
||||
@@ -23,8 +25,8 @@ public partial class VeloXWheel : Component
|
||||
[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();
|
||||
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;
|
||||
@@ -64,8 +66,8 @@ public partial class VeloXWheel : Component
|
||||
private Vector3 right;
|
||||
private Vector3 up;
|
||||
|
||||
private Friction forwardFriction;
|
||||
private Friction sideFriction;
|
||||
private float forwardFriction;
|
||||
private float sideFriction;
|
||||
private Vector3 force;
|
||||
public float CounterTorque { get; private set; }
|
||||
|
||||
@@ -111,9 +113,101 @@ 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;
|
||||
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 );
|
||||
@@ -128,163 +222,95 @@ public partial class VeloXWheel : Component
|
||||
.Cylinder( Width, Radius, pos, endPos )
|
||||
.Rotated( vehicle.WorldTransform.Rotation * CylinderOffset )
|
||||
.UseRenderMeshes( false )
|
||||
.UseHitPosition( false )
|
||||
.UseHitPosition( true )
|
||||
.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;
|
||||
forward = Vector3.VectorPlaneProject( ang.Forward, Trace.Normal );
|
||||
right = Vector3.VectorPlaneProject( ang.Right, Trace.Normal );
|
||||
|
||||
var fraction = Trace.Fraction;
|
||||
|
||||
contactPos = pos - maxLen * fraction * up;
|
||||
contactPos = pos - maxLen * fraction * ang.Up;
|
||||
|
||||
LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );
|
||||
|
||||
DoSuspensionSounds( vehicle, fraction - lastFraction );
|
||||
lastFraction = fraction;
|
||||
|
||||
var normal = Trace.Normal;
|
||||
if ( !IsOnGround )
|
||||
return;
|
||||
|
||||
var vel = vehicle.Body.GetVelocityAtPoint( contactPos );
|
||||
|
||||
var offset = maxLen - (fraction * maxLen);
|
||||
var springForce = (offset * SpringStrength);
|
||||
var springForce = offset * SpringStrength;
|
||||
var damperForce = (lastSpringOffset - offset) * SpringDamper;
|
||||
|
||||
lastSpringOffset = offset;
|
||||
|
||||
// Vector3.CalculateVelocityOffset is broken (need fix)
|
||||
var velU = normal.Dot( vel );
|
||||
|
||||
var velU = Trace.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;
|
||||
vehicle.Body.CalculateVelocityOffset( -velU / Time.Delta * Trace.Normal, pos, out var linearImp, out var angularImp );
|
||||
|
||||
Vector3 r = pos - com;
|
||||
vehicle.Body.Velocity += linearImp;
|
||||
vehicle.Body.AngularVelocity += angularImp;
|
||||
vehicle.Body.CalculateVelocityOffset( Trace.HitPosition - (contactPos + Trace.Normal * velU * Time.Delta), pos, out var lin, out _ );
|
||||
|
||||
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;
|
||||
vehicle.WorldPosition += lin / Time.Delta;
|
||||
damperForce = 0;
|
||||
|
||||
}
|
||||
|
||||
force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal;
|
||||
force = (springForce - damperForce) * Trace.Normal;
|
||||
|
||||
load = Math.Max( force.z, 0 ).InchToMeter();
|
||||
load = Math.Max( springForce - damperForce, 0 );
|
||||
float R = Radius.InchToMeter();
|
||||
|
||||
float forwardSpeed = vel.Dot( forward ).InchToMeter();
|
||||
float sideSpeed = vel.Dot( right ).InchToMeter();
|
||||
|
||||
float camber_rad = CamberAngle.DegreeToRadian();
|
||||
float R = Radius.InchToMeter();
|
||||
float longitudinalLoadCoefficient = GetLongitudinalLoadCoefficient( load );
|
||||
float lateralLoadCoefficient = GetLateralLoadCoefficient( load );
|
||||
|
||||
float linearSpeed = angularVelocity * Radius.InchToMeter();
|
||||
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
|
||||
);
|
||||
|
||||
float F_roll = TirePreset.GetRollingResistance( linearSpeed, 1.0f );
|
||||
F_roll *= -Math.Clamp( linearSpeed * 0.25f, -1, 1 );
|
||||
StepLateral( forwardSpeed, sideSpeed, lateralLoadCoefficient, out float Sy, out float Fy );
|
||||
|
||||
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();
|
||||
}
|
||||
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 );
|
||||
|
||||
}
|
||||
|
||||
//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
|
||||
#if 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 );
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
@@ -34,12 +34,12 @@ public partial class VeloXCar
|
||||
var inputSteer = Input.AnalogMove.y;
|
||||
|
||||
|
||||
VelocityAngle = -SignedAngle( Body.Velocity, WorldRotation.Forward, WorldRotation.Up );
|
||||
VelocityAngle = 0;// -SignedAngle( Body.Velocity, WorldRotation.Forward, WorldRotation.Up );
|
||||
|
||||
var steerConeFactor = Math.Clamp( TotalSpeed / SteerConeMaxSpeed, 0, 1 );
|
||||
var steerCone = 1 - steerConeFactor * (1 - SteerConeMaxAngle);
|
||||
//var steerConeFactor = Math.Clamp( TotalSpeed / SteerConeMaxSpeed, 0, 1 );
|
||||
//var steerCone = 1 - steerConeFactor * (1 - SteerConeMaxAngle);
|
||||
|
||||
inputSteer = ExpDecay( this.inputSteer, inputSteer * steerCone, SteerConeChangeRate, dt );
|
||||
inputSteer = ExpDecay( this.inputSteer, inputSteer, SteerConeChangeRate, dt );
|
||||
this.inputSteer = inputSteer;
|
||||
|
||||
float target = -inputSteer * MaxSteerAngle;
|
||||
|
||||
@@ -28,44 +28,38 @@ public static class PhysicsExtensions
|
||||
value.x * (xz2 - wy2) + value.y * (yz2 + wx2) + value.z * (1.0f - xx2 - yy2)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculates the linear and angular velocities on the center of mass for an offset impulse.
|
||||
/// Calculates the linear and angular velocities on the object's center of mass for an offset impulse.
|
||||
/// </summary>
|
||||
/// <param name="physObj">The physics object</param>
|
||||
/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
|
||||
/// <param name="position">The location of the impulse in world coordinates</param>
|
||||
/// <returns>
|
||||
/// Vector1: Linear velocity from the impulse (World frame)
|
||||
/// Vector2: Angular velocity from the impulse (Local frame)
|
||||
/// </returns>
|
||||
public static (Vector3 LinearVelocity, Vector3 AngularVelocity) CalculateVelocityOffset( this PhysicsBody physObj, Vector3 impulse, Vector3 position )
|
||||
/// <param name="LinearVelocity">Linear velocity on center of mass (World frame)</param>
|
||||
/// <param name="AngularVelocity">Angular velocity on center of mass (World frame)</param>
|
||||
public static void CalculateVelocityOffset(
|
||||
this Rigidbody physObj,
|
||||
Vector3 impulse,
|
||||
Vector3 position,
|
||||
out Vector3 LinearVelocity,
|
||||
out Vector3 AngularVelocity )
|
||||
{
|
||||
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
||||
return (Vector3.Zero, Vector3.Zero);
|
||||
{
|
||||
LinearVelocity = 0;
|
||||
AngularVelocity = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
|
||||
Rotation bodyRot = physObj.PhysicsBody.Rotation;
|
||||
|
||||
Vector3 linearVelocity = impulse / physObj.Mass;
|
||||
|
||||
Vector3 r = position - physObj.MassCenter;
|
||||
|
||||
|
||||
// Calculate torque impulse in world frame: τ = r × impulse
|
||||
Vector3 torqueImpulseWorld = r.Cross( impulse );
|
||||
Rotation worldToLocal = physObj.Rotation.Inverse;
|
||||
Vector3 torqueImpulseLocal = torqueImpulseWorld.Transform( worldToLocal );
|
||||
|
||||
var InverseInertiaDiagLocal = physObj.Inertia.Inverse;
|
||||
|
||||
// Compute angular velocity change in rad/s (local frame)
|
||||
Vector3 angularVelocityRadLocal = new(
|
||||
InverseInertiaDiagLocal.x * torqueImpulseLocal.x,
|
||||
InverseInertiaDiagLocal.y * torqueImpulseLocal.y,
|
||||
InverseInertiaDiagLocal.z * torqueImpulseLocal.z
|
||||
);
|
||||
const float radToDeg = 180f / MathF.PI;
|
||||
Vector3 angularVelocityDegLocal = angularVelocityRadLocal * radToDeg;
|
||||
|
||||
return (linearVelocity, angularVelocityDegLocal);
|
||||
Vector3 r = position - com;
|
||||
Vector3 torque = Vector3.Cross( r, impulse );
|
||||
Vector3 torqueLocal = bodyRot.Inverse * torque;
|
||||
Vector3 angularVelocityLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
|
||||
AngularVelocity = bodyRot * angularVelocityLocal;
|
||||
LinearVelocity = impulse * (1 / physObj.Mass);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -74,34 +68,31 @@ public static class PhysicsExtensions
|
||||
/// <param name="physObj">The physics object</param>
|
||||
/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
|
||||
/// <param name="position">The location of the impulse in world coordinates</param>
|
||||
/// <returns>
|
||||
/// Vector1: Linear impulse on center of mass (World frame)
|
||||
/// Vector2: Angular impulse on center of mass (Local frame)
|
||||
/// </returns>
|
||||
public static (Vector3 LinearImpulse, Vector3 AngularImpulse) CalculateForceOffset(
|
||||
this PhysicsBody physObj,
|
||||
/// <param name="LinearImpulse">Linear impulse on center of mass (World frame)</param>
|
||||
/// <param name="AngularImpulse">Angular impulse on center of mass (World frame)</param>
|
||||
public static void CalculateForceOffset(
|
||||
this Rigidbody physObj,
|
||||
Vector3 impulse,
|
||||
Vector3 position )
|
||||
Vector3 position,
|
||||
out Vector3 LinearImpulse,
|
||||
out Vector3 AngularImpulse )
|
||||
{
|
||||
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
||||
{
|
||||
return (Vector3.Zero, Vector3.Zero);
|
||||
LinearImpulse = 0;
|
||||
AngularImpulse = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. Linear impulse is the same as the input impulse (conservation of momentum)
|
||||
Vector3 linearImpulse = impulse;
|
||||
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
|
||||
Rotation bodyRot = physObj.PhysicsBody.Rotation;
|
||||
|
||||
// 2. Calculate angular impulse (torque) from the offset force
|
||||
// τ = r * F (cross product of position relative to COM and force)
|
||||
Vector3 centerOfMass = physObj.MassCenter;
|
||||
Vector3 relativePosition = position - centerOfMass;
|
||||
Vector3 worldAngularImpulse = relativePosition.Cross( impulse );
|
||||
|
||||
// Convert angular impulse to local space (since we'll use it with LocalInertia)
|
||||
Rotation bodyRotation = physObj.Transform.Rotation;
|
||||
Vector3 localAngularImpulse = bodyRotation.Inverse * worldAngularImpulse;
|
||||
|
||||
return (linearImpulse, localAngularImpulse);
|
||||
Vector3 r = position - com;
|
||||
Vector3 torque = Vector3.Cross( r, impulse );
|
||||
Vector3 torqueLocal = bodyRot.Inverse * torque;
|
||||
Vector3 angularImpulseLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
|
||||
AngularImpulse = bodyRot * angularImpulseLocal;
|
||||
LinearImpulse = impulse;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user