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 }; } } }