gene( const value_type l=real_type(0), const value_type u=real_type(1) ) : v_(l_), l_(l), u_(u), c_(0)
{ assert( l < u ); }
typename std::enable_if<sizeof...(Args) == D-1 && std::is_convertible<Arg1,real_type>::value, real_type>::type
dispersion(Arg1 in1, Args... in) {
return -2.0*_t*cos(real_type(in1)) + cubic_traits<D-1>(_t).dispersion(in...);
};
Vector6
Pacejka89::getForce(const real_type& rho, const real_type& rhoDot,
const real_type& alpha, const real_type& kappa,
const real_type& gamma, const real_type& phi) const
{
// Pacejka suggests this correction to avoid singularities at zero speed
const real_type e_v = 1e-2;
// The normal force
real_type Fz = rho*mSpringConstant + mDampingConstant*rhoDot;
// The normal force cannot get negative here.
Fz = max(real_type(0), Fz);
// Pacejka's equations tend to provide infinitesimal stiff tires at low
// normal forces. Limit the input to the magic formula to a minimal
// normal load and rescale the output force according to the original
// load.
real_type FzRatio = 1;
if (Fz < mFzMin) {
FzRatio = Fz/mFzMin;
Fz = mFzMin;
}
// The normal force is mean to be in kN
Fz *= 1e-3;
//
// Longitudinal force (Pure longitudinal slip)
//
// Shape factor
real_type Cx = mB0;
// Peak factor
real_type Dx = (mB1*Fz + mB2)*Fz;
// BCD
real_type BCDx = (mB3*Fz + mB4)*Fz*exp(-mB5*Fz);
// Stiffness factor
real_type Bx = BCDx/(Cx*Dx + e_v);
// Horizonal shift
real_type SHx = (mB9*Fz + mB10);
// Vertical shift
real_type SVx = 0;
// Shifted longitudinal slip
// FIXME is this really in % ??? Also the other old model!!!
// real_type kappax = kappa + SHx;
real_type kappax = 100*kappa + SHx;
// Curvature factor
real_type Ex = (mB6*Fz + mB7)*Fz + mB8;
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ex = min(Ex, real_type(1));
// The resulting longitudinal force
real_type Bxk = Bx*kappax;
real_type Fx = Dx*sin(Cx*atan(Bxk - Ex*(Bxk - atan(Bxk)))) + SVx;
//
// Lateral force (Pure side slip)
//
// Shape factor
real_type Cy = mA0;
// Peak factor
real_type Dy = Fz*(mA1*Fz + mA2);
// BCD
real_type BCDy = mA3*sin(atan(Fz/mA4)*2)*(1 - mA5*fabs(gamma));
// Stiffness factor
real_type By = BCDy/(Cy*Dy + e_v);
// Horizonal shift
real_type SHy = mA9*Fz + mA10 + mA8*gamma;
// Vertical shift
real_type SVy = mA11*Fz*gamma + mA12*Fz + mA13;
// Shifted lateral slip
real_type alphay = rad2deg*alpha + SHy;
// Curvature factor
real_type Ey = mA6*Fz + mA7;
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ey = min(Ey, real_type(1));
// The resulting lateral force
real_type Bya = By*alphay;
real_type Fy = Dy*sin(Cy*atan(Bya - Ey*(Bya - atan(Bya)))) + SVy;
//
// Self aligning torque
//
// Shape factor
real_type Cz = mC0;
// Peak factor
real_type Dz = (mC1*Fz + mC2)*Fz;
// BCD
real_type BCDz = (mC3*Fz + mC4)*Fz*(1 - mC6*fabs(gamma))*exp(-mC5*Fz);
// Stiffness factor
real_type Bz = BCDz/(Cz*Dz + e_v);
// Horizonal shift
real_type SHz = mC11*gamma + mC12*Fz + mC13;
// Vertical shift
real_type SVz = (mC14*Fz + mC15)*Fz*gamma + mC16*Fz + mC17;
// Shifted lateral slip
real_type alphaz = rad2deg*alpha + SHz;
// Curvature factor
real_type Ez = ((mC7*Fz + mC8)*Fz + mC9)*(1 - mC10*fabs(gamma));
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ez = min(Ez, real_type(1));
// The resulting lateral force
real_type Bza = Bz*alphaz;
real_type Mz = Dz*sin(Cz*atan(Bza - Ez*(Bza - atan(Bza)))) + SVz;
return FzRatio*Vector6(Vector3(0, 0, Mz), Vector3(Fx, Fy, 1e3*Fz));
}
/** Returns an analytic std::function of the dispersion. */
ArgFunType get_dispersion(){ return tools::extract_tuple_f(std::function<real_type(arg_tuple)>([this](arg_tuple in){return dispersion(in);})); }
std::ostream& operator<<(std::ostream& out, BZPoint<D> in)
{for (size_t i=0; i<D; ++i) out << real_type(in[i]) << " "; return out; }
/*
* Test 6: pairs (real x real)
* - start with pairs [9, 11], [11, 9]
*/
static void test6(void) {
type_t tau[2];
particle_t a, b, c, d;
particle_t q[40], x;
uint32_t n;
printf("\n"
"***********************\n"
"* TEST 6 *\n"
"***********************\n");
tau[0] = real_type(&types);
tau[1] = real_type(&types);
a = pstore_labeled_particle(&store, 9, tau[0]);
b = pstore_labeled_particle(&store, 11, tau[1]);
q[0] = b;
q[1] = a;
c = pstore_tuple_particle(&store, 2, q, tau);
q[0] = a;
q[1] = b;
d = pstore_tuple_particle(&store, 2, q, tau);
printf("\nInitial objects of type tau!%"PRId32"\n", tau[0]);
print_particle_def(a);
print_particle_def(b);
printf("\n");
printf("Initial objects of type tau!%"PRId32" x tau!%"PRId32"\n", tau[0], tau[1]);
print_particle_def(c);
print_particle_def(d);
printf("\n");
printf("\nInitial sets\n");
print_particle_set(pstore_find_set_for_type(&store, tau[0]));
printf("\n");
print_particle_set(pstore_find_set_for_types(&store, 2, tau));
printf("\n\n");
// Initial array: empty
printf("Test array: ");
print_particle_array(q, 0);
printf("\n");
// create new tuples until that fails
for (n = 0; n<40; n++) {
x = get_distinct_tuple(&store, 2, tau, n, q);
if (x == null_particle) {
printf("Saturation\n");
break;
}
printf("New particle:");
print_particle_def(x);
q[n] = x;
}
printf("\nFinal sets\n");
print_particle_set(pstore_find_set_for_type(&store, tau[0]));
printf("\n");
print_particle_set(pstore_find_set_for_types(&store, 2, tau));
printf("\n\n");
}
bool
LevenbergMarquart(Function& f,
real_type t,
Vector& x,
real_type atol, real_type rtol,
unsigned *itCount,
unsigned maxit)
{
Log(NewtonMethod, Debug3) << "Start guess\nx = " << trans(x) << endl;
Matrix J;
LinAlg::MatrixFactors<real_type,0,0,LinAlg::LUTag> jacFactors;
bool converged = false;
real_type tau = 1e-1;
real_type nu = 2;
// Compute in each step a new jacobian
f.jac(t, x, J);
Log(NewtonMethod, Debug3) << "Jacobian is:\n" << J << endl;
real_type mu = tau*norm1(J);
Vector fx;
// Compute the actual error
f.eval(t, x, fx);
Vector g = trans(J)*fx;
do {
jacFactors = trans(J)*J + mu*LinAlg::Eye<real_type,0,0>(rows(x), rows(x));
Log(NewtonMethod, Debug) << "Jacobian is "
<< (jacFactors.singular() ? "singular" : "ok")
<< endl;
// Compute the search direction
Vector h = jacFactors.solve(-g);
Log(NewtonMethod, Debug) << "Solve Residual "
<< norm(trans(J)*J*h + mu*h + g)/norm(g) << endl;
// Get a better search guess
Vector xnew = x + h;
// check convergence
converged = equal(x, xnew, atol, rtol);
Log(NewtonMethod, Debug) << "Convergence test: ||h||_1 = " << norm1(h)
<< ", converged = " << converged << endl;
if (converged)
break;
f.eval(t, x, fx);
real_type Fx = norm(fx);
f.eval(t, xnew, fx);
real_type Fxnew = norm(fx);
real_type rho = (Fx - Fxnew)/(0.5*dot(h, mu*h - g));
Log(NewtonMethod, Debug) << "Rho = " << rho
<< ", Fxnew = " << Fxnew
<< ", Fx = " << Fx
<< endl;
if (0 < rho) {
Log(NewtonMethod, Debug) << "Accepted step!" << endl;
Log(NewtonMethod, Debug3) << "xnew = " << trans(xnew) << endl;
Log(NewtonMethod, Debug3) << "h = " << trans(h) << endl;
// New guess is the better one
x = xnew;
f.jac(t, x, J);
Log(NewtonMethod, Debug3) << "Jacobian is:\n" << J << endl;
// Compute the actual error
f.eval(t, x, fx);
g = trans(J)*fx;
converged = norm1(g) < atol;
Log(NewtonMethod, Debug) << "||g||_1 = " << norm1(g) << endl;
mu = mu * max(real_type(1)/3, 1-pow(2*rho-1, real_type(3)));
nu = 2;
} else {
mu = mu * nu;
nu = 2 * nu;
}
} while (!converged);
return converged;
}
real_type fudge_factor() const
{
// as a pure reduction, this routine is extra sensitive to FP math
return real_type(50);
}