Logo Search packages:      
Sourcecode: vdrift version File versions

Tire.cc

//  Tire.cc - the tire 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/body/Tire.h>
#include <vamos/geometry/Conversions.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>

using namespace Vamos_Geometry;

#define MINSLIP 1.0e-4
//#define MINSLIP 1.0e-2

//* Class Tire_Friction

//** Constructor
Vamos_Body::
Tire_Friction::Tire_Friction (const std::vector <double>& long_parameters,
                                            const std::vector <double>& trans_parameters,
                                            const std::vector <double>& align_parameters) :
  m_longitudital_parameters (long_parameters),
  m_transverse_parameters (trans_parameters),
  m_aligning_parameters (align_parameters),
  m_slide (0.0)
{
  assert (m_longitudital_parameters.size () == 11);
  assert (m_transverse_parameters.size () == 15);
  assert (m_aligning_parameters.size () == 18);
}

double Vamos_Body::Tire_Friction::Pacejka_Fx(double sigma, double Fz, double friction_factor)
{
      //double Fz_squared = Fz * Fz;
      
      // Evaluate the longitudinal parameters.
      const std::vector <double>& b = m_longitudital_parameters;
      /*double Cx = b [0];
      double Dx = friction_factor * (b [1] * Fz_squared + b [2] * Fz);
      double Bx = (b [3] * Fz_squared + b [4] * Fz) * exp (-b [5] * Fz) 
      / (Cx * Dx);
      double Ex = b [6] * Fz_squared + b [7] * Fz + b [8];
      double Shx = b [9] * Fz + b [10];*/
      
      double D = (b[1]*Fz + b[2])*Fz*friction_factor;
      double B = (b[3]*Fz+b[4])*exp(-b[5]*Fz)/(b[0]*(b[1]*Fz+b[2]));
      double E = (b[6]*Fz*Fz+b[7]*Fz+b[8]);
      double S = (100*sigma + b[9]*Fz+b[10]);
      double Fx = D*sin(b[0] * atan(S*B+E*(atan(S*B)-S*B)));
      return Fx;
}

double Vamos_Body::Tire_Friction::Pacejka_Fy(double alpha, double Fz, double gamma, double friction_factor)
{
      //cout << alpha << "," << Fz << "," << gamma << endl;
      
      const std::vector <double>& a = m_transverse_parameters;
      
      /*double Cy = a [0];
  double Dy = (a [1] * Fz_squared + a [2] * Fz);
  double By = a [3] * sin (2.0 * atan (Fz / a [4])) 
      * (1.0 - a [5] * std::abs (gamma)) / (Cy * Dy);
  double Ey = a [6] * Fz + a [7];
  double Shy = a [8] * gamma + a [9] * Fz + a [10];
  double Svy = (a [11] * Fz + a [12]) * gamma * Fz 
      + a [13] * Fz + a [14];*/
      
      double D = (a[1]*Fz+a[2])*Fz*friction_factor;
      double B = a[3]*sin(2.0*atan(Fz/a[4]))*(1.0-a[5]*std::abs(gamma))/(a[0]*(a[1]*Fz+a[2])*Fz);
      double E = a[6]*Fz+a[7];
      double S = alpha + a[8]*gamma+a[9]*Fz+a[10];
//    double Sv = ((a[11]*Fz+a[12])*gamma + a[13])*Fz+a[14];
      
      double Fy = D*sin(a[0]*atan(S*B+E*(atan(S*B)-S*B)));//+Sv;
      return Fy;
}

double Vamos_Body::Tire_Friction::Pacejka_Mz(double sigma, double alpha, double Fz, double gamma, double friction_factor)
{
      const std::vector <double>& c = m_aligning_parameters;
      
      double D = (c[1]*Fz+c[2])*Fz*friction_factor;
      double B = (c[3]*Fz*Fz+c[4]*Fz)*(1.0-c[6]*std::abs(gamma))*exp(-c[5]*Fz)/(c[0]*D);
      double E = (c[7]*Fz*Fz+c[8]*Fz+c[9])*(1.0-c[10]*std::abs(gamma));
      double S = alpha + c[11]*gamma+c[12]*Fz+c[13];
//    double Sv = (c[14]*Fz*Fz+c[15]*Fz)*gamma+c[16]*Fz + c[17];
      
      double Mz = D*sin(c[0]*atan(S*B+E*(atan(S*B)-S*B)));//+Sv;
      return Mz;
}

// Return the friction vector calculated from the magic formula.
// HUB_VELOCITY is the velocity vector of the wheel's reference
// frame.  PATCH_SPEED is the rearward speed of the contact pacth with
// respect to the wheel's frame.
Three_Vector Vamos_Body::
Tire_Friction::friction_forces (double normal_force, double friction_factor,
                                                const Three_Vector& hub_velocity, 
                                                double patch_speed, double current_camber,
                                                double sigma_hat, double alpha_hat)
{
      double Fz = normal_force / 1000.0;
      
      // Calculate the slip parameters sigma and tan_alpha.  We have to
      // play some games to keep them reasonable in extreme conditions.
      
      // Leave the slip parameters at zero if the difference between the
      // patch speed and the hub velocity is very small.
      double sigma = 0.0;
      double tan_alpha = 0.0;
      double alpha = 0.0;
      
      double V = hub_velocity[0];
      //if (std::abs(V) > MINSLIP)
      {
            double denom = std::max (std::abs (hub_velocity [0]), 1.0);
            
            //sigma = (patch_speed/V - 1.0);
            sigma = (patch_speed - V)/denom;
            //sigma = patch_speed/V;
            //sigma = -sigma;
            
            tan_alpha = hub_velocity [1] / denom;
            //alpha = rad_to_deg(atan2(hub_velocity[1],hub_velocity[0]));
            alpha = -rad_to_deg(atan2(hub_velocity[1],denom));
      }
      
      /*if (std::abs (hub_velocity [0] - patch_speed) > MINSLIP)
      {
        // Put a lower limit on the denominator to keep sigma and
        // tan_alpha from getting out of hand at low speeds.
            double denom = std::max (std::abs (hub_velocity [0]), 3.0);
            sigma = (patch_speed - hub_velocity [0]) / denom;
            tan_alpha = hub_velocity [1] / denom;
            alpha = atan(tan_alpha);
      }*/
      
      double gamma = rad_to_deg (current_camber);
      
      //double Fx = Pacejka_Fx(sigma, Fz);
      //double Fy = Pacejka_Fy(alpha, Fz, gamma);
      //double Mz = Pacejka_Mz(sigma, alpha, Fz, gamma);
      
      //combine the grip
      //double sigma_hat = 0.08;
      //double alpha_hat = 8.0;
      //double s = std::abs(sigma) / sigma_hat;
      //double a = std::abs(alpha) / alpha_hat;
      double s = sigma / sigma_hat;
      double a = alpha / alpha_hat;
      double rho = sqrt(s*s+a*a);
      
      double Fx = (s / rho)*Pacejka_Fx(rho*sigma_hat, Fz, friction_factor);
      double Fy = (a / rho)*Pacejka_Fy(rho*alpha_hat, Fz, gamma, friction_factor);
      double Mz = Pacejka_Mz(sigma, alpha, Fz, gamma, friction_factor);
      //Fx = (s / rho)*Pacejka_Fx(sigma, Fz);
      //Fy = (a / rho)*Pacejka_Fy(alpha, Fz, gamma);
      //cout << sigma << "," << sigma_hat << ": " << endl;
      //cout << s << "," << a << ": " << rho << endl;
      //Mz = (a / rho)*Pacejka_Mz(rho*sigma_hat, rho*alpha_hat, Fz, gamma);
      //cout << Fx << "," << Fy << "," << Mz << endl;
      
      //cout << V << "," << patch_speed << "," << sigma << ": " << Fx << endl;
      //cout << hub_velocity[0] << "," << hub_velocity[1] << "," << alpha << ": " << Fy << endl;
      //cout << "sigma=0: " << Pacejka_Fx(0, 2.5) << endl;
      //cout << "Fy,alpha=0: " << Pacejka_Fy(0, 2.5, 0) << endl;
      //cout << "alpha=0: " << Pacejka_Mz(0, 0, 2.5, 0) << endl;
      
      //double slip_x = sigma;
      double slip_x = -sigma / (1.0 + std::abs (sigma));
      //double slip_x = patch_speed/V;
      //double d_alpha = 
      //deg_to_rad (-Shy - Svy / (By * Cy * Dy * (1.0 + (180.0 / pi - 1.0) * Ey)));
      // Genta lists d_alpha as -Shy - Svy / (By * Cy * Dy).  This is what
      // you get from the magic formula for Fy(alpha) using the small
      // angle approximation.  However, the constants calculated above are
      // for alpha in degrees.  The small-angle formulas atan(x) =
      // 180*x/pi and sin(x) = pi*x/180 for x << 180/pi give the formula
      // d_alpha shown above.  Ey, in general, is not << 1.  After we get
      // d_alpha, we have to convert it to radians.
      double slip_y = tan_alpha / (1.0+std::abs (sigma-1.0));
      //double slip_y = hub_velocity[1];
      double total_slip = std::sqrt (slip_x * slip_x + slip_y * slip_y);
      /*if (total_slip > 1.0e-6)
      {
            Fx *= slip_x / total_slip;
            Fy *= slip_y / total_slip;
            Mz *= slip_y / total_slip;
      }*/
      
      //cout << slip_x << "," << slip_y << "," << total_slip << endl;
      
      // Set the volume of the tire squeal sound. 
      //m_slide = 0.0;
      const std::vector <double>& b = m_longitudital_parameters;
      double maxforce = b[2] * 7.0; //(b[1]*(normal_force / 1000.0)+b[2])*4.0;
      double combforce = sqrt(Fx*Fx + Fy*Fy);
      //double overforce = maxforce - combforce;
      if (combforce > maxforce && combforce > 1.0e-3)
      {
            //cout << "max: " << maxforce << ", " << combforce << " -- " << Fx << "," << Fy << " -- " << total_slip << endl;
            Fx *= maxforce / combforce;
            Fy *= maxforce / combforce;
      }
      /*if (!(std::abs(Fx) < maxforce))
      {
            
            Fx = maxforce;
      }
      if (!(std::abs(Fy) < maxforce))
      {
            Fy = maxforce;
      }*/
      
      if (hub_velocity.abs () < 0.1)
      {
        m_slide = 0.0;
      }
  else
      {
        m_slide = total_slip;
        if (m_slide > 1.0)
            m_slide = 1.0;
      }
      
      //cout << Fx << endl;
      
      // Construct the friction vector.  The z-component is the aligning
      // torque.
      return Three_Vector (Fx, Fy, Mz);
}

//double Vamos_Body::Tire_Friction::Pacejka_Fy(double slip_y, double Fz, double gamma);

// Return the friction vector calculated from the magic formula.
// HUB_VELOCITY is the velocity vector of the wheel's reference
// frame.  PATCH_SPEED is the rearward speed of the contact pacth with
// respect to the wheel's frame.
/*Three_Vector Vamos_Body::
Tire_Friction::friction_forces (double normal_force, double friction_factor,
                                                const Three_Vector& hub_velocity, 
                                                double patch_speed, double current_camber)
{
  double Fz = normal_force / 1000.0;
  double Fz_squared = Fz * Fz;
      
  // Evaluate the longitudinal parameters.
  const std::vector <double>& b = m_longitudital_parameters;
  double Cx = b [0];
  double Dx = friction_factor * (b [1] * Fz_squared + b [2] * Fz);
  double Bx = (b [3] * Fz_squared + b [4] * Fz) * exp (-b [5] * Fz) 
      / (Cx * Dx);
  double Ex = b [6] * Fz_squared + b [7] * Fz + b [8];
  double Shx = b [9] * Fz + b [10];

  // Evaluate the transverse parameters.
  const std::vector <double>& a = m_transverse_parameters;
  double gamma = rad_to_deg (current_camber);

  double Cy = a [0];
  double Dy = friction_factor * (a [1] * Fz_squared + a [2] * Fz);
  double By = a [3] * sin (2.0 * atan (Fz / a [4])) 
      * (1.0 - a [5] * std::abs (gamma)) / (Cy * Dy);
  double Ey = a [6] * Fz + a [7];
  double Shy = a [8] * gamma + a [9] * Fz + a [10];
  double Svy = (a [11] * Fz + a [12]) * gamma * Fz 
      + a [13] * Fz + a [14];

  // Evaluate the formula for aligning torque.
  const std::vector <double>& c = m_aligning_parameters;
  double Cz = c [0];
  double Dz = friction_factor * (c [1] * Fz_squared + c [2] * Fz);
  double Bz = (c [3] * Fz_squared + c [4] * Fz) 
      * (1.0 - c [6] * std::abs (gamma))
      * exp (-c [5] * Fz) / (Cz * Dz);
  double Ez = (c [7] * Fz_squared + c [8] * Fz + c [9]) 
      * (1.0 - c [10] * std::abs (gamma));
  double Shz = c [11] * gamma + c [12] * Fz + c [13];
  double Svz = (c [14] * Fz_squared + c [15] * Fz) * gamma + c [16] * Fz 
      + c [17];

  // Calculate the slip parameters sigma and tan_alpha.  We have to
  // play some games to keep them reasonable in extreme conditions.

  // Leave the slip parameters at zero if the difference between the
  // patch speed and the hub velocity is very small.
  double sigma = 0.0;
  double tan_alpha = 0.0;
  if (std::abs (hub_velocity [0] - patch_speed) > MINSLIP)
      {
        // Put a lower limit on the denominator to keep sigma and
        // tan_alpha from getting out of hand at low speeds.
        double denom = std::max (std::abs (hub_velocity [0]), 3.0);
        sigma = (patch_speed - hub_velocity [0]) / denom;
        tan_alpha = hub_velocity [1] / denom;
      }

  double d_sigma = -Shx;
  double slip_x = -sigma / (1.0 + std::abs (sigma)) - d_sigma;

  double d_alpha = 
      deg_to_rad (-Shy - Svy / (By * Cy * Dy * (1.0 + (180.0 / pi - 1.0) * Ey)));
  // Genta lists d_alpha as -Shy - Svy / (By * Cy * Dy).  This is what
  // you get from the magic formula for Fy(alpha) using the small
  // angle approximation.  However, the constants calculated above are
  // for alpha in degrees.  The small-angle formulas atan(x) =
  // 180*x/pi and sin(x) = pi*x/180 for x << 180/pi give the formula
  // d_alpha shown above.  Ey, in general, is not << 1.  After we get
  // d_alpha, we have to convert it to radians.

  double slip_y = tan_alpha / (1.0 + std::abs (sigma)) + d_alpha;

  double total_slip = std::sqrt (slip_x * slip_x + slip_y * slip_y);
  double slip_percent = total_slip * 100.0;

  // Find the longitudinal force.
  double Fx = -Dx * sin (Cx * atan (Bx * (1.0 - Ex) * (slip_percent + Shx)
                                                      + Ex * atan (Bx * (slip_percent + Shx))));

  // Find the transverse force.
  double Fy = 
      -Dy * sin (Cy * atan (By * (1.0 - Ey) * (slip_percent + Shy)
                                      + Ey * atan (By * (slip_percent + Shy)))) + Svy;

  // Genta describes a more complicated method of combining Fx and Fy,
  // however I can't make sense of it.  No matter what I do I can't
  // get the Fx vs. Fy curves to look like anything reasonable.

  // Find the aligning torque.
  double Mz = 
      -Dz * sin (Cz * atan (Bz * (1.0 - Ez) * (slip_percent + Shz)
                                      + Ez * atan (Bz * (slip_percent + Shz)))) + Svz;

  if (total_slip > 1.0e-6)
      {
        Fx *= slip_x / total_slip;
        Fy *= slip_y / total_slip;
        Mz *= slip_y / total_slip;
      }
      
      //- Fy is the resulting combined slip lateral force
      //- Fy0 is the lateral force as calculated using the normal Fy formula
      //- Fx is the longitudinal force as calculated using the normal Fx formula
      //- Fx0 is the MAXIMUM longitudinal force possible (calculated as D+Sv in the Pacejka Fx formula).
      //double D = (b [1] * Fz_squared + b [2] * Fz);
      //double maxforce = Fy*sqrt(1.0-(Fx/D)*(Fx/D));
      double maxforce = b[2] * 4.0; //(b[1]*(normal_force / 1000.0)+b[2])*4.0;
      double combforce = sqrt(Fx*Fx + Fy*Fy);
      //double overforce = maxforce - combforce;
      if (combforce > maxforce && combforce > 1.0e-3)
      {
            //cout << "max: " << maxforce << ", " << combforce << " -- " << Fx << "," << Fy << " -- " << total_slip << endl;
            Fx *= maxforce / combforce;
            Fy *= maxforce / combforce;
      }
      
      //cout << slip_x << "," << total_slip << endl;

  // Set the volume of the tire squeal sound. 
  if (hub_velocity.abs () < 0.1)
      {
        m_slide = 0.0;
      }
  else
      {
        m_slide = total_slip;
        if (m_slide > 1.0)
            m_slide = 1.0;
      }

      //cout << Fx << endl;
      
  // Construct the friction vector.  The z-component is the aligning
  // torque.
  return Three_Vector (Fx, Fy, Mz);
}*/

//* Class Tire

//** Constructor
Vamos_Body::
Tire::Tire (double radius, double rolling_resistance_1,
                  double rolling_resistance_2, const Tire_Friction& friction, 
                  double inertia, double tread) :
  m_radius (radius),
  m_rolling_resistance_1 (rolling_resistance_1),
  m_rolling_resistance_2 (rolling_resistance_2),
  m_tire_friction (friction),
  m_inertia (inertia),
  m_rotational_speed (0.0),
  m_last_rotational_speed (0.0),
  m_slide (0.0),
  m_velocity (0.0, 0.0, 0.0),
  m_normal_ang_velocity (0.0),
  m_normal_force (0.0),
  m_camber (0.0),
  m_applied_torque (0.0),
  m_is_locked (false),
  m_material (0),
  m_feedback (0),
  m_tread (tread),
  m_friction1 (1.0),
  m_friction2 (1.0)
{
}

// Called by the wheel to update the tire.
void Vamos_Body::
Tire::input (const Three_Vector& velocity, 
                   double normal_ang_velocity,
                   const Three_Vector& normal_force,
                   double camber,
                   double torque,
                   bool is_locked,
                   Material_Handle material)
{
  orient_frame_with_unit_vector (normal_force.unit ());

  m_velocity = rotate_in (velocity);
  m_normal_ang_velocity = normal_ang_velocity;
  m_normal_force = normal_force.abs ();
  m_camber = camber;
  m_applied_torque = torque;
  m_is_locked = is_locked;
  m_material = material;
}

// Orient the tire's z-axis with the normal force.
void Vamos_Body::
Tire::orient_frame_with_unit_vector (const Three_Vector& normal_unit_vector)
{
  Three_Vector rotation_axis = 
      Three_Vector (-normal_unit_vector [1], normal_unit_vector [0], 0.0);

  double length = sqrt (normal_unit_vector [0] * normal_unit_vector [0]
                                    + normal_unit_vector [1] * normal_unit_vector [1]);
  double rotation_angle = asin (length);

  orient (Three_Matrix (1.0));
  rotate (rotation_axis.unit () * rotation_angle);
}

void Vamos_Body::
Tire::find_forces ()
{
  // m_force is the force of the road on the tire.  The force of the
  // tire on the body must be calculated.  The transverse component
  // won't change, but the longitudial component will be affected by
  // the tire's ability to move in that direction, and by applied
  // foces (acceleration and braking).

  // Skip this step if we don't have a surface yet.
  if (m_material.null ())
      return;

  m_slide = 0.0;

  if (m_normal_force <= 0.0)
      {
        m_force.zero ();
        m_torque.zero ();
        return;
      }

      double sh, ah, nf;
      nf = (m_normal_force / 1000.0);
      if (nf < HAT_LOAD)
      {
            sh = sigma_hat[0];
            ah = alpha_hat[0];
      }
      else if (nf > HAT_LOAD*HAT_ITERATIONS)
      {
            sh = sigma_hat[HAT_ITERATIONS-1];
            ah = alpha_hat[HAT_ITERATIONS-1];
      }
      else
      {
            int lbound;
            double blend;
            lbound = (int)(nf/HAT_LOAD);
            lbound--;
            if (lbound < 0)
                  lbound = 0;
            blend = (nf-HAT_LOAD*(lbound+1))/HAT_LOAD;
            sh = sigma_hat[lbound]*(1.0-blend)+sigma_hat[lbound+1]*blend;
            ah = alpha_hat[lbound]*(1.0-blend)+alpha_hat[lbound+1]*blend;
      }
      
      
  // Get the friction force (components 0 and 1) and the aligning
  // torque (component 2).
      double surface_friction_factor = (1.0-m_tread)*m_friction1 + m_tread*m_friction2;
      //cout << m_tread << ": " << m_friction1 << "," << m_friction2 << endl;
      //if (surface_friction_factor < 1.0) cout << surface_friction_factor << endl;
  Three_Vector friction_force = m_tire_friction.
      //friction_forces (m_normal_force, m_material->friction_factor (), m_velocity, speed(), m_camber, sh, ah);
      friction_forces (m_normal_force, surface_friction_factor, m_velocity, speed(), m_camber, sh, ah);
      
      //cout << m_normal_force << endl;
      
      //if (!firstprint)
      if (0)
      {
            ofstream of("/home/joe/temp/y.txt");
            double x;
            double load = 2.500;
            of << "Fx = c(";
            for (x = -2; x < 2; x += 0.01)
            {
                  /*float load = 2500;
                  float fspeed = 1.0;
                  Three_Vector fakevel(fspeed/(x+1),0,0);
                  cout << m_tire_friction.
            friction_forces (load, m_material->friction_factor (),
                                     fakevel, fspeed, m_camber)[0] << ", ";*/
                  //cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
                  //of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
                  of << m_tire_friction.Pacejka_Fx(x, load, 1.0) << ",";
            }
            of << "0)" << endl;
            //of.close();
            
            //ofstream of("/home/joe/temp/y3.txt");
            //double x;
            of << "Fy = c(";
            for (x = -20; x < 20; x += 0.1)
            {
                  /*float load = 2500;
                  float fspeed = 1.0;
                  Three_Vector fakevel(fspeed/(x+1),0,0);
                  cout << m_tire_friction.
            friction_forces (load, m_material->friction_factor (),
                                     fakevel, fspeed, m_camber)[0] << ", ";*/

                  //cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
                  //of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
                  of << m_tire_friction.Pacejka_Fy(x, load, 0, 1.0) << ",";
            }
            of << "0)" << endl;
            //of.close();
            
            //ofstream of("/home/joe/temp/y4.txt");
            //double x;
            of << "Mz = c(";
            for (x = -20; x < 20; x += 0.1)
            {
                  /*float load = 2500;
                  float fspeed = 1.0;
                  Three_Vector fakevel(fspeed/(x+1),0,0);
                  cout << m_tire_friction.
            friction_forces (load, m_material->friction_factor (),
                                     fakevel, fspeed, m_camber)[0] << ", ";*/
                  
                  
                  //cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
                  //of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
                  of << m_tire_friction.Pacejka_Mz(0, x, load, 0, 1.0) << ",";
            }
            of << "0)" << endl;
            
            of.close();
            
//          firstprint = true;
      }

  // the frictional force opposing the motion of the contact patch.
  m_force = Three_Vector (friction_force [0], friction_force [1], 0.0);

  // Apply the reaction torque from acceleration or braking.  In
  // normal conditions this torque comes from friction.  However, the
  // frictional force can sometimes be large when no acceleration or
  // braking is applied.  For instance, when you run into a gravel
  // trap.  In any case, the reaction torque should never be larger
  // than the applied accelerating or braking torque. 
  double reaction = m_force [0] * m_radius;
  if (((m_applied_torque > 0.0) && (reaction > m_applied_torque))
        || ((m_applied_torque < 0.0) && (reaction < m_applied_torque)))
      {
        reaction = m_applied_torque;
      }
      
      //cout << m_force[0] << endl;
      
      //cout << friction_force [2] << endl;

  m_torque = Three_Vector (0.0, reaction, friction_force [2]);
      
  double feedbackcoeff = 0.05;
  m_feedback = m_feedback*(1.0-feedbackcoeff) + friction_force[2]*feedbackcoeff;

  if (!m_is_locked)
      {
        double rolling_1 = m_rolling_resistance_1;
        if (speed () < 0.0)
            rolling_1 *= -1.0;
        // Include constant and quadratic rolling resistance.
        double rolling = m_rolling_resistance_factor  
            * (rolling_1 + m_rolling_resistance_2 * speed () * speed ());
        m_applied_torque -= (m_force [0] + rolling) * m_radius;
      }

  // Add the suface drag.
  m_force [0] -= m_rolling_drag * m_velocity [0];
  m_force [1] -= m_rolling_drag * m_velocity [1];

  m_slide = m_tire_friction.slide ();
      
      bool nan = false;
      if (!(m_force[0] < 0 || m_force[0] >= 0))
      {
            m_force[0] = 0;
            nan = true;
      }
      if (!(m_force[1] < 0 || m_force[1] >= 0))
      {
            m_force[1] = 0;
            nan = true;
      }
      if (!(m_force[2] < 0 || m_force[2] >= 0))
      {
            m_force[2] = 0;
            nan = true;
      }
      
      if (nan)    
            m_force.zero();
      
      nan = false;
      
      if (!(m_torque[0] < 0 || m_torque[0] >= 0))
      {
            m_torque[0] = 0;
            nan = true;
      }
      if (!(m_torque[1] < 0 || m_torque[1] >= 0))
      {
            m_torque[1] = 0;
            nan = true;
      }
      if (!(m_torque[2] < 0 || m_torque[2] >= 0))
      {
            m_torque[2] = 0;
            nan = true;
      }
      
      if (nan)
            m_torque.zero();
}

// Advance this suspension component forward in time by TIME.
void Vamos_Body::
Tire::propagate (double time) 
{
  m_last_rotational_speed = m_rotational_speed;
  if (m_is_locked)
      {
        // This causes speed() to return 0.0.
        m_rotational_speed = 0.0;
      }
  else
      {
        m_rotational_speed += m_applied_torque * time / m_inertia;
      }
}

void Vamos_Body::
Tire::rewind ()
{
  m_rotational_speed = m_last_rotational_speed;
}

// Return the position of the contact patch in the wheel's coordinate
// system.
Three_Vector Vamos_Body::
Tire::contact_position () const
{
  return Three_Vector (0.0, 0.0, -m_radius);
}

// Set the tire to its initial conditions.
void Vamos_Body::
Tire::reset ()
{
      //firstprint = false;
  m_rotational_speed = 0.0;
  m_force.zero ();
  m_torque.zero ();
      
      int i;
      for (i = 0; i < HAT_ITERATIONS; i++)
      {
            FindSigmaHatAlphaHat((double)(i+1)*HAT_LOAD, sigma_hat[i], alpha_hat[i]);
            //cout << i << ": " << sigma_hat[i] << "," << alpha_hat[i] << endl;
      }
}

void Vamos_Body::
Tire::FindSigmaHatAlphaHat(double load, double & sh, double & ah)
{
      double x, y, ymax;
      ymax = 0;
      for (x = -2; x < 2; x += 0.01)
      {
            y = m_tire_friction.Pacejka_Fx(x, load, 1.0);
            if (y > ymax)
            {
                  sh = x;
                  ymax = y;
            }
      }
      
      ymax = 0;
      for (x = -20; x < 20; x += 0.1)
      {
            y = m_tire_friction.Pacejka_Fy(x, load, 0, 1.0);
            if (y > ymax)
            {
                  ah = x;
                  ymax = y;
            }
      }
}

Generated by  Doxygen 1.6.0   Back to index