src/vamos/body/Suspension.cc

Go to the documentation of this file.

//  Suspension.cc - the suspension component for a wheel.
//
//  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/geometry/Conversions.h>
#include <vamos/body/Suspension.h>

#include <cmath>
#include <cassert>

using namespace Vamos_Geometry;

//* Static Member

// The axis of rotation for steering and toe adjustments.
const Three_Vector Vamos_Body::Suspension::
STEER_AXIS = Three_Vector (0.0, 0.0, 1.0);

// Note that all angles are stored as right-hand rotations.  As a
// result, m_caster 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_caster is contrary to convention.


//* Class Hinge
Vamos_Body::
Hinge::Hinge (const Three_Vector& position) :
  Particle (0.0, position)
{
}

void Vamos_Body::
Hinge::input (const Three_Vector& torque, const Three_Vector& radius)
{
        double t_magnitude = sqrt(torque[0]*torque[0]+torque[1]*torque[1]+torque[2]*torque[2]);
        double r_magnitude = sqrt(radius[0]*radius[0]+radius[1]*radius[1]+radius[2]*radius[2]);
  m_force = t_magnitude 
    / r_magnitude * (torque.cross (radius).unit ());
}

//* Struct Suspension_Model
struct Vamos_Body::Suspension_Model
{
/*  GLuint display_list;
  double x;
  double y;
  double z;

  Suspension_Model (GLuint list_id, const Three_Vector& position)
        : display_list (list_id),
          x (position[0]),
          y (position[1]),
          z (position[2])
  {
  };*/
};

//* Class Suspension

//** Constructor
Vamos_Body::
Suspension::Suspension (const Three_Vector& position,
                                                const Three_Vector& center_of_translation,
                                                Side side_of_car, double spring_constant, 
                                                double bounce, double rebound, double travel, 
                                                double max_compression_velocity) : 
  Particle (0.0, position),
  mp_hinge (new Hinge (center_of_translation)),
  m_initial_z (position[2]),
  m_spring_constant (spring_constant),
  m_bounce (bounce),
  m_rebound (rebound),
  m_travel (travel),
  m_displacement (0.0),
  m_time_step (0.0),
  m_compression_velocity (0.0),
  m_max_compression_velocity (max_compression_velocity),
  m_bottomed_out (false),
  m_anti_roll_k (0.0),
  m_anti_roll_suspension (0),
  m_steer_angle (0.0),
  m_camber (0.0),
  m_caster (0.0),
  m_toe (0.0),
  m_side (side_of_car),
  m_normal (Three_Vector (0.0, 0.0, 1.0))
{
  m_static_orientation.identity ();

  // m_radius points from m_position to the hinge.
  m_radius = center_of_translation - m_position;

  // m_radius_magnitude is the length of m_radius.  It will not
  // change.
  m_radius_magnitude = m_radius.magnitude ();
}


Vamos_Body::
Suspension::~Suspension ()
{
  /*for (std::vector <Suspension_Model*>::iterator it = m_models.begin ();
           it != m_models.end ();
           it++)
        {
          delete *it;
        }*/
}

// Specify the suspension component that is attached to this one with
// an anti-roll bar.  The anti-roll bar will have a spring constant of
// SPRING_CONSTANT.
void Vamos_Body::
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;
}

// Displace this suspension component by DISTANCE.  A positive
// DISTANCE means compression.
void Vamos_Body::Suspension::
displace (double distance)
{
  double last_displacement = m_displacement;
  m_displacement = distance;
  if (m_displacement > m_travel)
        {
          m_bottomed_out = true;
                //VENZON: suspension bumper
          //m_displacement = m_travel;
        }
  else
        {
          m_bottomed_out = false;
        }

  // Update m_angle.  m_angle is a right-hand rotation about the
  // y-axis from the x-axis to m_radius.
  const Three_Vector& hinge_pos = mp_hinge->position ();
  double z = hinge_pos[2] - m_initial_z - m_displacement;
  assert (z <= m_radius_magnitude);
  m_angle = asin (z / m_radius_magnitude);
  // Get the right quadrant.
  if (hinge_pos[0] > m_position[0])
        {
          m_angle = pi - m_angle;
        }

  // Update m_position preserving the magnitude of m_radius.  We don't
  // change the y position yet.
  Three_Vector new_position = hinge_pos 
        + m_radius_magnitude * Three_Vector (cos (m_angle), 0.0, -sin (m_angle));
  m_position[0] = new_position[0];
  m_position[2] = new_position[2];

  // m_radius points from m_position to the hinge.
  m_radius = hinge_pos - m_position;

  // m_tangent is a unit vector that is perpendicular to m_radius such
  // that m_tangent x m_radius points in the y direction.
  m_tangent = Three_Vector (-m_radius[2], 0.0, m_radius[0]).unit ();

  m_compression_velocity = (m_displacement - last_displacement) / m_time_step;
}

void Vamos_Body::
Suspension::input (const Three_Vector& wheel_force,
           const Three_Vector& normal)
{
  m_wheel_force = wheel_force;
  m_normal = rotate_out (normal);
}

void Vamos_Body::
Suspension::torque (double wheel_torque)
{
  mp_hinge->input (Three_Vector (0.0, -wheel_torque, 0.0), m_radius);
}

// Calculate the force exerted by the suspension in its current state.
void Vamos_Body::
Suspension::find_forces ()
{
  double anti_roll_force = 0.0;
  if (m_anti_roll_suspension)
        {
          anti_roll_force = m_anti_roll_k *
                (m_displacement - m_anti_roll_suspension->m_displacement);
        }

  // Use `m_bounce' for compression, `m_rebound' for decompression.
  double damp = m_bounce;
  if (m_compression_velocity < 0.0)
        {
          damp = m_rebound;
        }

  if (m_displacement <= 0.0)
        {
          // Don't exert a force if this suspension is not compressed.
          m_force.zero ();
        }
  else
        {
          // If the suspension is moving at a speed > m_max_compression_velocity,
          // the damper 'locks up' due to turbulence in the fluid.  The effect
          // is the same as bottoming out.
          if (std::abs (m_compression_velocity) > m_max_compression_velocity)
                {
                  m_bottomed_out = true;
                }

          double spring_force = m_spring_constant * m_displacement;
          double damp_force = damp * m_compression_velocity;
          m_force =
                rotate_in (m_normal * (spring_force + damp_force + anti_roll_force));
        }
        
        //VENZON:  suspension bumper (special case for bottomed out springs)
        if (m_bottomed_out)
        {
                double spring_force = 1000000.0 * (m_displacement-m_travel);
                double damp_force = damp * m_compression_velocity;
                m_force =
                rotate_in (m_normal * (spring_force + damp_force + anti_roll_force));
        }
}

// Advance this suspension component forward in time by TIME.
void Vamos_Body::
Suspension::propagate (double time)
{
  m_time_step = time;

  // Start with the static orientation.
  orient (m_static_orientation);
  rotate (m_steer_angle * STEER_AXIS);
}

// Undo the last propagation.
void Vamos_Body::
Suspension::rewind ()
{
}

// Set the steering angle.
void Vamos_Body::
Suspension::steer (double degree_angle)
{
  m_steer_angle = deg_to_rad (degree_angle);
}

// Set the camber angle.
void Vamos_Body::
Suspension::camber (double degree_angle)
{
  if (m_side == LEFT)
        degree_angle *= -1.0;

  // Undo the current camber setting before applying the new one.
  m_static_orientation.rotate (Three_Vector (-m_camber, 0.0, 0.0));
  m_camber = deg_to_rad (degree_angle);
  m_static_orientation.rotate (Three_Vector (m_camber, 0.0, 0.0));
}

// Set the caster angle.
void Vamos_Body::
Suspension::caster (double degree_angle)
{
  // The caster rotation is in the same direction for both sides.

  // Undo the current caster setting before applying the new one.
  m_static_orientation.rotate (Three_Vector (0.0, -m_caster, 0.0));
  m_caster = -deg_to_rad (degree_angle);
  m_static_orientation.rotate (Three_Vector (0.0, m_caster, 0.0));
}

// Set the toe angle.
void Vamos_Body::
Suspension::toe (double degree_angle)
{
  if (m_side == LEFT)
        degree_angle *= -1.0;

  // Undo the current toe setting before applying the new one.
  m_static_orientation.rotate (-m_toe * STEER_AXIS);
  m_toe = deg_to_rad (degree_angle);
  m_static_orientation.rotate (m_toe * STEER_AXIS);
}

// Return the camber angle in radians for a suspension displacement of
// DISPLACEMENT.
double Vamos_Body::Suspension::
camber_function (double displacement) const
{
  return 0.0;
}

double Vamos_Body::
Suspension::current_camber (double normal_y) const
{
  return Vamos_Geometry::clip (normal_y, -0.5, 0.5);;
}

// Return this suspension component to equilibrium.
void Vamos_Body::
Suspension::reset ()
{
  m_force.zero ();
  m_displacement = 0.0;
}


void Vamos_Body::
Suspension::set_model (std::string file_name,
                                           double scale,
                                           const Three_Vector& translation, 
                                           const Three_Vector& rotation)
{
  Three_Vector position = translation;
  Three_Vector orientation = rotation;
  if (m_side == LEFT)
        {
          // Make the right and left sides symmetric.
          position[1] *= -1.0;
          orientation[0] *= -1.0;
          orientation[1] *= -1.0;
        }

  /*Vamos_Media::Ac3d* model = 
    new Vamos_Media::Ac3d (file_name, scale, Three_Vector (), orientation);
  m_models.push_back (new Suspension_Model (model->build (), position));
  delete model;*/
}

void Vamos_Body::
Suspension::draw ()
{
/*  for (std::vector <Suspension_Model*>::iterator it = m_models.begin ();
           it != m_models.end ();
           it++)
        {
          glPushMatrix ();

          glTranslatef (m_position[0] + (*it)->x, 
                                        m_position[1] + (*it)->y,
                                        m_position[2] + (*it)->z - m_displacement);

          double angle = rad_to_deg (std::atan2 (-m_displacement, (*it)->y));
          glRotatef (angle, 1.0, 0.0, 0.0);

          glCallList ((*it)->display_list);
          glPopMatrix ();
        }*/
}

---