Vamos Automotive Simulator git

//  A rigid body.
//
//  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 "Rigid_Body.hpp"
#include "../geometry/Constants.hpp"
#include "Aerodynamic_Device.hpp"
#include "Contact_Point.hpp"

#include <cassert>
#include <numeric>

using namespace Vamos_Geometry;
using namespace Vamos_Body;

Rigid_Body::Rigid_Body(const Three_Vector &pos, const Three_Matrix &orient)
    : Frame(pos, orient),
      m_initial_position(pos),
      m_initial_orientation(orient)
{
}

void Rigid_Body::set_initial_conditions(const Vamos_Geometry::Three_Vector &position,
                                        const Vamos_Geometry::Three_Vector &orientation,
                                        const Vamos_Geometry::Three_Vector &velocity,
                                        const Vamos_Geometry::Three_Vector &angular_velocity)
{
    m_initial_position = position;
    m_initial_velocity = velocity;
    m_initial_orientation.identity();
    m_initial_orientation = Three_Matrix().rotate(orientation * deg_to_rad(1.0));
    m_initial_angular_velocity = angular_velocity * deg_to_rad(1.0);
    reset(0.0);
}

void Rigid_Body::add_drag_particle(P_Drag d)
{
    m_drag_particles.push_back(d);
    add_particle(d);
}

Three_Vector Rigid_Body::cm_position() const
{
    return transform_to_parent(m_body_cm);
}

Three_Vector Rigid_Body::contact_position(P_Particle contact_point)
{
    return transform_to_parent(contact_point->contact_position());
}
Three_Vector Rigid_Body::position(P_Particle p)
{
    return transform_to_parent(p->position());
}

double Rigid_Body::lowest_contact_position() const
{
    auto get_z = [this](auto particle) {
        return transform_to_parent(particle->contact_position()).z;
    };
    auto lowest = std::min_element(m_particles.begin(), m_particles.end(),
                                   [get_z](auto a, auto b) { return get_z(a) < get_z(b); });
    return get_z(*lowest);
}

void Rigid_Body::update_center_of_mass()
{
    // Find the center of mass in the body frame.
    m_body_cm.zero();
    m_mass = 0.0;
    for (const auto& p : m_particles)
    {
        m_mass += p->mass();
        // The particle reports its position in the body frame.
        m_body_cm += p->mass_position() * p->mass();
    }
    m_body_cm /= m_mass;

    // Initialize the inertia tensor for rotations about the center of mass.
    m_inertia.zero();
    for (const auto& p : m_particles)
        m_inertia.add(p->mass(), p->mass_position() - m_body_cm);
    m_inertia.update();
}

void Rigid_Body::find_forces()
{
    for (const auto& p : m_particles)
        p->find_forces();
}

// Advance the body in time by TIME.
void Rigid_Body::propagate(double time)
{
    // Re-calculate the inertia tensor and center of mass.
    update_center_of_mass();

    // Process single-collision contact.
    if (m_contact_parameters.m_distance > 0.0)
    {
        auto point = m_contact_parameters.mp_contact_point;
        assert(point); // There must be a point if there's a non-zero distance.
        auto world_v = velocity(point->position());
        auto world_ang_v = angular_velocity();
        m_contact_parameters.mp_contact_point->contact(
            rotate_from_parent(m_contact_parameters.m_impulse), rotate_from_parent(world_v),
            m_contact_parameters.m_distance, rotate_from_parent(m_contact_parameters.m_normal),
            rotate_from_parent(world_ang_v), m_contact_parameters.m_material);
        translate(m_contact_parameters.m_distance * m_contact_parameters.m_normal);
        m_contact_parameters.m_distance = 0.0;
    }

    // Propagate the particles
    for (const auto& p : m_particles)
        p->propagate(time);
    for (const auto& c : m_temporary_contact_points)
        c->propagate(time);

    // Move the body and the particles in response to forces applied to them and their
    // momenta, while keeping their relative positions fixed.
    m_delta_time = time;
    auto total_force = m_cm_force;
    Three_Vector total_torque;

    for (const auto& p : m_particles)
    {
        // Find the force that the particle exerts on the rest of the system.  The
        // particle reports its force in the Body frame.
        Three_Vector body_force = p->force() + p->impulse() / time;
        total_force += body_force;

        // Find the force and torque that the particle exerts on the Body.  Find the
        // vector from the cm to the particle in the world frame.
        Three_Vector torque_dist = m_body_cm - p->torque_position();
        Three_Vector torque = p->torque();
        double I = (m_inertia * torque.unit()).magnitude();
        torque *= I / (I + m_mass * torque_dist.dot(torque_dist));
        Three_Vector force_dist = m_body_cm - p->force_position();
        total_torque += torque - force_dist.cross(body_force);
    }

    // Transform the forces to the parent's coordinates so we can find out how the Body
    // moves w.r.t its parent.
    total_force = rotate_to_parent(total_force) + m_gravity * m_mass;

    auto delta_omega = time * total_torque * m_inertia.inverse();
    auto delta_theta = (angular_velocity() + delta_omega) * time;
    m_last_angular_velocity = angular_velocity();
    angular_accelerate(delta_omega);

    m_acceleration = total_force / m_mass;
    auto delta_v = m_acceleration * time;
    auto delta_r = (m_cm_velocity + delta_v) * time;
    m_last_cm_velocity = m_cm_velocity;
    m_cm_velocity += delta_v;

    // Because the body's origin is not necessarily coincident with the center of mass,
    // the body's translation has a component that depends on the orientation.  Place the
    // Body by translating to the cm, rotating and then translating back.
    m_last_position = position();
    translate(orientation() * m_body_cm);

    // rotate() acts in the body frame.
    m_last_orientation = orientation();
    rotate(delta_theta);
    translate(orientation() * -m_body_cm + delta_r);

    // Determine the velocity of the origin.
    m_last_velocity = Frame::velocity();
    set_velocity((position() - m_last_position) / time);
}

void Rigid_Body::rewind()
{
    set_position(m_last_position);
    set_velocity(m_last_velocity);
    m_cm_velocity = m_last_cm_velocity;

    set_orientation(m_last_orientation);
    set_angular_velocity(m_last_angular_velocity);
}

void Rigid_Body::end_timestep()
{
    for (const auto& p : m_particles)
        p->end_timestep();
    m_temporary_contact_points.clear();
    m_cm_force.zero();
}

Three_Vector Rigid_Body::velocity(P_Particle particle)
{
    return velocity(particle->position());
}

Three_Vector Rigid_Body::velocity(const Three_Vector& r)
{
    return m_cm_velocity + rotate_to_parent(angular_velocity().cross(moment(r)));
}

Three_Vector Rigid_Body::acceleration() const
{
    return rotate_from_world(m_acceleration);
}

Three_Vector Rigid_Body::moment(const Three_Vector& position)
{
    return position - m_body_cm;
}

Three_Vector Rigid_Body::world_moment(const Three_Vector& world_position)
{
    return rotate_to_world(moment(transform_from_world(world_position)));
}

void Rigid_Body::contact(P_Particle contact_point, const Three_Vector& impulse,
                         const Three_Vector& velocity, double depth, const Three_Vector& normal,
                         const Material& material)
{
    if (contact_point->single_contact())
    {
        if (depth > m_contact_parameters.m_distance)
            m_contact_parameters
                = Contact_Parameters{contact_point, impulse, depth, normal, material};
    }
    else
        contact_point->contact(rotate_from_parent(impulse),
                               rotate_from_parent(velocity),
                               depth,
                               rotate_from_parent(normal),
                               rotate_from_parent(angular_velocity().project(normal)),
                               material);
}

void Rigid_Body::temporary_contact(const Three_Vector& position, const Three_Vector& impulse,
                                   const Three_Vector& velocity, double depth,
                                   const Three_Vector& normal, const Material& material)
{
    Contact_Point *point
        = new Contact_Point(0.0, transform_from_parent(position), material.type(), 0.0, 1.0, this);

    // The material, restitution and friction are not used for temporaries.
    // The impulse is calculated externally and passed in.
    point->contact(rotate_from_parent(impulse), rotate_from_parent(velocity), depth,
                   rotate_from_parent(normal),
                   rotate_from_parent(angular_velocity().project(normal)), material);

    m_temporary_contact_points.emplace_back(point);
}

void Rigid_Body::wind(const Three_Vector& wind_vector, double density)
{
    for (const auto& d : m_drag_particles)
        d->wind(rotate_from_parent(wind_vector - velocity(d)), density);
}

double Rigid_Body::aerodynamic_drag() const
{
    return std::accumulate(m_drag_particles.begin(), m_drag_particles.end(), 0,
                           [](auto a, auto b) { return a + b->drag_factor(); });
}

double Rigid_Body::aerodynamic_lift() const
{
    return std::accumulate(m_drag_particles.begin(), m_drag_particles.end(), 0,
                           [](auto a, auto b) { return a + b->lift_factor(); });
}

void Rigid_Body::reset(double direction)
{
    set_position(m_initial_position);
    Three_Matrix orient(m_initial_orientation);
    set_orientation(orient.rotate(Three_Vector::Z * direction));
    private_reset();
}

void Rigid_Body::reset(const Three_Vector &position, const Three_Matrix &orientation)
{
    set_position(position);
    set_orientation(orientation);
    private_reset();
}

void Rigid_Body::private_reset()
{
    m_cm_velocity = m_initial_velocity;
    set_velocity(m_cm_velocity + m_initial_velocity.cross(moment(Three_Vector::ZERO)));
    set_angular_velocity(m_initial_angular_velocity);
    for (auto& p : m_particles)
        p->reset();
}