Vamos Automotive Simulator git

//  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 "Suspension.hpp"
#include "Wheel.hpp"
#include "../geometry/Constants.hpp"
#include "../geometry/Numeric.hpp"
#include "../media/Ac3d.hpp"

#include <cassert>

using namespace Vamos_Body;
using namespace Vamos_Geometry;

// Note that all angles are stored as right-hand rotations.  As a result, m_camber for a
// wheel on the right side of the car follows the common convention that positive camber
// means that the wheel leans away from the centerline.  For the wheel on the left,
// m_camber is contrary to convention.

//----------------------------------------------------------------------------------------
Suspension::Suspension(const Three_Vector& position,
                       Direction side_of_car, bool is_steered,
                       double camber, double caster, double toe,
                       double spring_constant, double bounce, double rebound, double travel,
                       double max_compression_velocity, std::shared_ptr<Wheel> wheel)
    : Particle(0.0, position),
      m_side(side_of_car),
      m_is_steered(is_steered),
      m_spring_constant(spring_constant),
      m_bounce(bounce),
      m_rebound(rebound),
      m_travel(travel),
      m_max_compression_velocity(max_compression_velocity),
      mp_wheel(wheel)
{
    auto symmetric_angle = [side_of_car](double degrees) {
        return deg_to_rad(side_of_car == LEFT ? -1.0 : 1.0);
    };
    mp_wheel->rotate(Three_Vector::X * symmetric_angle(camber));
    mp_wheel->rotate(Three_Vector::Y * deg_to_rad(caster)); // Same sign on both sides.
    mp_wheel->rotate(Three_Vector::Z * symmetric_angle(toe));
    m_static_position = mp_wheel->position();
    m_static_orientation = mp_wheel->orientation();
}

void Suspension::propagate(double time)
{
    // Set the wheel's position and orientation.
    mp_wheel->set_position(m_static_position);
    mp_wheel->set_orientation(m_static_orientation);
    // Steering is about the wheel's z-axis.
    mp_wheel->rotate(Three_Vector::Z * m_steer_angle);

    mp_wheel->propagate(time);

    if (m_displacement <= 0.0)
        return reset();

    // Get the suspension force to apply to the chassis.
    double d = mp_wheel->displacement();
    double spring_force = m_spring_constant * d;
    double v = mp_wheel->velocity().z;
    double damp = v > 0.0 ? m_bounce : m_rebound;
    double damp_force = damp * v;
    double anti_roll_force = m_anti_roll_suspension ?
        m_anti_roll_k * (d - m_anti_roll_suspension->mp_wheel->displacement()) : 0.0;
    //!! Apply the wheel's force in z.
    //!! Apply the wheel's force and impulse in x-y.
    m_force = rotate_out((spring_force + damp_force + anti_roll_force) * Three_Vector::Z
                         + mp_wheel->force());
    //!! Torque should be applied at the hub.
    m_torque = rotate_out(mp_wheel->torque());

    //!!Orient the hinge, set the force to the component of tire force in the x-y plane.
}

void Suspension::anti_roll(Suspension* other, double spring_constant)
{
    m_anti_roll_suspension = other;
    m_anti_roll_k = spring_constant;
    m_anti_roll_suspension->m_anti_roll_suspension = this;
    m_anti_roll_suspension->m_anti_roll_k = m_anti_roll_k;
}

void Suspension::steer(double degree_angle)
{
   if (m_is_steered)
       m_steer_angle = deg_to_rad(degree_angle);
}

void Suspension::reset()
{
    Particle::reset();
    mp_wheel->set_position(m_static_position);
    mp_wheel->set_orientation(m_static_orientation);
}

void Suspension::set_model(std::string file_name, double scale, const Three_Vector& translation,
                           const Three_Vector& rotation)
{
    auto position = translation;
    auto orientation = rotation;
    if (m_side == LEFT)
    {
        // Make the right and left sides symmetric.
        position.y *= -1.0;
        orientation.x *= -1.0;
        orientation.y *= -1.0;
    }
    auto model = Vamos_Media::Ac3d(file_name, scale, Three_Vector(), orientation);
    m_models.emplace_back(Suspension_Model{model.build(), position});
}

void Suspension::draw()
{
    for (const auto& model : m_models)
    {
        glPushMatrix();
        double d = mp_wheel->displacement();
        glTranslatef(position().x + model.position.x, position().y + model.position.y,
                     position().z + model.position.z - d);
        double angle = rad_to_deg(std::atan2(-d, model.position.y));
        glRotatef(angle, 1.0, 0.0, 0.0);
        glCallList(model.display_list);
        glPopMatrix();
    }
}