src/vamos/world/World.cc

Go to the documentation of this file.

//  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];
}

---