348 lines
9.6 KiB
C#
348 lines
9.6 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 = 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);
|
||
}
|
||
}
|