Vamos Automotive Simulator git

//  Copyright (C) 2001--2004 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 "World.hpp"
#include "Driver.hpp"
#include "Timing_Info.hpp"

#include "../body/Car.hpp"
#include "../geometry/Three_Vector.hpp"
#include "../track/Strip_Track.hpp"

#include <cassert>
#include <limits>

using namespace Vamos_Body;
using namespace Vamos_Geometry;
using namespace Vamos_World;
using namespace Vamos_Track;

const double slipstream_time_constant = 0.7;

//-----------------------------------------------------------------------------
Car_Information::Car_Information(Car* car, Driver* driver)
    : car(car),
      driver(driver),
      m_record(1000)
{}

void Car_Information::reset()
{
    road_index = 0;
    segment_index = 0;
    if (driver)
        driver->reset();
    car->reset();
}

void Car_Information::propagate(double time_step, double total_time,
                                const Three_Vector& track_position,
                                const Three_Vector& pointer_position)
{
    m_record.push_back(Record(total_time, car, track_position));
    m_pointer_position = pointer_position;
    if (driver != 0)
        driver->propagate(time_step);
    car->propagate(time_step);
}

const Three_Vector& Car_Information::track_position() const
{
    return m_record.empty() ? Three_Vector::ZERO : m_record.back().m_track_position;
}

//-----------------------------------------------------------------------------
Car_Information::Record::Record(double time, Car* car, const Three_Vector& track_position)
    : m_time(time),
      m_track_position(track_position),
      m_position(car->chassis().position()),
      m_orientation(car->chassis().orientation())
{}

//-----------------------------------------------------------------------------
World::World(Vamos_Track::Strip_Track& track, const Atmosphere& atmosphere)
    : m_track(track),
      m_atmosphere(atmosphere)
{}

void World::start(bool qualify, size_t laps_or_minutes)
{
    // Here qualifying implies no start sequence, but that's not necessarily the case.
    // That's why we pass !qualify to the constructor and also call set_qualifying();
    mp_timing.reset(
        new Timing_Info(m_cars.size(), m_track.timing_lines(), !qualify && m_cars.size() > 1));
    if (qualify)
    {
        mp_timing->set_qualifying();
        mp_timing->set_time_limit(laps_or_minutes);
    }
    else
        mp_timing->set_lap_limit(laps_or_minutes);
}

inline Three_Vector rotation_term(const Inertia_Tensor& I, const Three_Vector& r,
                                  const Three_Vector& n)
{
    return (I.inverse() * (r.cross(n))).cross(r);
}

Three_Vector impulse(const Three_Vector& r1, double m1, const Inertia_Tensor& I1,
                     const Three_Vector& r2, double m2, const Inertia_Tensor& I2,
                     const Three_Vector& v, double restitution, double friction,
                     const Three_Vector& normal)
{
    return -normal * (1.0 + restitution) * v.dot(normal)
               / (normal.dot(normal) * (1.0 / m1 + 1.0 / m2)
                  + (rotation_term(I1, r1, normal) + rotation_term(I2, r2, normal)).dot(normal))
           + friction * (v.project(normal) - v);
}

Three_Vector impulse(const Three_Vector& r, const Three_Vector& v, double m,
                     const Inertia_Tensor& I, double restitution, double friction,
                     const Three_Vector& normal)
{
    return -normal * (1.0 + restitution) * v.dot(normal)
               / (normal.dot(normal) / m + rotation_term(I, r, normal).dot(normal))
           + friction * (v.project(normal) - v);
}

void World::propagate_cars(double time_step)
{
    for (auto& info : m_cars)
    {
        double air_density_factor = 1.0;
        if (m_cars_can_interact)
        {
            for (auto& other : m_cars)
            {
                if (&info == &other)
                    continue;
                collide(&info, &other);
                air_density_factor
                    = std::min(air_density_factor, slipstream_air_density_factor(info, other));
            }
        }
        info.car->wind(m_atmosphere.velocity, m_atmosphere.density * air_density_factor);
        info.propagate(time_step, mp_timing->total_time(),
                       m_track.track_coordinates(info.car->front_position(), info.road_index,
                                                 info.segment_index),
                       m_track.track_coordinates(info.car->target_position(), info.road_index,
                                                 info.segment_index));
        interact(info.car, info.road_index, info.segment_index);
    }
}

// Return the fraction of air density at car1 due to the slipstream of car2.
double World::slipstream_air_density_factor(Car_Information& car1, Car_Information& car2)
{
    if (car1.road_index != car2.road_index)
        return 1.0;

    const auto& p1 = car1.track_position();
    const auto& p2 = car2.track_position();

    const Vamos_Track::Road& road = m_track.get_road(car1.road_index);
    if (road.distance(p1.x, p2.x) > 0.0)
        return 1.0;

    const double now = mp_timing->total_time();

    // Go through car2's history starting with the most recent event to find out
    // how long ago car2 was at car1's position. Calculate the reduction in air
    // density as a function of that time and distance across the track.
    for (size_t i = car2.m_record.size(); i > 0; i--)
    {
        const double arg = (now - car2.m_record[i - 1].m_time) / slipstream_time_constant;
        // If we're more than 5 time constants behind the density factor would be
        // at least 1-exp(-5) ~ 0.993. Not far enough away from 1.0 to worry about.
        if (arg > 5.0)
            break;

        const auto& p2 = car2.m_record[i - 1].m_track_position;
        if (road.distance(p1.x, p2.x) > 0.0)
        {
            double longitudinal = std::exp(-arg);
            double transverse
                = longitudinal * std::max(1.0 - std::abs(p2.y - p1.y) / car2.car->width(), 0.0);
            return 1.0 - longitudinal * transverse;
        }
    }

    return 1.0;
}

void World::interact(Car* car, size_t road_index, size_t segment_index)
{
    // Get the impulse from contact as an optional.
    auto local_impulse = [car](const Three_Vector& pos, const Three_Vector& velocity,
                               const Material& material, const Contact_Info& info) {
        std::optional<Three_Vector> j;
        if (info.contact)
            j = impulse(car->chassis().world_moment(pos), velocity, car->chassis().mass(),
                        car->chassis().inertia(),
                        material.restitution_factor() * info.material.restitution_factor(),
                        material.friction_factor() * info.material.friction_factor(), info.normal);
        return j;
    };
    // Add the contact to the interaction info.
    auto add_interaction = [car, this](const Three_Vector& velocity, const Material& material,
                                       const Contact_Info& info) {
        auto v_perp = velocity.project(info.normal);
        auto v_par = velocity - v_perp;
        m_interaction_info.push_back({car, material.type(), info.material.type(),
                                      v_par.magnitude(), v_perp.magnitude()});
    };

    // Check for contact with the track.
    for (auto particle : car->chassis().particles())
    {
        const auto& pos = car->chassis().contact_position(particle);
        assert(pos.magnitude() >= 0.0);
        double bump_parameter = car->distance_traveled() + particle->position().x;
        auto info = m_track.test_for_contact(pos, bump_parameter, road_index, segment_index);
        const auto& velocity = car->chassis().velocity(particle);
        if (auto j = local_impulse(pos, velocity, particle->material(), info))
        {
            car->chassis().contact(particle, *j, velocity, info.depth, info.normal, info.material);
            add_interaction(velocity, particle->material(), info);
        }
    }
    // Check for contact with track objects.
    for (const auto& object : m_track.objects())
    {
        auto info = car->collision(object.position, Three_Vector(), true);
        auto velocity
            = car->chassis().velocity(car->chassis().transform_from_world(object.position));
        if (auto j = local_impulse(object.position, velocity, object.material, info))
        {
            car->chassis().temporary_contact(object.position, *j, velocity, info.depth, info.normal,
                                             info.material);
            add_interaction(velocity, object.material, info);
        }
    }
}

void World::collide(Car_Information* car1_info, Car_Information* car2_info)
{
    Car* car1 = car1_info->car;
    Car* car2 = car2_info->car;
    assert(car1 != car2);
    auto delta_r = car1->chassis().cm_position() - car2->chassis().cm_position();

    // Ignore cars that are too far away to make contact.
    if (delta_r.magnitude() > 1.5 * car2->length())
        return;

    const auto delta_v = car1->chassis().cm_velocity() - car2->chassis().cm_velocity();

    // Handle collisions between the contact points of car 1 and the crash box of car 2.
    for (const auto& p : car1->chassis().particles())
    {
        const auto& pos = car1->chassis().contact_position(p);
        auto info = car2->collision(pos, car1->chassis().velocity(p));
        if (info.contact)
        {
            const auto& velocity = car1->chassis().velocity(p) - car2->chassis().velocity(p);
            auto j = impulse(
                car1->chassis().world_moment(pos), car1->chassis().mass(),
                car1->chassis().inertia(), car2->chassis().world_moment(pos),
                car2->chassis().mass(), car2->chassis().inertia(), delta_v,
                p->material().restitution_factor() * p->material().restitution_factor(),
                p->material().friction_factor() * p->material().friction_factor(),
                info.normal);

            car1->chassis().contact(p, j, delta_v, info.depth, info.normal, info.material);

            car2->chassis().temporary_contact(car1->chassis().contact_position(p), -j, -delta_v,
                                              info.depth, -info.normal, info.material);

            auto v_perp = velocity.project(info.normal);
            auto v_par = velocity - v_perp;
            m_interaction_info.push_back(Interaction_Info{car1, info.material.type(),
                                                          info.material.type(), v_par.magnitude(),
                                                          v_perp.magnitude()});
        }
    }
}

void World::reset()
{
    if (!m_controlled_car_index)
        return;

    size_t& segment_index = controlled_car()->segment_index;
    size_t& road_index = controlled_car()->road_index;
    auto car = controlled_car()->car;
    car->reset();
    place_car(car, m_track.reset_position(car->chassis().position(), road_index, segment_index),
              m_track.get_road(road_index));
}

void World::restart()
{
    mp_timing->reset();
    if (m_controlled_car_index)
        controlled_car()->reset();
}

void World::gravity(double g)
{
    // Always downward, regardless  of sign.
    m_gravity = std::abs(g);
    for (auto info : m_cars)
        if (info.car)
            info.car->chassis().gravity(Three_Vector::Z * -m_gravity);
}

void World::place_car(Car* car, const Three_Vector& track_pos, const Road& road)
{
    const auto& segment = *road.segment_at(track_pos.x);

    car->chassis().reset(0.0);

    // Orient the car to be level with the track.
    {
        Three_Matrix orientation;
        double along = track_pos.x - segment.start_distance();
        double angle = segment.angle(along);
        orientation.rotate(Three_Vector(0.0, 0.0, angle));
        orientation.rotate(Three_Vector(-segment.banking().angle(along), 0.0, 0.0));
        Two_Vector up = road.elevation().normal(track_pos.x);
        orientation.rotate(Three_Vector(0.0, atan2(up.x, up.y), 0.0));
        car->chassis().set_orientation(orientation);
    }

    // Raise the car to the requested height above the track.
    double gap = std::numeric_limits<double>::max();
    for (auto p : car->chassis().particles())
    {
        auto pos = car->chassis().transform_to_world(p->contact_position());
        gap = std::min(gap, pos.z - segment.world_elevation(pos));
    }
    // Move the car to its initial x-y position.
    car->set_front_position(road.position(track_pos.x, track_pos.y));
    car->chassis().translate(Three_Vector(0.0, 0.0, track_pos.z - gap));
}

void World::add_car(Car& car, Driver& driver)
{
    car.chassis().gravity(Three_Vector(0.0, 0.0, -m_gravity));
    m_cars.push_back(Car_Information(&car, &driver));
    driver.set_cars(&m_cars);
    place_car(&car, car.chassis().position(), m_track.get_road(0));
    if (driver.is_interactive())
        set_controlled_car(m_cars.size() - 1);
}

void World::set_focused_car(size_t index)
{
    assert(index < m_cars.size());
    m_focused_car_index = index;
}

void World::focus_other_car(int delta)
{
    set_focused_car((m_focused_car_index + delta) % m_cars.size());
}

Car_Information* World::focused_car()
{
    return m_focused_car_index >= m_cars.size() ? nullptr : &m_cars[m_focused_car_index];
}

void World::set_controlled_car(size_t index)
{
    assert(index < m_cars.size());
    m_controlled_car_index = index;
}

Car_Information* World::controlled_car()
{
    return !m_controlled_car_index || *m_controlled_car_index >= m_cars.size()
        ? nullptr : &m_cars[*m_controlled_car_index];
}

void World::write_results(const std::string& file) const
{
    const auto* p_fastest = mp_timing->fastest_lap_timing();
    std::ofstream os(file.c_str());
    os << m_track.track_file() << std::endl
       << (p_fastest ? p_fastest->laps_complete() : 0) << std::endl
       << mp_timing->total_laps() << std::endl
       << (p_fastest ? p_fastest->best_lap_time() : 0) << std::endl;

    for (const auto& car : mp_timing->running_order())
    {
        const auto& info = m_cars[car->grid_position() - 1];
        os << info.car->car_file() << '\t' << info.car->name() << '\t'
           << (info.driver->is_interactive() ? "interactive" : "robot") << '\t'
           << car->laps_complete() << '\t' << car->best_lap_time() << std::endl;
    }
}