src/vamos/body/Rigid_Body.cc

Go to the documentation of this file.

//  Rigid_Body.cc - a rigid body.
//
//  Copyright (C) 2001--2004 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  This program 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 2 of the License, or
//  (at your option) any later version.
//
//  This program 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 this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include <vamos/body/Rigid_Body.h>
#include <vamos/body/Aerodynamic_Device.h>
#include <vamos/body/Contact_Point.h>

#include <cassert>

using Vamos_Geometry::Three_Vector;
using Vamos_Geometry::Three_Matrix;

//* Struct Contact_Parameters

//** Constructor

Vamos_Body::
Contact_Parameters::Contact_Parameters () 
  : m_distance (0.0)
{
}

//* Class Body

//** Constructors

// Specify the position and orientation of the body.
Vamos_Body::
Rigid_Body::Rigid_Body (const Three_Vector& pos, const Three_Matrix& orient) 
  : Frame (pos, orient),
    m_initial_position (pos),
    m_delta_time (0.0),
    m_mass (0.0)
{
        valid = false;
}

// Specify the position, the orientation is the same as the parent.
Vamos_Body::
Rigid_Body::Rigid_Body (const Three_Vector& pos) 
  : Frame (pos),
    m_initial_position (pos),
    m_delta_time (0.0),
    m_mass (0.0)
{
        valid = false;
}

// Specify the position, the orientation is the same as the parent.
Vamos_Body::
Rigid_Body::Rigid_Body () 
  : Frame (),
    m_delta_time (0.0),
    m_mass (0.0)
{
        valid = false;
}


//** Destuctor
Vamos_Body::Rigid_Body::
~Rigid_Body ()
{
  // The body is responsible for deleting the partiles even though
  // they were constructed elsewhere
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
        //delete *it;
    }
}

// Return the position of the center of mass of the body with respect
// to the world.
Three_Vector Vamos_Body::
Rigid_Body::cm_position ()
{
  return transform_out (m_body_cm, m_body_cm);
}

// Return the contact position of the INDEXth particle of
// `m_particles' with respect to the world.
Three_Vector Vamos_Body::
Rigid_Body::contact_position (Particle* contact_point)
{
  return transform_out (contact_point->contact_position (), m_body_cm);
}

Three_Vector Vamos_Body::
Rigid_Body::last_contact_position (Particle* contact_point)
{
        Three_Vector temp_position;
        Three_Matrix temp_orientation;
        
        temp_position = m_position;
        temp_orientation = m_orientation;
        
        m_position = m_last_position;
        m_orientation = m_last_orientation;
        
  Three_Vector out;
        
        out = transform_out (contact_point->contact_position (), m_last_body_cm);
        
        m_position = temp_position;
        m_orientation = temp_orientation;
        
        return out;
}

//set current state to last state
void Vamos_Body::
Rigid_Body::roll_back()
{
        m_position = m_last_position;
        m_orientation = m_last_orientation;
        m_body_cm = m_last_body_cm;
        m_cm_velocity = m_last_cm_velocity;
        m_ang_velocity = m_last_ang_velocity;
}

void Vamos_Body::
Rigid_Body::kill_vel()
{
        m_velocity = m_velocity * 0; 
        m_ang_velocity = m_ang_velocity * 0;
        
        std::vector <Particle*>::const_iterator it = m_particles.begin ();

        for (it++; it != m_particles.end (); it++)
    {
                (*it)->set_ang_velocity(m_ang_velocity);
                (*it)->set_velocity(m_velocity);
        }
}

// Return the smallest contact position z-value of the particles.
double Vamos_Body::
Rigid_Body::lowest_contact_position () const
{
  std::vector <Particle*>::const_iterator it = m_particles.begin ();
  double pos = transform_out ((*it)->contact_position ()) [2];
  double lowest = pos;

  for (it++; it != m_particles.end (); it++)
    {
      pos = transform_out ((*it)->contact_position ()) [2];
      if (pos < lowest)
        {
          lowest = pos;
        }
    }

  return lowest;
}

// Calculate the center of mass, the ineritia tensor, and its inverse.
void Vamos_Body::
Rigid_Body::update_center_of_mass ()
{
        if (m_contact_parameters.m_distance > 0.0)
    {
        }
        else
                m_last_body_cm = m_body_cm;
        
  // Find the center of mass in the body frame.
  m_body_cm = Three_Vector (0.0, 0.0, 0.0);
  m_mass = 0.0;
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      m_mass += (*it)->mass ();
      // The particle reports its position in the body frame.
      m_body_cm += (*it)->mass_position () * (*it)->mass ();
    }
  m_body_cm /= m_mass;

  // Initialize the inertia tensor.
  m_inertia.zero ();
  // Inertia tensor for rotations about the center of mass.
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {  
      m_inertia.add ((*it)->mass (),
                     (*it)->mass_position () - m_body_cm);
    }
  m_inertia.update ();
}

void Vamos_Body::
Rigid_Body::find_forces ()
{
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->find_forces ();
    }
}

void Vamos_Body::
Rigid_Body::propagate_contact()
{
        // Process single-collision contact.
        if (m_contact_parameters.m_distance > 0.0)
    {
                Particle* point = m_contact_parameters.mp_contact_point;
                Three_Vector position = rotate_out (point->contact_position () 
                                                                                  - m_body_cm);
                Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
                cross (rotate_out (point->position () - m_body_cm));
                Three_Vector ang_vel = 
                m_ang_velocity.project (m_contact_parameters.m_normal);
                point->contact (position,
                                          m_inertia, 
                                          rotate_in (velocity),
                                          m_contact_parameters.m_distance, 
                                          rotate_in (m_contact_parameters.m_normal), 
                                          rotate_in (ang_vel), 
                                          m_contact_parameters.m_material);
                
                translate ((m_contact_parameters.m_distance + 0.00) 
                                 * m_contact_parameters.m_normal);
                
                m_contact_parameters.m_distance = 0.0;
    }
}

void Vamos_Body::
Rigid_Body::single_point_contact(Vamos_Geometry::Three_Vector worldposition,
                double distance,
                Vamos_Geometry::Three_Vector normal,
                Vamos_Geometry::Material_Handle material, double time)
{
        if (distance > 0.0)
    {
                Three_Vector contactposition = transform_in(worldposition);
                Three_Vector position = rotate_out (contactposition - m_body_cm);
                Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
                        cross (position);
                Three_Vector ang_vel = 
                        m_ang_velocity.project (normal);
                                
                Vamos_Geometry::Three_Vector v_perp = rotate_in (velocity.project (normal));

                
                
                // Find the effective mass.
                double meff = m_inertia.inertia (position, normal);
                Three_Vector impulse = -(1.0 + (material->restitution_factor ())) * meff * v_perp;
                
                Vamos_Geometry::Three_Vector v_par = rotate_in (velocity) - v_perp;
                impulse -= v_par.unit () * material->friction_factor () * impulse.abs ();
                
                /*double impmax = 1;
                for (int i = 0; i < 3; i++)
                {
                        if (impulse[i] > impmax)
                                impulse[i] = impmax;
                        if (impulse[i] < -impmax)
                                impulse[i] = -impmax;
                }*/
                
                /*double impscale = 0.1;
                for (int i = 0; i < 3; i++)
                        impulse[i] *= impscale;*/
                
                /*double maxvimpulse = 0;
                if (impulse[1] > maxvimpulse)
                        impulse[1] = maxvimpulse;
                if (impulse[1] < -maxvimpulse)
                        impulse[1] = -maxvimpulse;*/
                
                //cout << impulse[0] << "," << impulse[1] << "," << impulse[2] << endl;
                
                
                
                
                translate ((distance + 0.00) 
                                 * normal);



                // Find the force that the particle exerts on the rest of the system.
                // The particle reports its force in the Body frame.
                Three_Vector total_force;
                Three_Vector total_torque;
                
                Three_Vector body_force = 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 - (*it)->torque_position ();
                //Three_Vector torque = (*it)->torque ();
                //double I = (m_inertia * torque.unit ()).abs ();
                //torque *= I / (I + m_mass * torque_dist.dot (torque_dist));
                Three_Vector force_dist = m_body_cm - contactposition;
                Three_Vector zero(0,0,0);
                total_torque += zero - 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_out (total_force) + m_gravity * m_mass;
                
                Three_Vector delta_omega = time * total_torque * m_inertia.inverse ();
                Three_Vector delta_theta = (m_ang_velocity + delta_omega) * time;
                //m_last_ang_velocity = m_ang_velocity;
                m_ang_velocity += delta_omega;
                
                Three_Vector delta_v = time * total_force / m_mass;
                Three_Vector 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 = m_position;
                translate (m_body_cm);
                
                // rotate() acts in the body frame.
                //m_last_orientation = m_orientation;
                rotate (delta_theta);
                translate (delta_r - m_body_cm);
                
                // Determine the velocity of the origin.
                //m_last_velocity = m_velocity;
                m_velocity = (m_position - m_last_position) / time;
    }
}

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

  //VENZON:  the stuff in propagate_contact used to be here
        /*if (m_contact_parameters.m_distance > 0.0)
    {
                contact = true;
        }*/
        //propagate_contact();

  // Propagate the particles
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->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;
  Three_Vector total_force;
  Three_Vector total_torque;

  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      // 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 = (*it)->force () + (*it)->impulse () / time;
                //Three_Vector body_force = (*it)->force ();
      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 - (*it)->torque_position ();
      Three_Vector torque = (*it)->torque ();
      double I = (m_inertia * torque.unit ()).abs ();
      torque *= I / (I + m_mass * torque_dist.dot (torque_dist));
      Three_Vector force_dist = m_body_cm - (*it)->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_out (total_force) + m_gravity * m_mass;

  Three_Vector delta_omega = time * total_torque * m_inertia.inverse ();
  Three_Vector delta_theta = (m_ang_velocity + delta_omega) * time;
  if (!contact) 
          m_last_ang_velocity = m_ang_velocity;
  m_ang_velocity += delta_omega;

  Three_Vector delta_v = time * total_force / m_mass;
  Three_Vector delta_r = (m_cm_velocity + delta_v) * time;
  if (!contact) 
          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.
  if (!contact)
          m_last_position = m_position;
  translate (m_body_cm);

  // rotate() acts in the body frame.
  if (!contact)
          m_last_orientation = m_orientation;
  rotate (delta_theta);
  translate (delta_r - m_body_cm);

  // Determine the velocity of the origin.
  if (!contact) 
          m_last_velocity = m_velocity;
  m_velocity = (m_position - m_last_position) / time;
}

// Undo the last propagation.
void Vamos_Body::
Rigid_Body::rewind ()
{
  m_position = m_last_position;
  m_velocity = m_last_velocity;
  m_cm_velocity = m_last_cm_velocity;

  m_orientation = m_last_orientation;
  m_ang_velocity = m_last_ang_velocity;
}

// Finish the timestep.
void Vamos_Body::
Rigid_Body::end_timestep ()
{
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->end_timestep ();
    }
}

// Return the velocity of the particle in the parent frame.
Three_Vector Vamos_Body::
Rigid_Body::velocity (Particle* particle)
{
  Three_Vector r_vec = particle->position ();
  return m_cm_velocity + rotate_out (m_ang_velocity).
    cross (rotate_out (r_vec - m_body_cm));
}

// Handle a collision.
void Vamos_Body::
Rigid_Body::contact (Particle* contact_point, 
               double distance,
               const Three_Vector& normal,
               const Vamos_Geometry::Material_Handle material)
{
  if (contact_point->single_contact ())
    {
      if (distance > m_contact_parameters.m_distance)
        {
          m_contact_parameters.mp_contact_point = contact_point;
          m_contact_parameters.m_distance = distance;
          m_contact_parameters.m_normal = normal;
          m_contact_parameters.m_material = material;
        }
    }
  else
    {
      Three_Vector position = 
        rotate_out (contact_point->contact_position () - m_body_cm);
      Three_Vector ang_vel = m_ang_velocity.project (normal);
      Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
        cross (rotate_out (contact_point->position () - m_body_cm));
      contact_point->contact (position,
                              m_inertia,
                              rotate_in (velocity),
                              distance,
                              rotate_in (normal),
                              rotate_in (ang_vel),
                              material);
    }
}

// Transform the wind into the body frame and send it to the INDEXth
// particle which must be an Aerodynamic_Device.
void Vamos_Body::
Rigid_Body::wind (Particle* aero_device, 
                  const Three_Vector& wind_vector, 
                  double density)
{
  aero_device->wind (rotate_in (wind_vector), density);
}

// Return the body to its initial state at its initial position.
void Vamos_Body::
Rigid_Body::reset ()
{
  m_position = m_initial_position;
  m_orientation.identity ();

  private_reset ();
}

// Return the body to its initial state at a particular position and
// orientation.
void Vamos_Body::
Rigid_Body::reset (const Three_Vector& position, 
                   const Three_Matrix& orientation)
{
  m_position = position;
  m_orientation = orientation;

  private_reset ();
}

// Common code for the two reset () methods.
void Vamos_Body::
Rigid_Body::private_reset ()
{
  m_velocity.zero ();
  m_cm_velocity.zero ();
  m_ang_velocity.zero ();

  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->reset ();
    }
}

---