304 lines
9.2 KiB
C#
304 lines
9.2 KiB
C#
using Sandbox;
|
|
using Sandbox.Services;
|
|
using System;
|
|
|
|
namespace VeloX;
|
|
public class Pacejka
|
|
{
|
|
public struct LateralForce()
|
|
{
|
|
[Description( "Shape factor" )]
|
|
[Range( 1, 3 )] public float a0 { get; set; } = 1.4f; // 0
|
|
|
|
[Description( "Load infl on lat friction coeff (*1000) (1/kN)" )]
|
|
[Range( -100, 100 )] public float a1 { get; set; } = -0f; // 1
|
|
|
|
[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 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) );
|
|
}
|
|
}
|