Logo Search packages:      
Sourcecode: vdrift version File versions

Rigid_Body.cc

//  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 ();
    }
}

Generated by  Doxygen 1.6.0   Back to index