347 lines
11 KiB
C#
347 lines
11 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
using System;
|
|
|
|
public class PacejkaMagicParams{
|
|
public double B;
|
|
public double C;
|
|
public double D;
|
|
public double E;
|
|
public double Sh;
|
|
public double Sv;
|
|
}
|
|
|
|
[Serializable]
|
|
public class PacejkaCoeffs2 {
|
|
public double Fz0 = 3000;
|
|
|
|
//longitudinal
|
|
[Header("Longitudinal")]
|
|
public double p_cx1 = 1.65;
|
|
public double p_dx1 = 1;
|
|
public double p_dx2;
|
|
public double p_ex1 = -0.5;
|
|
public double p_ex2;
|
|
public double p_ex3;
|
|
public double p_ex4;
|
|
public double p_hx1;
|
|
public double p_hx2;
|
|
public double p_vx1;
|
|
public double p_vx2;
|
|
public double p_kx1 = 12;
|
|
public double p_kx2 = 10;
|
|
public double p_kx3 = -0.6;
|
|
|
|
//lateral
|
|
[Header("Lateral")]
|
|
public double p_cy1 = 1.3;
|
|
public double p_hy1;
|
|
public double p_hy2;
|
|
public double p_ky6 = 2.5;
|
|
public double p_ky7;
|
|
public double p_vy3 = 0.15;
|
|
public double p_vy4;
|
|
public double p_ky1 = 10;
|
|
public double p_ky3;
|
|
public double p_ky4 = 2;
|
|
public double p_ky2 = 1.5;
|
|
public double p_ky5;
|
|
public double p_vy1;
|
|
public double p_vy2;
|
|
public double p_dy1 = 1;
|
|
public double p_dy2;
|
|
public double p_dy3;
|
|
public double p_ey1 = -1;
|
|
public double p_ey2;
|
|
public double p_ey3;
|
|
public double p_ey4;
|
|
public double p_ey5;
|
|
|
|
//align torque
|
|
[Header("Self alignment")]
|
|
public double q_cz1 = 1.05;
|
|
public double q_hz1;
|
|
public double q_hz2;
|
|
public double q_hz3;
|
|
public double q_hz4;
|
|
public double q_dz1 = 0.12;
|
|
public double q_dz2 = -0.03;
|
|
public double q_dz3;
|
|
public double q_dz4 = -1;
|
|
public double q_dz6;
|
|
public double q_dz7;
|
|
public double q_dz8 = 0.6;
|
|
public double q_dz9 = 0.2;
|
|
public double q_dz10;
|
|
public double q_dz11;
|
|
public double q_ez1 = -10;
|
|
public double q_ez2;
|
|
public double q_ez3;
|
|
public double q_ez4;
|
|
public double q_ez5;
|
|
public double q_bz1 = 6;
|
|
public double q_bz2 = -4;
|
|
public double q_bz3 = 0.6;
|
|
public double q_bz5;
|
|
public double q_bz6;
|
|
public double q_bz9;
|
|
public double q_bz10 = 0.7;
|
|
|
|
//Cobmined friction
|
|
[Header("Combined longitudnal")]
|
|
public double r_cx1 = 1;
|
|
public double r_hx1;
|
|
public double r_bx1 = 5;
|
|
public double r_bx2 = 8;
|
|
public double r_bx3;
|
|
public double r_ex1;
|
|
public double r_ex2;
|
|
|
|
[Header("Combined lateral")]
|
|
public double r_by1 = 7;
|
|
public double r_by2 = 2.5;
|
|
public double r_by3;
|
|
public double r_by4;
|
|
public double r_cy1 = 1;
|
|
public double r_ey1;
|
|
public double r_ey2;
|
|
public double r_hy1 = 0.02;
|
|
public double r_hy2;
|
|
public double r_vy1;
|
|
public double r_vy2;
|
|
public double r_vy3 = -0.2;
|
|
public double r_vy4 = 14;
|
|
public double r_vy5 = 1.9;
|
|
public double r_vy6 = 10;
|
|
|
|
[Header("Combined moment")]
|
|
public double s_sz1;
|
|
public double s_sz2 = -0.1;
|
|
public double s_sz3 = -1.0;
|
|
public double s_sz4;
|
|
}
|
|
|
|
public class Pacejka {
|
|
|
|
|
|
|
|
public static Double3 CalcPacejka(Double3 Vc, double w, double r0, double re, double Fz, double camber, PacejkaCoeffs2 c, bool combined)
|
|
{
|
|
//r0 - wheel unloaded radius
|
|
//re - wheel effective radius
|
|
//w - wheel alngular velocity
|
|
//Vc => X:lon, Y: lat
|
|
double Fz0 = c.Fz0;
|
|
Double3 result = new Double3();
|
|
|
|
double[] dzeta = new double[] {1, 1, 1, 1, 1, 1, 1, 1, 1};
|
|
|
|
|
|
|
|
//Longitudinal
|
|
double eps_k = 0.001;
|
|
double eps_vx = 0.001;
|
|
double eps_x = 0.001;
|
|
double eps_y = 0.001;
|
|
double eps_v = 0.001;
|
|
|
|
double Vr = re * w; //contact patch velocity
|
|
double Vsx = Vc.x - Vr; //slip velocity
|
|
double dfz = (Fz - Fz0)/Fz0;
|
|
double mu_x = (c.p_dx1 + c.p_dx2*dfz); //>0
|
|
double Shx = (c.p_hx1 + c.p_hx2*dfz);
|
|
double k = -(Vsx / (eps_vx + Math.Abs(Vc.x)));
|
|
double kx = k + Shx;
|
|
double Svx = Fz*(c.p_vx1 + c.p_vx2*dfz)*(Math.Abs(Vc.x)/(eps_vx + Math.Abs(Vc.x)))*dzeta[1];
|
|
double Cx = c.p_cx1; //>0
|
|
double Dx = mu_x*Fz* dzeta[1]; //>0
|
|
double Ex = (c.p_ex1 + c.p_ex2*dfz + c.p_ex3*dfz*dfz)*(1.0 - c.p_ex4*Math.Sign(kx)); //<=1
|
|
double Kxk = Fz*(c.p_kx1+c.p_kx2*dfz)*Math.Exp(c.p_kx3*dfz);
|
|
double Bx = Kxk/(Cx*Dx + eps_x);
|
|
double Fx0 = Dx*Math.Sin(Cx*Math.Atan(Bx*kx - Ex*(Bx*kx - Math.Atan(Bx*kx)))) + Svx;
|
|
|
|
//Lateral
|
|
double gamma = camber;
|
|
double gamma_s = Math.Sin(gamma);
|
|
double a_star = -(Vc.y / (eps_vx + Math.Abs(Vc.x)));
|
|
double Svyy = Fz*(c.p_vy3 + c.p_vy4*dfz)*gamma_s* dzeta[2];
|
|
double Kyy0 = Fz*(c.p_ky6 + c.p_ky7*dfz);
|
|
double Kya = c.p_ky1*Fz0*Math.Sin(c.p_ky4*Math.Atan(Fz/((c.p_ky2 + c.p_ky5*gamma_s*gamma_s)*Fz0)))/
|
|
(1.0 + c.p_ky3*gamma_s*gamma_s)*dzeta[3];
|
|
double Shy = (c.p_hy1 + c.p_hy2 * dfz) + (Kyy0 * gamma_s - Svyy) * dzeta[0] / (Kya + eps_k) + dzeta[4] - 1;
|
|
double ay = a_star + Shy;
|
|
double Svy = Fz * (c.p_vy1 + c.p_vy2 * dfz) * dzeta[2] + Svyy;
|
|
double Cy = c.p_cy1; //>0
|
|
double mu_y = (c.p_dy1 + c.p_dy2*dfz)/(1.0 + c.p_dy3*gamma_s*gamma_s); //>0
|
|
double Dy = mu_y*Fz*dzeta[2];
|
|
double By = Kya/(Cy*Dy + eps_y);
|
|
double Ey = (c.p_ey1 + c.p_ey2*dfz)*(1.0 + c.p_ey5*gamma_s*gamma_s - (c.p_ey3 + c.p_ey4*gamma_s)*Math.Sign(ay)); // <= 1
|
|
double Fy0 = Dy*Math.Sin(Cy*Math.Atan(By*ay - Ey*(By*ay - Math.Atan(By*ay)))) + Svy;
|
|
|
|
//align torque
|
|
double Vcs = Vc.magnitude + eps_v;
|
|
double cos_a_s = Vc.x/Vcs;
|
|
double Sht = c.q_hz1 + c.q_hz2*dfz + (c.q_hz3 + c.q_hz4*dfz)*gamma_s;
|
|
double Dr = Fz*r0*((c.q_dz6 + c.q_dz7*dfz)*dzeta[2] + (c.q_dz8 + c.q_dz9*dfz)*gamma_s*dzeta[0] +
|
|
(c.q_dz10 + c.q_dz11*dfz)*gamma_s*Math.Abs(gamma_s)*dzeta[0])*cos_a_s*Math.Sign(Vc.x) +
|
|
dzeta[8] - 1.0;
|
|
|
|
double Dt0 = Fz*(r0/Fz0)*(c.q_dz1 + c.q_dz2*dfz)*Math.Sign(Vc.x);
|
|
double Dt = Dt0*(1.0 + c.q_dz3*Math.Abs(gamma_s) + c.q_dz4*gamma_s*gamma_s)*dzeta[5];
|
|
|
|
double at = a_star + Sht;
|
|
double Ct = c.q_cz1; //>0
|
|
double Bt = (c.q_bz1 + c.q_bz2*dfz + c.q_bz3*dfz*dfz)*
|
|
(1.0 + c.q_bz5*Math.Abs(gamma_s) + c.q_bz6*gamma_s*gamma_s); //>0
|
|
|
|
double Et = (c.q_ez1 + c.q_ez2*dfz + c.q_ez3*dfz*dfz)*
|
|
(1.0 + (c.q_ez4 + c.q_ez5*gamma_s)*(2.0/3.14159265358979)*Math.Atan(Bt*Ct*at)); //<= 1
|
|
|
|
double Cr = dzeta[7];
|
|
double Kya_s = Kya + eps_k;
|
|
double Shf = Shy + Svy/Kya_s;
|
|
double ar = a_star + Shf;
|
|
double Br = (c.q_bz9 + c.q_bz10*By*Cy)*dzeta[6];
|
|
double Mzr0 = Dr*Math.Cos(Cr*Math.Atan(Br*ar));
|
|
|
|
double t0 = Dt*Math.Cos(Ct*Math.Atan(Bt*at - Et*(Bt*at - Math.Atan(Bt*at))))*cos_a_s;
|
|
|
|
double Mz0_s = -t0 * Fy0;
|
|
double Mz0 = Mz0_s + Mzr0;
|
|
|
|
result.x = Fx0;
|
|
result.y = Fy0;
|
|
result.z = Mz0;
|
|
|
|
if (!combined)
|
|
{
|
|
return result;
|
|
}
|
|
|
|
//Combined longitudinal friction
|
|
double Cxa = c.r_cx1;
|
|
double Bxa = (c.r_bx1 + c.r_bx3*gamma_s*gamma_s)*Math.Cos(Math.Atan(c.r_bx2 * k));
|
|
double Shxa = c.r_hx1;
|
|
double a_s = a_star + Shxa;
|
|
double Exa = c.r_ex1 + c.r_ex2*dfz;
|
|
double Gxa0 = Math.Cos(Cxa*Math.Atan(Bxa*Shxa - Exa*(Bxa*Shxa - Math.Atan(Bxa*Shxa))));
|
|
double Gxa = Math.Cos(Cxa * Math.Atan(Bxa*a_s - Exa*(Bxa*a_s - Math.Atan(Bxa * a_s)))) / Gxa0;
|
|
double Fx = Gxa * Fx0;
|
|
|
|
//Combined lateral friction
|
|
double Cyk = c.r_cy1;
|
|
double Eyk = c.r_ey1 + c.r_ey2*dfz;
|
|
double Shyk = c.r_hy1 + c.r_hy2*dfz;
|
|
double Dvyk = mu_y*Fz*(c.r_vy1 + c.r_vy2*dfz + c.r_vy3*gamma_s)*Math.Cos(Math.Atan(c.r_vy4*a_star))*dzeta[2];
|
|
double Svyk = Dvyk*Math.Sin(c.r_vy5 * Math.Atan(c.r_vy6*k));
|
|
|
|
double Byk = (c.r_by1 + c.r_by4*gamma_s*gamma_s)*Math.Cos(Math.Atan(c.r_by2*(a_star - c.r_by3)));
|
|
double Gyk0 = Math.Cos(Cyk * Math.Atan(Byk*Shyk - Eyk*(Byk*Shyk - Math.Atan(Byk*Shyk))));
|
|
double ks = k + Shyk;
|
|
double Gyk = Math.Cos(Cyk*Math.Atan(Byk* ks - Eyk*(Byk*ks - Math.Atan(Byk * ks))))/Gyk0;
|
|
double Fy = Gyk * Fy0 + Svyk;
|
|
|
|
//Combined moment
|
|
double Fy_s = Fy - Svyk;
|
|
double dr = Kxk/Kya_s;
|
|
double a_teq = Math.Sqrt(at * at + (dr * dr) * (k * k) * Math.Sign(at));
|
|
double a_req = Math.Sqrt(ar * ar + (dr * dr) * (k * k) * Math.Sign(ar));
|
|
|
|
double s = r0*(c.s_sz1 + c.s_sz2*(Fy/Fz0) + (c.s_sz3 + c.s_sz4*dfz)*gamma_s);
|
|
double Mzr = Dr*Math.Cos(Cr * Math.Atan(Br * a_req));
|
|
double t = Dt*Math.Cos(Ct * Math.Atan(Bt * a_teq - Et*(Bt*a_teq - Math.Atan(Bt*a_teq))))* cos_a_s;
|
|
double Mz_s = -t*Fy_s;
|
|
double Mz = Mz_s + Mzr + s * Fx;
|
|
|
|
result.x = Fx;
|
|
result.y = Fy;
|
|
result.z = Mz;
|
|
return result;
|
|
}
|
|
|
|
|
|
|
|
public static double Extremum(PacejkaMagicParams p){
|
|
return p.D + p.Sv;
|
|
}
|
|
}
|
|
|
|
[Serializable]
|
|
public class PacejkaCoeffs{
|
|
//Lateral force parameters
|
|
public double a0 = 1.599f;
|
|
public double a1 = 0.0f;
|
|
public double a2 = 1688.0f;
|
|
public double a3 = 4140;
|
|
public double a4 = 3.026f;
|
|
public double a5 = -1.65f;
|
|
public double a6 = -0.3589f;
|
|
public double a7 = 1.0f;
|
|
public double a8 = 0.0f;
|
|
public double a9 = 0.0f;
|
|
public double a10 = 0.0f;
|
|
public double a11 = 0.0f;
|
|
public double a12 = 0.0f;
|
|
public double a13 = 0.0f;
|
|
public double a14 = 0.0f;
|
|
public double a15 = 1.01f;
|
|
|
|
//Longitudinal force parameters
|
|
public double b0 = 1.65f;
|
|
public double b1 = -21.3f;
|
|
public double b2 = 1144.0f;
|
|
public double b3 = 49.6f;
|
|
public double b4 = 226.0f;
|
|
public double b5 = 0.069f;
|
|
public double b6 = -0.006f;
|
|
public double b7 = 0.056f;
|
|
public double b8 = 0.486f;
|
|
public double b9 = 0.0f;
|
|
public double b10 = 0.0f;
|
|
public double b11 = 0.0f;
|
|
public double b12 = 0.0f;
|
|
|
|
//Aligning parameters
|
|
public double c0 = 2.4f;
|
|
public double c1 = -2.72f;
|
|
public double c2 = -2.28f;
|
|
public double c3 = -1.86f;
|
|
public double c4 = -2.73f;
|
|
public double c5 = 0.11f;
|
|
public double c6 = 0.03f;
|
|
public double c7 = -0.07f;
|
|
public double c8 = 0.643f;
|
|
public double c9 = -4.04f;
|
|
public double c10 = 0.03f;
|
|
public double c11 = 0.015f;
|
|
public double c12 =0.0f;
|
|
public double c13 = 0.0f;
|
|
public double c14 = -0.066f;
|
|
public double c15 = 0.945f;
|
|
public double c16 = 0.0f;
|
|
public double c17 = 0.0f;
|
|
|
|
public double[] ParamsA{
|
|
get{
|
|
return new double[]{
|
|
a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15
|
|
};
|
|
}
|
|
}
|
|
|
|
public double[] ParamsB{
|
|
get{
|
|
return new double[]{
|
|
b0,b1,b2,b3,b4,b5,b6,b7,b8,b9,b10,b11,b12
|
|
};
|
|
}
|
|
}
|
|
|
|
public double[] ParamsC{
|
|
get{
|
|
return new double[]{
|
|
c0,c1,c2,c3,c4,c5,c6,c7,c8,c9,c10,c11,c12,c13,c14,c15,c16,c17
|
|
};
|
|
}
|
|
}
|
|
} |