Vamos Automotive Simulator git

//  Car.cc - a body with wheels.
//
//  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/>.
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include "Car.hpp"
#include "../geometry/Numeric.hpp"
#include "../media/Texture_Image.hpp"
#include "Aerodynamic_Device.hpp"
#include "Brake.hpp"
#include "Clutch.hpp"
#include "Dashboard.hpp"
#include "Differential.hpp"
#include "Engine.hpp"
#include "Fuel_Tank.hpp"
#include "Particle.hpp"
#include "Suspension.hpp"
#include "Tire.hpp"
#include "Transmission.hpp"
#include "Wheel.hpp"

#include <cassert>

using namespace Vamos_Body;
using namespace Vamos_Geometry;
using namespace Vamos_Media;

namespace
{
const double acceleration_average_weight = 10.0;
const double view_shift_factor = 0.003;
} // namespace

Key_Control::Key_Control(bool block)
    : m_block(block)
{
}

void Key_Control::target(double target, double time, double delay)
{
    if (m_block && m_value != m_target)
    {
        m_next = Stage{target, time, delay};
        return;
    }
    m_target = target;
    m_rate = time == 0.0 ? 0.0 : (target - m_value) / time;
    m_time = 0.0;
    m_delay = delay;
}

void Key_Control::update(double time)
{
    m_time += time;
    if (m_time < m_delay)
        return;

    if (m_rate == 0.0 || std::abs(m_target - m_value) < std::abs(m_rate * time))
    {
        end();
        if (m_next)
        {
            target(m_next->target, m_next->time, m_next->delay);
            m_next.reset();
        }
        return;
    }
    m_value += m_rate * time;
}

void Key_Control::end()
{
    m_value = m_target;
    m_time = m_delay;
}

//----------------------------------------------------------------------------------------
Car::Car(const Three_Vector &position, const Three_Matrix &orientation)
    : m_chassis(position, orientation),
      mp_fuel_tank(0),
      m_max_steer_angle(15.0),
      m_steer_exponent(1.0),
      m_slide(0.0),
      m_shift_pending(false),
      m_shift_timer(0.0),
      m_shift_delay(0.2),
      m_new_gear(0),
      m_last_gear(0),
      m_clutch_key_control(true),
      mp_front_particle(0),
      m_distance_traveled(0.0),
      m_field_of_view(60.0),
      m_pan_angle(90.0),
      m_show_dashboard_extras(false),
      m_air_density(0.0)
{
}

void Car::read(std::string data_dir, std::string car_file)
{
    // Remember the file name for re-reading.
    if (!data_dir.empty() && !car_file.empty())
    {
        m_data_dir = data_dir;
        m_car_file = car_file;
    }

    m_wheels.clear();
    m_chassis.particles().clear();
    Car_Reader reader(m_data_dir, m_car_file, this);

    // Find the bounding box for the particles.
    auto it = m_chassis.particles().cbegin();
    m_crash_box.front = (*it)->position().x;
    m_crash_box.back = m_crash_box.front;
    m_crash_box.left = (*it)->position().y;
    m_crash_box.right = m_crash_box.left;
    m_crash_box.top = (*it)->position().z;
    m_crash_box.bottom = m_crash_box.top;
    mp_front_particle = *it;
    for (; it != m_chassis.particles().end(); it++)
    {
        const auto& position = (*it)->position();
        if (position.x > m_crash_box.front)
        {
            m_crash_box.front = position.x;
            mp_front_particle = *it;
        }
        else if (position.x < m_crash_box.back)
            m_crash_box.back = position.x;
        if (position.y > m_crash_box.left)
            m_crash_box.left = position.y;
        else if (position.y < m_crash_box.right)
            m_crash_box.right = position.y;
        if (position.z > m_crash_box.top)
            m_crash_box.top = position.z;
        else if (position.z < m_crash_box.bottom)
            m_crash_box.bottom = position.z;
    }
}

void Car::set_robot_parameters(double slip_ratio, double deceleration, double lateral_acceleration)
{
    m_robot_parameters.slip_ratio = slip_ratio;
    m_robot_parameters.deceleration = deceleration;
    m_robot_parameters.lateral_acceleration = lateral_acceleration;
}

void Car::adjust_robot_parameters(double slip_ratio_factor, double deceleration_factor,
                                  double lateral_acceleration_factor)
{
    m_robot_parameters.slip_ratio += slip_ratio_factor * m_robot_parameters.slip_ratio;
    m_robot_parameters.deceleration += deceleration_factor * m_robot_parameters.deceleration;
    m_robot_parameters.lateral_acceleration
        += lateral_acceleration_factor * m_robot_parameters.lateral_acceleration;
}

void Car::propagate(double time)
{
    assert(mp_drivetrain);

    // Propagate the key controls.
    m_steer_key_control.update(time);
    m_gas_key_control.update(time);
    m_brake_key_control.update(time);
    m_clutch_key_control.update(time);
    m_pan_key_control.update(time);

    // Update the transmission.
    double gas = m_gas_key_control.value();
    if (m_shift_pending)
    {
        m_shift_timer += time;
        if (m_shift_timer > m_shift_delay)
        {
            mp_drivetrain->transmission().shift(m_new_gear);
            m_shift_pending = false;
        }
    }

    assert(mp_fuel_tank);
    if (mp_fuel_tank->empty())
        gas = 0.0;
    mp_drivetrain->engine()->out_of_gas(mp_fuel_tank->empty());
    // Update the fuel tank.
    mp_fuel_tank->consume(mp_drivetrain->engine()->fuel_rate() * time);

    static bool going = false;
    if (mp_drivetrain->transmission().get_gear() != 0
        && mp_drivetrain->clutch().pressure() != 0.0)
        going = true;

    m_slide = 0.0;
    double right_speed = 0.0;
    double left_speed = 0.0;
    for (auto wheel : m_wheels)
    {
        if (wheel->steered())
            wheel->steer(m_steer_key_control.value());

        wheel->brake(m_brake_key_control.value());
        if (!going)
            wheel->brake(1.0);

        if (mp_drivetrain && wheel->driven())
        {
            wheel->drive_torque(mp_drivetrain->torque(wheel->side()));
            (wheel->side() == RIGHT ? right_speed : left_speed) = wheel->rotational_speed();
        }
        m_slide += wheel->slide();
    }
    m_slide = std::min(1.0, m_slide / m_wheels.size());

    mp_drivetrain->input(gas, m_clutch_key_control.value(), left_speed, right_speed);
    mp_drivetrain->find_forces();
    m_chassis.find_forces();

    // Midpoint propagation
    mp_drivetrain->propagate(time / 2.0);
    m_chassis.propagate(time / 2.0);

    mp_drivetrain->find_forces();
    m_chassis.find_forces();

    mp_drivetrain->rewind();
    m_chassis.rewind();

    mp_drivetrain->propagate(time);
    m_chassis.propagate(time);
    m_chassis.end_timestep();

    m_distance_traveled += m_chassis.rotate_from_parent(m_chassis.cm_velocity()).x * time;

    // Update the smoothed acceleration.
    const double weight = std::min(acceleration_average_weight * time, 1.0);
    m_smoothed_acceleration
        = (1.0 - weight) * m_smoothed_acceleration + weight * m_chassis.acceleration();
}

void Car::steer(double angle, double time, bool direct /* = false */)
{
    if (!direct)
    {
        // non-linearity
        angle = (angle < 0.0 ? -1.0 : 1.0) * std::pow(std::abs(angle), m_steer_exponent);
        // speed sensitivity
        auto v = m_chassis.cm_velocity().dot(m_chassis.cm_velocity());
        angle *= m_max_steer_angle / (1.0 + 1.0e-4 * m_steer_speed_sensitivity * v);
    }
    m_steer_key_control.target(clip(angle, -m_max_steer_angle, m_max_steer_angle), time);
}

void Car::gas(double factor, double time)
{
    m_gas_key_control.target(factor, time);
}

void Car::brake(double factor, double time)
{
    m_brake_key_control.target(factor, time);
}

void Car::pan(double factor, double time)
{
    m_pan_key_control.target(factor * m_pan_angle, time / m_pan_angle);
}

int Car::shift_down()
{
    return shift(mp_drivetrain->transmission().get_gear() - 1);
}

int Car::shift_up()
{
    return shift(mp_drivetrain->transmission().get_gear() + 1);
}

int Car::shift(int gear)
{
    if (m_new_gear == gear)
        return gear;
    // Do the shift if gear is accessible.
    if (gear <= mp_drivetrain->transmission().num_forward_gears()
        && -gear <= mp_drivetrain->transmission().num_reverse_gears())
    {
        m_shift_pending = true;
        m_shift_timer = 0.0;
        m_last_gear = mp_drivetrain->transmission().get_gear();
        m_new_gear = gear;
    }
    return m_new_gear;
}

void Car::clutch(double factor, double time)
{
    m_clutch_key_control.target(factor, time, 0.0);
}

void Car::engage_clutch(double time)
{
    // Wait for the shift timer.
    double delay = m_shift_delay - m_shift_timer;
    m_clutch_key_control.target(1.0, time, delay);
}

void Car::disengage_clutch(double time)
{
    // Wait for the shift timer.
    double delay = m_shift_delay - m_shift_timer;
    m_clutch_key_control.target(0.0, time, delay);
}

Wheel* Car::wheel(size_t wheel_index) const
{
    return wheel_index >= m_wheels.size() ? nullptr : m_wheels[wheel_index];
}

Three_Vector Car::view_position(bool world, bool bob) const
{
    auto pos = m_driver_view;
    if (bob)
        pos -= view_shift_factor * acceleration(true);
    return world ? m_chassis.transform_to_world(pos) : pos;
}

void Car::start_engine()
{
    if (mp_drivetrain)
        mp_drivetrain->engine()->start();
    m_clutch_key_control.end();
}

void Car::reset()
{
    m_chassis.reset(0.0);
    private_reset();
}

void Car::reset(const Three_Vector &position, const Three_Matrix &orientation)
{
    m_chassis.reset(position, orientation);
    private_reset();
}

void Car::private_reset()
{
    if (mp_drivetrain)
    {
        mp_drivetrain->reset();
        shift(0);
        start_engine();
    }
}

void Car::drivetrain(Drivetrain *drive)
{
    mp_drivetrain.reset(drive);
    assert(mp_drivetrain);
}

Contact_Info Car::collision(const Three_Vector &position, const Three_Vector &velocity,
                            bool ignore_z) const
{
    auto in = m_crash_box.penetration(m_chassis.transform_from_world(position),
                                      m_chassis.transform_velocity_from_world(velocity),
                                      ignore_z);
    return Contact_Info{!in.null(), in.magnitude(), m_chassis.rotate_to_world(in), Material::METAL};
}

void Car::wind(const Vamos_Geometry::Three_Vector &wind_vector, double density)
{
    m_air_density = density;
    m_chassis.wind(wind_vector, density);
}

Three_Vector Car::chase_position() const
{
    auto v1 = m_chassis.cm_velocity().unit();
    double w1 = std::min(m_chassis.cm_velocity().magnitude(), 1.0);
    auto v2 = m_chassis.rotate_to_world(Three_Vector::X);
    double w2 = 1.0 - w1;
    return m_chassis.transform_to_world(center() - 0.1 * acceleration(true))
        + (Three_Vector::Z - 3.0 * (w1 * v1 + w2 * v2)) * length();
}

Three_Vector Car::front_position() const
{
    return m_chassis.transform_to_world(front());
}

void Car::set_front_position(const Three_Vector &pos)
{
    m_chassis.set_position(pos - m_chassis.rotate_to_world(front()));
}

Three_Vector Car::target_position() const
{
    return m_chassis.transform_to_world(center() + Three_Vector::X * target_distance());
}

double Car::target_distance() const
{
    return 2.0 * length() + 0.2 * m_chassis.cm_velocity().magnitude();
}

double Car::grip() const
{
    double g = 0.0;
    for (const auto* wheel : m_wheels)
        g += wheel->grip();
    return g / m_wheels.size();
}

Three_Vector Car::acceleration(bool smooth) const
{
    return smooth ? m_smoothed_acceleration : m_chassis.acceleration();
}

Three_Vector Car::Crash_Box::penetration(const Three_Vector &point, const Three_Vector &velocity,
                                         bool ignore_z) const
{
    if (!within(point, ignore_z))
        return Three_Vector::ZERO;

    if (velocity.x != 0.0 && is_in_range(point.x, back, front))
    {
        double x_limit = closer(point.x, back, front);
        if (((point.x - back < front - point.x) && velocity.x > 0.0)
            || ((point.x - back >= front - point.x) && velocity.x < 0.0))
        {
            Three_Vector x_intercept(x_limit,
                                     intercept(x_limit, point.x, point.y, velocity.y / velocity.x),
                                     intercept(x_limit, point.x, point.z, velocity.z / velocity.x));
            if (is_in_range(x_intercept.y, right, left)
                && (ignore_z || is_in_range(x_intercept.z, bottom, top)))
                return Three_Vector::X * (x_limit - point.x);
        }
    }

    if (velocity.y != 0.0 && is_in_range(point.y, right, left))
    {
        double y_limit = closer(point.y, right, left);
        if ((point.y - right < left - point.y && velocity.y > 0.0)
            || (point.y - right >= left - point.y && velocity.y < 0.0))
        {
            Three_Vector y_intercept(intercept(y_limit, point.y, point.x, velocity.x / velocity.y),
                                     y_limit,
                                     intercept(y_limit, point.y, point.z, velocity.z / velocity.y));
            if (is_in_range(y_intercept.x, back, front)
                && (ignore_z || is_in_range(y_intercept.z, bottom, top)))
                return Three_Vector::Y * (y_limit - point.y);
        }
    }

    if (!ignore_z && velocity.z != 0.0 && is_in_range(point.z, bottom, top))
    {
        double z_limit = closer(point.z, bottom, top);
        if ((point.z - bottom < top - point.z && velocity.z > 0.0)
            || (point.z - bottom >= top - point.z && velocity.z < 0.0))
        {
            Three_Vector z_intercept(intercept(z_limit, point.z, point.x, velocity.x / velocity.z),
                                     intercept(z_limit, point.z, point.y, velocity.y / velocity.z),
                                     z_limit);
            if (is_in_range(z_intercept.x, back, front) && is_in_range(z_intercept.y, right, left))
                return Three_Vector::Z * (z_limit - point.z);
        }
    }

    return Three_Vector::ZERO;
}

bool Car::Crash_Box::within(const Three_Vector &position, bool ignore_z) const
{
    // Return true if the position is within the crash box.  ignore_z == true will only
    // consider the x and y coordinates -- useful for collisions with the edge of the
    // world.
    return is_in_range(position.x, back, front) && is_in_range(position.y, right, left)
        && (ignore_z || is_in_range(position.z, bottom, top));
}