Logo Search packages:      
Sourcecode: vdrift version File versions

World.cc

//  Vamos Automotive Simulator
//  Copyright (C) 2001--2004 Sam Varner
//
//  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/world/World.h>

#include <cassert>

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

//-----------------------------------------------------------------------------
Vamos_World::
Times::Times () :
  m_current (0.0),
  m_previous (0.0),
  m_best (0.0),
  m_difference (0.0)
{
}

void Vamos_World::
Times::update (double time_step)
{
  m_current += time_step;
}

void Vamos_World::
Times::finalize ()
{
  if (m_best != 0.0)
    {
      m_difference = m_current - m_best;
    }

  if ((m_current < m_best) || (m_best == 0.0))
    {
      m_best = m_current;
    }

  m_previous = m_current;
  m_current = 0.0;
}

void Vamos_World::
Times::reset ()
{
  m_current = 0.0;
}

//-----------------------------------------------------------------------------
Vamos_World::
Timing_Info::Timing_Info () :
  m_sector (-1),
  m_previous_sector (-1),
  m_distance (0.0)
{
}

void Vamos_World::
Timing_Info::update_sector_info (int sector)
{
  if (sector == int (ma_sector_times.size ()))
    { // We're entering a sector for the first time.
      ma_sector_times.push_back (Times ());
    }

  if ((sector != m_sector) && (sector != -1))
    { // We have left a sector.
      if (m_sector != -1)
        {
          ma_sector_times [m_sector].finalize ();
          
          if (sector == 0)
            { // We have finished a lap.
              m_lap_times.finalize ();
            }
        }
      m_previous_sector = m_sector;
      m_sector = sector;
    }
}

void Vamos_World::
Timing_Info::update_times (int sector, double time_step)
{
  m_lap_times.update (time_step);
  if (sector != -1)
    {
      ma_sector_times [sector].update (time_step);
    }
}

void Vamos_World::
Timing_Info::update (double time_step, double distance, int sector)
{
  update_sector_info (sector);
  update_times (sector, time_step);
  m_distance = distance;
}

void Vamos_World::
Timing_Info::reset ()
{
  m_lap_times.reset ();
  ma_sector_times [m_sector].reset ();

  m_sector = -1;
  m_previous_sector = -1;
  m_distance = 0.0;
}

//-----------------------------------------------------------------------------
void Vamos_World::
Car_Information::reset ()
{
  timing.reset ();
  segment_index = 0;
  car->reset ();
}

//-----------------------------------------------------------------------------
Vamos_World::
World::World (TRACK * track) 
  : m_focused_car_index (0),
    p_track (track),
    m_gravity (-9.8)
{
}

extern bool verbose_output;

Vamos_World::
World::~World ()
{
      if (verbose_output)
            std::cout << "world deinit" << std::endl;
  for (std::vector <Car_Information>::iterator it = m_cars.begin ();
       it != m_cars.end ();
       it++)
    {
      delete it->car;
    }
      //std::cout << (void*)mp_track << std::endl;
      //delete mp_track;
      
      if (verbose_output)
            std::cout << "world deinit done" << std::endl;
}

extern float FrameTime();

void Vamos_World::
World::interact (Vamos_Body::Car* car, size_t segment_index)
{
      bool verbose = false;
      
      Vamos_Geometry::Material * tmatroad = new Vamos_Geometry::Material(
            Vamos_Geometry::Material::ASPHALT, 1.0, 1.0,
            1.0, 0.0, 
            0.0, 0.0, 
            0);
      Vamos_Geometry::Material * tmatwall = new Vamos_Geometry::Material(
            Vamos_Geometry::Material::CONCRETE, 1.0, -0.8,
            1.0, 0.0, 
            0.0, 0.0, 
            0);
      Vamos_Geometry::Material_Handle material = tmatroad;
      Vamos_Geometry::Material_Handle materialwall = tmatwall;
      
      size_t i;
      
      //do additional collisions for walls and stuff
      int loopcount = 0;
      bool col = true;
      const int maxloops = 4;
      while (col && loopcount < maxloops)
      {
            loopcount++;
            col = false;
            //find the orientation of the car
            QUATERNION rot;         
            double angle;
            Three_Vector aa = car->chassis().axis_angle(&angle);
            //rot.SetAxisAngle(angle*(3.141593/180.0),aa[0],aa[1],aa[2]);
            //rot.SetAxisAngle(angle*(3.141593/180), -aa[0], -aa[2], aa[1]);
            //rot.Rotate(3.141593/2.0,0,1,0);
            rot.SetAxisAngle(angle*(3.141593/180), aa[0], aa[2], -aa[1]);
            
            //find the position of the car
            Three_Vector chassispos = car->chassis().position();
            VERTEX cp;
            cp.Set(chassispos[0],chassispos[2],-chassispos[1]);
            
            VERTEX cmv;
            Vamos_Geometry::Three_Vector cm = car->chassis().center_of_mass();
            cmv.Set(cm[0], cm[2], -cm[1]);
            
            //get the collision mesh
            JOEMODEL * mod = car->GetCollisionModel();
            if (mod != NULL)
            {     
                  unsigned int nfaces = mod->GetFaces();
                  
                  //for the AABB calculation
                  float maxv[3];
                  float minv[3];
                  bool havevals[6];
                  int n = 0;
                  for (n = 0; n < 6; n++)
                        havevals[n] = false;
                  
                  unsigned int f;
                  unsigned int v;
                  VERTEX modelfaces[nfaces*3];
                  for (f = 0; f < nfaces; f++)
                  {
                        for (v = 0; v < 3; v++)
                        {
                              //transform the mesh faces to the world frame
                              modelfaces[f*3+v].Set(mod->GetVert(mod->GetFace(f)[v]));
                              //modelfaces[f*3+v].z = -modelfaces[f*3+v].z;
                              modelfaces[f*3+v] = cp + rot.RotateVec(cmv.ScaleR(-1.0)) + 
                                                            rot.RotateVec(modelfaces[f*3+v]);
                              
                              //z mirror due to weirdness in the object code
                              modelfaces[f*3+v].z = -modelfaces[f*3+v].z;
                              
                              //cache for bbox stuff
                              for (n = 0; n < 3; n++)
                              {
                                    if (modelfaces[f*3+v].v3()[n] > maxv[n] || !havevals[n])
                                    {
                                          maxv[n] = modelfaces[f*3+v].v3()[n];
                                          havevals[n] = true;
                                    }
                                    if (modelfaces[f*3+v].v3()[n] < minv[n] || !havevals[n+3])
                                    {
                                          minv[n] = modelfaces[f*3+v].v3()[n];
                                          havevals[n+3] = true;
                                    }
                              }
                        }
                  }
                  
                  //assign the world space AABB
                  AABB modelbbox;
                  VERTEX minvals, maxvals;
                  minvals.Set(minv);
                  maxvals.Set(maxv);
                  modelbbox.SetFromCorners(minvals, maxvals);
                  
                  VERTEX normal, colpt;
                  float depth;
                  
                  col = p_track->CollideModel(modelfaces, nfaces, modelbbox, colpt, false, normal, depth);
                  
                  if (verbose && col)
                  {
                        float miny = 10000;
                        for (f = 0; f < nfaces; f++)
                        {
                              for (v = 0; v < 3; v++)
                              {
                                    if (modelfaces[f*3+v].y < miny)
                                          miny = modelfaces[f*3+v].y;
                              }
                        }
                        
                        cout << "Loop " << loopcount << endl;
                        colpt.DebugPrint();
                        cout << miny << endl;
                        normal.DebugPrint();
                        cout << "depth: " << depth << endl << endl;
                  }
                  
                  if (col && !colpt.nan() && !normal.nan() && (depth >= 0 || depth < 0))
                  {
                        Three_Vector colpos;
                        Three_Vector norm;
                        
                        //normal.y = 0;
                        //normal = normal.normalize();
                        
                        norm.m_vec[0] = normal.x;
                        norm.m_vec[1] = -normal.z;
                        norm.m_vec[2] = normal.y;
                        
                        colpos.m_vec[0] = colpt.x;
                        colpos.m_vec[1] = -colpt.z;
                        colpos.m_vec[2] = colpt.y;
                        
                        car->chassis().single_point_contact(colpos, depth+0.001, norm, materialwall, FrameTime());
                  }
            }
      }
      
      //keep the wheels on the track
      i = 0;
      int wheelcount = 0;
      for (i = 0; i < 4; i++)
            car->SetColPatch(i, NULL);
      for (std::vector <Vamos_Body::Particle*>::iterator 
             it = car->chassis ().particles ().begin ();
         it != car->chassis ().particles ().end ();
         it++, i++)
    {
      if (!(*it)->single_contact ())
            {
                  const Three_Vector& pos = car->chassis ().contact_position (*it);
                  //const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
                  //double bump_parameter = car->distance_traveled () + (*it)->position () [0];
                  
                  VERTEX particlepos;
                  particlepos.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
                  
                  bool con = false;
                  Three_Vector norm;
                  double depth = 0;
                  VERTEX normal;
                  VERTEX origin = particlepos;
                  origin.y += 0.5;
                  BEZIER * tcolpatch = NULL;
                  OBJECTNODE * tcolnode = NULL;
                  depth = p_track->ElevationSeg(origin, normal, 10.0, tcolpatch, tcolnode) - particlepos.y;
                  
                  car->SetColPatch(wheelcount, tcolpatch);
                  
                  //calculate friction coefficients from collision data and track data
                  float f1 = 1.0;
                  float f2 = 1.0;
                  float rr = 1.0;
                  float rd = 0.0;
                  if (tcolpatch != NULL)
                  {
                        p_track->GetFrictionParams(f1, f2);
                  }
                  else if (tcolnode != NULL)
                  {
                        f1 = tcolnode->friction1;
                        f2 = tcolnode->friction2;
                        //cout << "Setting friction wheel " << wheelcount << " to " << f1 << "," << f2 << endl;
                  
                        //add in bump effects                     
                        float posx = origin.x;
                        float posz = origin.z;
                        float phase = 2*3.141593*(posx+posz) / tcolnode->bumplength;
                        float shift = 2.0*sin(phase*1.414214);
                        float amplitude = 0.25*tcolnode->bumpmag;
                        float bumpoffset = amplitude * (sin (phase + shift) + sin (1.414214*phase) - 2.0);
                        depth += bumpoffset;
                        
                        rr = tcolnode->rolling_resistance_factor;
                        rd = tcolnode->rolling_drag;
                  }
                  car->SetColParams(wheelcount, f1, f2, rr, rd);
                  
                  wheelcount++;

                  //depth = p_track->Elevation(particlepos, normal) - particlepos.y;
                  
                  if (depth < 0 || depth >= 0)
                  {
                        //depth = -particlepos.y;
                        //normal.Set(0,1,0);
                        if (depth > 0)
                              con = true;
                        norm.m_vec[0] = normal.x;
                        norm.m_vec[1] = -normal.z;
                        norm.m_vec[2] = normal.y;
                        if (con)
                        {
                              if (depth > 1.0)
                              {
                                    cout << "Large suspension displacement detected: " << depth << endl;
                              }
                              
                              //cout << p_track->Elevation(particlepos, normal) << endl;
                              //cout << depth << endl;
                              //particlepos.DebugPrint();   cout << endl;
                              //Vamos_Geometry::Material_Handle mat = &info.material;
                              car->chassis ().contact (*it, depth, 
                                                               norm, material);
                              //car->chassis().propagate_contact();
                        }
                  }
                  else
                        cout << "Not contacting with NaN surface" << endl;
            }
      }
      
      /*int loopcount = 0;
      int numcols = 1;
      const int maxloops = 50;
      while (numcols > 0 && loopcount < maxloops)
      {
            loopcount++;
            numcols = 0;
            i = 0;
            for (std::vector <Vamos_Body::Particle*>::iterator 
                   it = car->chassis ().particles ().begin ();
               it != car->chassis ().particles ().end ();
               it++, i++)
            {     
                  if ((*it)->single_contact ())
                  {
                        bool con = false;
                        
                        const Three_Vector& pos = car->chassis ().contact_position (*it);
                        const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
                        
                        Three_Vector norm;
                        double depth = 0;
                        
                        VERTEXD lastparticlepos;
                        lastparticlepos.Set(lastpos.m_vec[0],lastpos.m_vec[2],-lastpos.m_vec[1]);
                        
                        VERTEXD particleposd;
                        particleposd.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
                        
                        VERTEXD colpt;
                        double coldist = 0;
                        VERTEXD coldir;
                        VERTEXD normald;
                        coldir.Set(pos.m_vec[0]-lastpos.m_vec[0],pos.m_vec[2]-lastpos.m_vec[2],-(pos.m_vec[1]-lastpos.m_vec[1]));
                        double seglen = coldir.len();
                        //const float fluff = 0.5;
                        const double fluff = 0.00001;
                        if (seglen < fluff)
                              seglen = fluff;
                        //lastparticlepos = lastparticlepos - coldir.normalize().ScaleR(fluff/2.0);
                        con = p_track->CollideD(lastparticlepos, coldir, seglen, colpt, true, normald, coldist);
                        //con = p_track->Collide(lastparticlepos, particlepos, colpt, true, normal, coldist);
                        //if (particleposd.y < 0)
                        if (con)
                        //while (con && numcols < 5)
                        {
                              //car->chassis ().roll_back();
                              
                              //normal.Scale(100.0);
                              
                              norm.m_vec[0] = normald.x;
                              norm.m_vec[1] = -normald.z;
                              norm.m_vec[2] = normald.y;
                              
                              depth = (seglen - coldist)*coldir.normalize().dot(normald.ScaleR(-1.0));
                              //depth *= 2.0;
                              //depth = seglen - coldist;
                              //depth = - ((particleposd - colpt).dot(normald));
                              depth += fluff;
                              
                              //depth = -particleposd.y;
                              //normald.Set(0,1,0);
                              
                              if (verbose)
                              {
                                    cout << "Loop " << loopcount << endl;
                                    cout << "Collision " << numcols << endl;
                                    cout << "Particle " << i << endl;
                                    particleposd.DebugPrint();
                                    lastparticlepos.DebugPrint();
                                    colpt.DebugPrint();
                                    coldir.DebugPrint();
                                    coldir.normalize().DebugPrint();
                                    normald.DebugPrint();
                                    cout << coldir.len() << endl;
                                    cout << seglen << endl;
                                    cout << (colpt - lastparticlepos).len() << endl;
                                    cout << coldist << endl;
                                    cout << "depth: " << depth << endl << endl;
                              }
                              
                              if (depth >= 0)
                              {
                                    //roll back chassis state
                                    //car->chassis ().roll_back();
                                    
                                    //depth = 1.0;
                                    
                                    numcols++;
                                    
                                    car->chassis ().contact (*it, depth, norm, materialwall);
                                    car->chassis().propagate_contact();
                                    //Three_Vector colpos;
                                    //colpos.m_vec[0] = colpt.x;
                                    //colpos.m_vec[1] = -colpt.z;
                                    //colpos.m_vec[2] = colpt.y;
                                    //car->chassis().single_point_contact(colpos, depth, norm, materialwall, FrameTime());
                                    
                                    //car->chassis().kill_vel();

                              }
                              else 
                              {
                                    con = false;
                                    if (verbose)
                                          cout << "weird collision depth" << endl << endl;
                              }
                        }
                  }
            }
      }
      
      if (loopcount == maxloops)
      {
            car->chassis().roll_back();
            car->chassis().kill_vel();
      }*/

      // Handle air resistance.
      i = 0;
      for (std::vector <Vamos_Body::Particle*>::iterator 
             it = car->chassis ().particles ().begin ();
         it != car->chassis ().particles ().end ();
         it++, i++)
      {
            car->chassis ().wind (*it, Three_Vector(0,0,0)
                                          - car->chassis ().velocity (*it),
                              1.2);
      }
      
      /*for (std::vector <Vamos_Body::Particle*>::iterator 
             it = car->chassis ().particles ().begin ();
         it != car->chassis ().particles ().end ();
         it++, i++)
    {
      const Three_Vector& pos = car->chassis ().contact_position (*it);
            const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
            //double bump_parameter = car->distance_traveled () + (*it)->position () [0];
            
            //Vamos_Track::Track_Contact_Info info = mp_track->test_for_contact (pos, bump_parameter, segment_index);
            
            VERTEX particlepos;
            particlepos.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
            
            //keep the tires on the track
            bool con = false;
            Three_Vector norm;
            Vamos_Geometry::Material * tmat = new Vamos_Geometry::Material;
            Vamos_Geometry::Material_Handle material = tmat;
            double depth = 0;
            
            //do additional collisions for walls and stuff
            VERTEXD lastparticlepos;
            lastparticlepos.Set(lastpos.m_vec[0],lastpos.m_vec[2],-lastpos.m_vec[1]);
            
            VERTEXD particleposd;
            particleposd.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
            
            VERTEXD colpt;
            double coldist;
            VERTEXD coldir;
            VERTEXD normald;
            coldir.Set(pos.m_vec[0]-lastpos.m_vec[0],pos.m_vec[2]-lastpos.m_vec[2],-(pos.m_vec[1]-lastpos.m_vec[1]));
            double seglen = coldir.len();
            //const float fluff = 0.5;
            const double fluff = 0.00001;
            if (seglen < fluff)
                  seglen = fluff;
            //lastparticlepos = lastparticlepos - coldir.normalize().ScaleR(fluff/2.0);
            con = p_track->CollideD(lastparticlepos, coldir, seglen, colpt, false, normald, coldist);
            //OLD con = p_track->Collide(lastparticlepos, particlepos, colpt, true, normal, coldist);
            int numcols = 0;
            //while (con && numcols < 5)
            if (con)
            {
                  //car->chassis ().roll_back();
                  
                  //normal.Scale(100.0);
                  
                  norm.m_vec[0] = normald.x;
                  norm.m_vec[1] = -normald.z;
                  norm.m_vec[2] = normald.y;
                  
                  depth = (seglen - coldist)*coldir.normalize().dot(normald.ScaleR(-1.0));
                  //depth *= 10.0;
                  //depth = seglen - coldist;
                  //depth = - ((particleposd - colpt).dot(normald));
                  depth += fluff;
                  
                  if (verbose)
                  //if (0)
                  {
                        cout << "Second pass:  Collision " << count << " particle " << i << endl;
                        if (numcols != 0)
                              cout << "Sub-collision " << numcols << endl;
                        particleposd.DebugPrint();
                        lastparticlepos.DebugPrint();
                        colpt.DebugPrint();
                        coldir.DebugPrint();
                        coldir.normalize().DebugPrint();
                        normald.DebugPrint();
                        cout << coldir.len() << endl;
                        cout << seglen << endl;
                        cout << (colpt - lastparticlepos).len() << endl;
                        cout << coldist << endl;
                        cout << "depth: " << depth << endl << endl;
                  }
                  
                  count++;
                  
                  if (depth >= 0)
                  {
                        //roll back chassis state
                        //car->chassis ().roll_back();
                        
                        //depth = 1.0;
                        
                        numcols++;
                        
                        car->chassis ().contact (*it, depth, 
                                                   norm, materialwall);
                        
                        //car->chassis().propagate_contact();
                  }
                  //else if (verbose)     cout << "weird collision depth" << endl << endl;
            }
      }*/
}

void Vamos_World::
World::collide (Vamos_Body::Car* car1, Vamos_Body::Car* car2)
{
  // Handle collisions between the contact points of car1 and the
  // crash box of car 2. 
  for (std::vector <Vamos_Body::Particle*>::iterator 
         it = car1->chassis ().particles ().begin ();
       it != car1->chassis ().particles ().end ();
       it++)
    {
      if (car2->collision (car1->chassis ().transform_out ((*it)->position ())))
        {
          std::cout << "boom" << std::endl;
        }
    }
}
void Vamos_World::
World::reset ()
{
      reset(false);
}

// Place the car back on the track at its current position.
void Vamos_World::
World::reset (bool all)
{
      //if (all)
      {
            for (std::vector <Vamos_World::Car_Information>::iterator carit = m_cars.begin ();
                     carit != m_cars.end ();
                     carit++)
                  reset(&(*carit));
      }
      //else reset(focused_car());
      
      /*if (all)
      {
            for (std::vector <Vamos_World::Car_Information>::iterator carit = m_cars.begin ();
               carit != m_cars.end ();
               carit++)
            {
//                size_t& segment_index = carit->segment_index;
                  
                    // Find the position and orientation specified by the track.
//                  const Three_Vector& car_pos = carit->car->chassis ().position ();
                  Three_Matrix orientation(1.0);
                  orientation.identity();
                  VERTEX vpos = p_track->GetStart();
                  Three_Vector position(vpos.x, -vpos.z, vpos.y);
                  //const Three_Vector& position(p_track->GetStart().v3());
                  
                    // Adjust the vertical position so that at least one point on the
                    // car is in contact with the road, and the rest are above it.
                  
                    std::vector <Vamos_Body::Particle*>::iterator 
                        it = carit->car->chassis ().particles ().begin (); 
                  
                    const Three_Vector& pos = 
                        carit->car->chassis ().contact_position (*it);
                    double dist = carit->car->distance_traveled () 
                        + (*it)->position () [0];
                    //double elevation = mp_track->elevation (pos, dist, segment_index);
                    VERTEX cp;
                    cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
                    double elevation = p_track->Elevation (cp);
                    double max_diff = elevation - pos [2];
                  
                    for (size_t i = 0; 
                           it != carit->car->chassis ().particles ().end (); 
                           it++, i++)
                        {
                          const Three_Vector& pos = 
                              carit->car->chassis ().contact_position (*it);
                          dist = carit->car->distance_traveled () 
                              + (*it)->position () [0];
                              
                        cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
                        elevation = p_track->Elevation (cp);
                              
                  
                          double diff = elevation - pos [2];
                          if (diff > max_diff)
                              {
                                max_diff = diff;
                              }
                        }
                  
                    // Set the car's position and orientation.
                    carit->car->reset (position + Three_Vector (0.0, 0.0, max_diff), orientation);
            }
      }
      else
      {
//          size_t& segment_index = focused_car ()->segment_index;
            
            // Find the position and orientation specified by the track.
//          const Three_Vector& car_pos = focused_car ()->car->chassis ().position ();

            Three_Matrix orientation(1.0);
            orientation.identity();
            VERTEX vpos = p_track->GetStart();
            Three_Vector position(vpos.x, -vpos.z, vpos.y);
            
            // Adjust the vertical position so that at least one point on the
            // car is in contact with the road, and the rest are above it.
            
            std::vector <Vamos_Body::Particle*>::iterator 
            it = focused_car ()->car->chassis ().particles ().begin (); 
            
            const Three_Vector& pos = 
            focused_car ()->car->chassis ().contact_position (*it);
            double dist = focused_car ()->car->distance_traveled () 
            + (*it)->position () [0];
            //double elevation = mp_track->elevation (pos, dist, segment_index);
            
            VERTEX cp;
            cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
            double elevation = p_track->Elevation (cp);
            
            double max_diff = elevation - pos [2];
            
            for (size_t i = 0; 
               it != focused_car ()->car->chassis ().particles ().end (); 
               it++, i++)
            {
              const Three_Vector& pos = 
                  focused_car ()->car->chassis ().contact_position (*it);
              dist = focused_car ()->car->distance_traveled () 
                  + (*it)->position () [0];
                  
                  cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
                  elevation = p_track->Elevation (cp);
            
              double diff = elevation - pos [2];
              if (diff > max_diff)
                  {
                    max_diff = diff;
                  }
            }
            
            // Set the car's position and orientation.
            focused_car ()->car->reset (position + Three_Vector (0.0, 0.0, max_diff), orientation);
      }*/
}

void Vamos_World:: 
World::reset (Car_Information * cartoreset)
{
//    size_t& segment_index = cartoreset->segment_index;
            
      // Find the position and orientation specified by the track.
//    const Three_Vector& car_pos = cartoreset->car->chassis ().position ();
      /*const Three_Vector& position = 
      mp_track->reset_position (car_pos, segment_index);
      const Three_Matrix& orientation = 
      mp_track->reset_orientation (car_pos, segment_index);*/
      
      VERTEX vpos = p_track->GetStart();
      Three_Vector position(vpos.x, -vpos.z, vpos.y);
      
      QUATERNION qstart;
      /*qstart = p_track->GetStartOrientation();
      //qstart = qstart.ReturnConjugate();
      //qstart.Rotate(3.141593/2.0, 0,0,1);
      //qstart.Rotate(3.141593/2.0, 0,1,0);
      //qstart.Rotate(-3.141593/2.0, 1,0,0);
      //qstart.Rotate(3.141593/2.0, 0,1,0);
      qstart.Rotate(-3.141593/2.0, 1,0,0);
      QUATERNION goofyfoot;
      goofyfoot.Rotate(3.141593/2.0, 1,0,0);
      qstart.Multiply(goofyfoot);*/
      
      double o3x3[3][3];
      float orient[16];
      
      qstart = p_track->GetStartOrientation();
      QUATERNION qtemp = qstart;
      qstart.x = qtemp.x;
      qstart.y = qtemp.z;
      qstart.z = -qtemp.y;
      
      qstart.Rotate(-3.141593/2.0, 0,0,1);
      
      qstart.GetMat(orient);
      bool flip = false;
      for (int i = 0; i < 3; i++)
      {
            if (flip)
            o3x3[i][0] = orient[i+0];
            else
            o3x3[0][i] = orient[i+0];
      }
      for (int i = 0; i < 3; i++)
      {
            if (flip)
            o3x3[i][1] = orient[i+4];
            else
            o3x3[1][i] = orient[i+4];
      }
      for (int i = 0; i < 3; i++)
      {
            if (flip)
            o3x3[i][2] = orient[i+8];
            else
            o3x3[2][i] = orient[i+8];
      }
      //Three_Matrix orientation(1.0);
      //orientation.identity();
      Three_Matrix orientation(o3x3);
      
      cartoreset->car->reset (position, orientation);
      
      // Adjust the vertical position so that at least one point on the
      // car is in contact with the road, and the rest are above it.
      
      
      
      //Three_Vector mpos = 
            //cartoreset->car->chassis ().contact_position (*it);
      //double mdist = cartoreset->car->distance_traveled () 
            //+ (*it)->position () [0];
      //double elevation = mp_track->elevation (pos, dist, segment_index);
      
      VERTEX cp;
      //cp.Set(mpos.m_vec[0], mpos.m_vec[2], -mpos.m_vec[1]);
      double elevation;
      //elevation = p_track->Elevation (cp);
            
      //double max_diff = elevation - mpos [2];
      bool have_max = false;
      double max_diff = 0.0;
      
      //const Three_Vector& car_pos = cartoreset->car->chassis ().position ();
      
      for (std::vector <Vamos_Body::Particle*>::iterator it = 
            cartoreset->car->chassis ().particles ().begin (); 
         it != cartoreset->car->chassis ().particles ().end (); 
         it++)
      {
        Three_Vector pos = 
            cartoreset->car->chassis ().contact_position (*it);
        //dist = cartoreset->car->distance_traveled () 
            //+ (*it)->position () [0];
        //elevation = mp_track->elevation (pos, dist, segment_index);
            
            cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
            elevation = p_track->Elevation (cp);
      
            double diff = elevation - pos [2];
            //double diff = pos[2];
            if (diff > max_diff || !have_max)
            {
                  max_diff = diff;
                  have_max = true;
            }
            
            //cout << diff << endl;
      }
      
      // Set the car's position and orientation.
      cartoreset->car->reset (position + Three_Vector (0.0, 0.0, max_diff+0.0), orientation);
      //cartoreset->car->reset (position, orientation);
      //cout << max_diff << endl;
}

// Place the car back on the track at the starting line.
void Vamos_World::
World::restart ()
{
  focused_car ()->reset ();
}

// Set the acceleration due to gravity.  Always downward, regardless
// of sign.
void Vamos_World::
World::gravity (double g)
{
  m_gravity = -std::abs (g);
  if (focused_car () != 0)
    {
      focused_car ()->car->chassis ().
        gravity (Three_Vector (0.0, 0.0, m_gravity));
    }
}

void Vamos_World::
World::add_car (Vamos_Body::Car* car)
{
  car->chassis ().gravity (Three_Vector (0.0, 0.0, m_gravity));
  m_cars.push_back (Car_Information (car));
}

void Vamos_World::
World::set_focused_car (size_t index)
{
  assert (index < m_cars.size ());
  m_focused_car_index = index;
}

void Vamos_World::
World::focus_other_car (int delta)
{
  set_focused_car ((m_focused_car_index + delta) % m_cars.size ());
}

Vamos_World::Car_Information* Vamos_World::
World::focused_car ()
{
  if (m_focused_car_index >= m_cars.size ()) return 0;
  return &m_cars [m_focused_car_index];
}

Generated by  Doxygen 1.6.0   Back to index