Vamos Automotive Simulator git

//  The tire for a wheel.
//
//  Copyright (C) 2001-2019 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  Vamos is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  Vamos is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with Vamos.  If not, see <http://www.gnu.org/licenses/>.

#include "Tire.hpp"
#include "../geometry/Constants.hpp"
#include "../geometry/Numeric.hpp"
#include "../geometry/Parameter.hpp"
#include "../geometry/Three_Matrix.hpp"

#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>

using namespace Vamos_Body;
using namespace Vamos_Geometry;

namespace
{
const double epsilon = 1.0e-12;
const double ambient_temperature = 300.0;
const double initial_temperature = 345.0;

// The magic equation.  Slip is a percentage for longitudinal force, an angle in degrees
// for transverse force and aligning torque.
double pacejka_equation(double slip, double B, double C, double D, double E, double Sh, double Sv)
{
    return D * sin(C * atan(B * (1.0 - E) * (slip + Sh) + E * atan(B * (slip + Sh)))) + Sv;
}

// Return the slip value for which the Pacejka function is maximized.
double peak_slip(double B, double C, double E, double Sh, double guess)
{
    // The Pacejka function is maximized when this function is zero..
    auto function = [](double x, double B, double C, double E, double Sh) {
            return B * (1.0 - E) * (x + Sh) + E * atan(B * (x + Sh)) - tan(half_pi / C);
    };
    auto derivative = [](double x, double B, double C, double E, double Sh) {
        return B * (1.0 - E) + E * B / (1.0 + B * B * (x + Sh) * (x + Sh));
    };

    // Newton's method.  The shape of the function makes it converge quickly.
    double x = guess;
    double y;
    for (int i = 0; i < 10; i++)
    {
        y = function(x, B, C, E, Sh);
        if (std::abs(y) < 0.001)
            return x;
        x -= y / derivative(x, B, C, E, Sh);
    }
    return guess;
}

std::pair<double, double> get_slip(double patch_speed, const Three_Vector& hub_velocity)
{
    // Put a lower limit on the denominator to keep sigma and tan_alpha from getting out
    // of hand at low speeds.
    double denom = std::max(std::abs(hub_velocity.x), 3.0);
    return std::make_pair(100.0 * (patch_speed - hub_velocity.x) / denom,
                          -rad_to_deg(atan2(hub_velocity.y, denom)));
}
} // namespace

//----------------------------------------------------------------------------------------
Tire_Friction::Tire_Friction(const V_Long& long_parameters,
                             const V_Trans& trans_parameters,
                             const V_Align& align_parameters)
    : m_longitudital_parameters(long_parameters),
      m_transverse_parameters(trans_parameters),
      m_aligning_parameters(align_parameters)
{}

Three_Vector Tire_Friction::friction_forces(double normal_force, double friction_factor,
                                            const Three_Vector& hub_velocity, double patch_speed,
                                            double current_camber)
{
    double Fz = normal_force / 1000.0;
    double Fz2 = square(Fz);

    // Evaluate the longitudinal parameters.
    const auto& b = m_longitudital_parameters;
    double Cx = b[0];
    double Dx = friction_factor * (b[1] * Fz2 + b[2] * Fz);
    double Bx = (b[3] * Fz2 + b[4] * Fz) * exp(-b[5] * Fz) / (Cx * Dx);
    double Ex = b[6] * Fz2 + b[7] * Fz + b[8];
    double Shx = b[9] * Fz + b[10];

    // Evaluate the transverse parameters.
    const auto& a = m_transverse_parameters;
    double gamma = rad_to_deg(current_camber);
    double Cy = a[0];
    double Dy = friction_factor * (a[1] * Fz2 + a[2] * Fz);
    double By = a[3] * sin(2.0 * atan(Fz / a[4])) * (1.0 - a[5] * std::abs(gamma)) / (Cy * Dy);
    double Ey = a[6] * Fz + a[7];
    double Shy = a[8] * gamma + a[9] * Fz + a[10];
    double Svy = (a[11] * Fz + a[12]) * gamma * Fz + a[13] * Fz + a[14];

    // Evaluate the aligning parameters.
    const auto& c = m_aligning_parameters;
    double Cz = c[0];
    double Dz = friction_factor * (c[1] * Fz2 + c[2] * Fz);
    double Bz = (c[3] * Fz2 + c[4] * Fz) * (1.0 - c[6] * std::abs(gamma)) * exp(-c[5] * Fz)
                / (Cz * Dz);
    double Ez = (c[7] * Fz2 + c[8] * Fz + c[9]) * (1.0 - c[10] * std::abs(gamma));
    double Shz = c[11] * gamma + c[12] * Fz + c[13];
    double Svz = (c[14] * Fz2 + c[15] * Fz) * gamma + c[16] * Fz + c[17];

    // Longitudinal traction is not independent of transverse traction.  If the tire is
    // sliding, it's sliding for both.  Combine the slip ratios by normalizing relative to
    // the peak of the force function.
    auto [sigma, alpha] = get_slip(patch_speed, hub_velocity);
    m_peak_slip_ratio = peak_slip(Bx, Cx, Ex, Shx, m_peak_slip_ratio);
    Three_Vector slip;
    if (m_peak_slip_ratio > epsilon)
        slip.x = sigma / m_peak_slip_ratio;
    m_peak_slip_angle = peak_slip(By, Cy, Ey, Shy, m_peak_slip_angle);
    if (m_peak_slip_angle > epsilon)
        slip.y = alpha / m_peak_slip_angle;
    m_peak_aligning_angle = peak_slip(Bz, Cz, Ez, Shz, m_peak_aligning_angle);
    if (m_peak_aligning_angle > epsilon)
        slip.z = alpha / m_peak_aligning_angle;

    double slip_xy = Three_Vector(slip.x, slip.y, 0.0).magnitude();
    double Fx = pacejka_equation(sign(slip.x) * slip_xy
                                 * m_peak_slip_ratio, Bx, Cx, Dx, Ex, Shx, 0.0);
    double Fy = pacejka_equation(sign(slip.y) * slip_xy
                                 * m_peak_slip_angle, By, Cy, Dy, Ey, Shy, Svy);

    double slip_xz = Three_Vector(slip.x, 0.0, slip.z).magnitude();
    double Mz = pacejka_equation(slip_xz * m_peak_aligning_angle, Bz, Cz, Dz, Ez, Shz, Svz);

    if (slip_xy > epsilon)
    {
        Fx *= std::abs(slip.x) / slip_xy;
        Fy *= std::abs(slip.y) / slip_xy;
    }
    if (slip_xz > epsilon)
        Mz *= slip.z / slip_xz;

    m_slide = hub_velocity.magnitude() > 0.1 && normal_force > 0.0 ? slip_xy : 0.0;
    // Construct the friction vector.  The z-component is the aligning torque.
    return Three_Vector(Fx, Fy, Mz);
}

//----------------------------------------------------------------------------------------
Tire::Tire(double radius, double rolling_resistance_1, double rolling_resistance_2,
           const Tire_Friction& friction, double hardness, double inertia)
    : m_radius(radius),
      m_rolling_resistance({rolling_resistance_1, rolling_resistance_2}),
      m_tire_friction(friction),
      m_hardness(hardness),
      m_rotational_inertia(inertia),
      m_temperature(initial_temperature)
{}

void Tire::input(const Three_Vector& velocity, double patch_speed,
                 const Three_Vector& normal_force, double camber, double torque, bool is_locked,
                 const Material& surface_material)
{
    m_normal_force = normal_force.magnitude();

    // m_force is the force of the road on the tire.  The force of the tire on the body
    // must be calculated.  The transverse component won't change, but the longitudinal
    // component will be affected by the tire's ability to move in that direction, and by
    // applied foces (acceleration and braking).

    // Skip this step if we don't have a surface yet.
    if (surface_material.type() == Material::UNKNOWN)
        return;

    if (m_normal_force <= 0.0)
    {
        reset();
        return;
    }

    // Get the friction force (components 0 and 1) and the aligning torque (component 2).
    double grip = std::max(m_temperature / 380.0 - m_wear, 0.3);
    m_surface_friction = surface_material.friction_factor();
    auto friction_force = m_tire_friction.friction_forces(
        m_normal_force * grip, m_surface_friction, velocity, patch_speed, camber);

    // the frictional force opposing the motion of the contact patch.
    m_force = Three_Vector(friction_force.x, friction_force.y, 0.0);

    // Apply the reaction torque from acceleration or braking.  In normal conditions this
    // torque comes from friction.  However, the frictional force can sometimes be large
    // when no acceleration or braking is applied.  For instance, when you run into a
    // gravel trap.  In any case, the reaction torque should never be larger than the
    // applied accelerating or braking torque.
    double reaction = force().x * m_radius;
    if ((torque > 0.0 && reaction > torque) || (torque < 0.0 && reaction < torque))
        reaction = torque;

    m_torque = Three_Vector(0.0, -reaction, friction_force.z);

    if (!is_locked)
    {
        double rolling_1 = m_rolling_resistance[0];
        if (patch_speed < 0.0)
            rolling_1 *= -1.0;
        // Include constant and quadratic rolling resistance.
        double rolling = surface_material.rolling_resistance_factor()
            * (rolling_1 + m_rolling_resistance[1] * square(patch_speed));
        torque -= (force().x + rolling) * m_radius;
    }
    // Add the suface drag.
    m_force = force()
              - surface_material.drag_factor() * Three_Vector(velocity.x, velocity.y, 0.0);
}

void Tire::propagate(double time)
{
    // My made-up model of tire heating and wear.
    static const double stress_heating = 2e-4;
    static const double abrasion_heating = 1e-1;
    static const double wear = 1e-8;
    static const double cooling = 5e-3;

    const double dT = m_temperature - ambient_temperature;
    if (slide() > 0.0)
    {
        // Forces applied through the tire flex, stretch, and compress the rubber
        // heating it up.  Slipping results in heating due to sliding friction.
        const double F = (force() + m_normal_force * Three_Vector::Z).magnitude();
        const double friction = slide() * m_surface_friction;
        const double heat = time * (stress_heating * F / m_hardness + abrasion_heating * friction);
        m_temperature += heat;

        // Slipping wears the tire through abrasion.  The tire wears more quickly
        // at high temperature.
        m_wear += time * wear * friction * pow(dT, 2);
    }
    // Cooling is proportional to difference from ambient.
    m_temperature -= time * cooling * dT;
}

Three_Vector Tire::contact_position() const
{
    return Three_Vector::Z * -m_radius;
}

void Tire::reset()
{
    m_force.zero();
    m_torque.zero();
    m_temperature = initial_temperature;
    m_wear = 0.0;
}