collide.cpp

Go to the documentation of this file.

/***************************************************************************

    file                 : collide.cpp
    created              : Sun Mar 19 00:06:19 CET 2000
    copyright            : (C) 2000-2017 by Eric Espie, Bernhard Wymann
    email                : torcs@free.fr
    version              : $Id$

 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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.                                   *
 *                                                                         *
 ***************************************************************************/

#include "sim.h"

#define CAR_DAMMAGE 0.1


void SimCarCollideZ(tCar *car)
{
    int i;
    const float CRASH_THRESHOLD = -5.0f;

    if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
        return;
    }

    // Calculate transformation matrix to transform the surface normal into the car body local coordinate system.
    // Because this is just a rotation matrix the inverse is the transposed matrix.
    sgMat4 dst;
    sgMakeRotMat4(dst, RAD2DEG(car->DynGC.pos.az), RAD2DEG(car->DynGC.pos.ax), RAD2DEG(car->DynGC.pos.ay));
    sgTransposeNegateMat4(dst);
    
    // Get the normal of the surface under the center of gravity of the car. Beware, TR_LPOS_SEGMENT must
    // be set to get this result.
    t3Dd normal;
    tTrkLocPos normalPos;
    RtTrackGlobal2Local(car->trkPos.seg, car->DynGCg.pos.x, car->DynGCg.pos.y, &normalPos, TR_LPOS_SEGMENT);
    RtTrackSurfaceNormalL(&normalPos, &normal);

    // Now transform the normal to the car body coordinate system.
    sgVec3 dstVec;
    sgXformVec3(dstVec, (float *) &normal.x, dst);
    tdble sumdz = 0.0f;

    // Check if any car corners are below the surface. If so, try to rotate the corner out of the ground.
    for (i = 0; i < 4; i++) {
        tTrkLocPos cornerPos;
        RtTrackGlobal2Local(car->trkPos.seg, car->corner[i].pos.ax, car->corner[i].pos.ay, &cornerPos, TR_LPOS_SEGMENT);
        tdble z = RtTrackHeightL(&cornerPos);
        tdble dz = car->corner[i].pos.az - z;
        sumdz += dz;

        // Car corner below surface
        if (dz < 0.0f) {
            tdble dPosAy = dz/car->corner[i].pos.x*fabs(dstVec[0]);
            car->DynGCg.pos.ay += dPosAy;
            tdble dPosAx = dz/car->corner[i].pos.y*fabs(dstVec[1]);
            car->DynGCg.pos.ax -= dPosAx;
            car->collision |= SEM_COLLISION | SEM_COLLISION_Z;
        }
    }

    if (sumdz > 0.0f) sumdz = 0.0f;

    // The above part cannot resolve the collision in all cases, e.g. when all corners are below the surface.
    // For this case we check if the car floor below the center of gravity is eventually below the ground too.
    tdble z = RtTrackHeightG(car->trkPos.seg, car->DynGCg.pos.x, car->DynGCg.pos.y);
    tdble dz =  car->DynGCg.pos.z - (car->statGC.z - sumdz)/normal.z - z;

    if (dz < 0.0f) {
        tdble dotProd = (car->DynGCg.vel.x * normal.x + car->DynGCg.vel.y * normal.y + car->DynGCg.vel.z * normal.z);
        if (dotProd < 0.0f) {
            if (dotProd < CRASH_THRESHOLD) {
                car->collision |= SEM_COLLISION_Z_CRASH;
            }
            car->collision |= SEM_COLLISION | SEM_COLLISION_Z;
            car->DynGCg.vel.x -= normal.x * dotProd;
            car->DynGCg.vel.y -= normal.y * dotProd;
            car->DynGCg.vel.z -= normal.z * dotProd;

            if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
                car->dammage += (int)(normalPos.seg->surface->kDammage * fabs(dotProd) * rulesDamageFactor * simDammageFactor[car->carElt->_skillLevel]);
            }
        }
    }
}

const tdble BorderFriction = 0.0f;

// Collision of car/track borders.
// Be aware that it does not work for convex edges (e.g. e-track-2, end of the straight, left),
// because under these conditions it is possible that all car corners are on the track and
// the car body is partially outside the track.
void SimCarCollideXYScene(tCar *car)
{
    tTrackSeg *seg = car->trkPos.seg;
    tTrkLocPos trkpos;
    int i;
    tDynPt *corner;
    tdble initDotProd;
    tdble dotProd, cx, cy, dotprod2;
    tTrackBarrier *curBarrier;
    tdble dmg;

    if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
        return;
    }

    corner = &(car->corner[0]);
    for (i = 0; i < 4; i++, corner++) {
        seg = car->trkPos.seg;
        RtTrackGlobal2Local(seg, corner->pos.ax, corner->pos.ay, &trkpos, TR_LPOS_TRACK);
        seg = trkpos.seg;
        tdble toSide;

        if (trkpos.toRight < 0.0) {
            // collision with right border.
            curBarrier = seg->barrier[TR_SIDE_RGT];
            toSide = trkpos.toRight;
        } else if (trkpos.toLeft < 0.0) {
            // collision with left border.
            curBarrier = seg->barrier[TR_SIDE_LFT];
            toSide = trkpos.toLeft;
        } else {
            continue;
        }

        const tdble& nx = curBarrier->normal.x;
        const tdble& ny = curBarrier->normal.y;

        car->DynGCg.pos.x -= nx * toSide;
        car->DynGCg.pos.y -= ny * toSide;

        // Corner position relative to center of gravity.
        cx = corner->pos.ax - car->DynGCg.pos.x;
        cy = corner->pos.ay - car->DynGCg.pos.y;

        car->blocked = 1;
        car->collision |= SEM_COLLISION;

        // Impact speed perpendicular to barrier (of corner).
        initDotProd = nx * corner->vel.x + ny * corner->vel.y;

        // Compute dmgDotProd (base value for later damage) with a heuristic.
        tdble absvel = MAX(1.0, sqrt(car->DynGCg.vel.x*car->DynGCg.vel.x + car->DynGCg.vel.y*car->DynGCg.vel.y));
        tdble GCgnormvel = car->DynGCg.vel.x*nx + car->DynGCg.vel.y*ny;
        tdble cosa = GCgnormvel/absvel;
        tdble dmgDotProd = GCgnormvel*cosa;

        dotProd = initDotProd * curBarrier->surface->kFriction;
        car->DynGCg.vel.x -= nx * dotProd;
        car->DynGCg.vel.y -= ny * dotProd;
        dotprod2 = (nx * cx + ny * cy);

        // Angular velocity change caused by friction of colliding car part with wall.
        static tdble VELSCALE = 10.0f;
        static tdble VELMAX = 6.0f;
        car->DynGCg.vel.az -= dotprod2 * dotProd / VELSCALE;
        if (fabs(car->DynGCg.vel.az) > VELMAX) {
            car->DynGCg.vel.az = SIGN(car->DynGCg.vel.az) * VELMAX;
        }

        // Damage.
        dotProd = initDotProd;
        if (dotProd < 0.0f && (car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
            dmg = curBarrier->surface->kDammage * (0.5f*dmgDotProd*dmgDotProd + 0.005f*fabs(1.0f-cosa)*absvel) * rulesDamageFactor * simDammageFactor[car->carElt->_skillLevel];
            car->dammage += (int) dmg;
        } else {
            dmg = 0.0f;
        }

        dotProd *= curBarrier->surface->kRebound;

        // If the car moves toward the barrier, rebound.
        if (dotProd < 0.0f) {
            car->collision |= SEM_COLLISION_XYSCENE;
            car->normal.x = nx * dmg;
            car->normal.y = ny * dmg;
            car->collpos.x = corner->pos.ax;
            car->collpos.y = corner->pos.ay;
            car->DynGCg.vel.x -= nx * dotProd;
            car->DynGCg.vel.y -= ny * dotProd;
        }
    }
}


static void SimCarCollideResponse(void * /*dummy*/, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
    sgVec2 n;       // Collision normal delivered by solid: Global(point1) - Global(point2)
    tCar *car[2];   // The cars.
    sgVec2 p[2];    // Collision points delivered by solid, in body local coordinates.
    sgVec2 r[2];    // Collision point relative to center of gravity.
    sgVec2 vp[2];   // Speed of collision point in world coordinate system.
    sgVec3 pt[2];   // Collision points in global coordinates.

    int i;

    car[0] = (tCar*)obj1;
    car[1] = (tCar*)obj2;

    // Handle cars collisions during pit stops as well.
    static const int NO_SIMU_WITHOUT_PIT = RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT;

    if ((car[0]->carElt->_state & NO_SIMU_WITHOUT_PIT) ||
        (car[1]->carElt->_state & NO_SIMU_WITHOUT_PIT))
    {
        return;
    }

    if (car[0]->carElt->index < car[1]->carElt->index) {
        // vector conversion from double to float.
        p[0][0] = (float)collData->point1[0];
        p[0][1] = (float)collData->point1[1];
        p[1][0] = (float)collData->point2[0];
        p[1][1] = (float)collData->point2[1];
        n[0]  = (float)collData->normal[0];
        n[1]  = (float)collData->normal[1];
    } else {
        // swap the cars (not the same for the simu).
        car[0] = (tCar*)obj2;
        car[1] = (tCar*)obj1;
        p[0][0] = (float)collData->point2[0];
        p[0][1] = (float)collData->point2[1];
        p[1][0] = (float)collData->point1[0];
        p[1][1] = (float)collData->point1[1];
        n[0]  = -(float)collData->normal[0];
        n[1]  = -(float)collData->normal[1];
    }

    sgNormaliseVec2(n);
    
    // Because of the type conversion and the transformation to 2D the length of the normal might be 0 now, which could cause NaN
    if (isnan(n[0]) || isnan(n[1])) {
        return;
    }

    sgVec2 rg[2];   // radius oriented in global coordinates, still relative to CG (rotated aroung CG).
    tCarElt *carElt;

    for (i = 0; i < 2; i++) {
        // vector GP (Center of gravity to collision point). p1 and p2 are delivered from solid as
        // points in the car coordinate system.
        sgSubVec2(r[i], p[i], (const float*)&(car[i]->statGC));

        // Speed of collision points, linear motion of center of gravity (CG) plus rotational
        // motion around the CG.
        carElt = car[i]->carElt;
        float sina = sin(carElt->_yaw);
        float cosa = cos(carElt->_yaw);
        rg[i][0] = r[i][0]*cosa - r[i][1]*sina;
        rg[i][1] = r[i][0]*sina + r[i][1]*cosa;

        vp[i][0] = car[i]->DynGCg.vel.x - car[i]->DynGCg.vel.az * rg[i][1];
        vp[i][1] = car[i]->DynGCg.vel.y + car[i]->DynGCg.vel.az * rg[i][0];
    }

    // Relative speed of collision points.
    sgVec2 v1ab;
    sgSubVec2(v1ab, vp[0], vp[1]);

    // try to separate the cars. The computation is necessary because dtProceed is not called till
    // the collision is resolved. 
    for (i = 0; i < 2; i++) {
        sgCopyVec2(pt[i], r[i]);
        pt[i][2] = 0.0f;
        // Transform points relative to cars local coordinate system into global coordinates.
        sgFullXformPnt3(pt[i], car[i]->carElt->_posMat);
    }

    // Compute distance of collision points.
    sgVec3 pab;
    sgSubVec2(pab, pt[1], pt[0]);
    float distpab = sgLengthVec2(pab);

    sgVec2 tmpv;
    
    sgScaleVec2(tmpv, n, MIN(distpab, 0.05));
    // No "for" loop here because of subtle difference AddVec/SubVec. 
    if (car[0]->blocked == 0 && !(car[0]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
        sgAddVec2((float*)&(car[0]->DynGCg.pos), tmpv);
        car[0]->blocked = 1;
    }
    if (car[1]->blocked == 0 && !(car[1]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
        sgSubVec2((float*)&(car[1]->DynGCg.pos), tmpv);
        car[1]->blocked = 1;
    }

    // Doing no dammage and correction if the cars are moving out of each other.
    if (sgScalarProductVec2(v1ab, n) > 0) {
        return;
    }

    // impulse.
    float rpn[2];
    rpn[0] = sgScalarProductVec2(rg[0], n);
    rpn[1] = sgScalarProductVec2(rg[1], n);

    // Pseudo cross product to find out if we are left or right.
    // TODO: SIGN, scrap value?
    float rpsign[2];
    rpsign[0] =  n[0]*rg[0][1] - n[1]*rg[0][0];
    rpsign[1] = -n[0]*rg[1][1] + n[1]*rg[1][0];

    const float e = 1.0f;   // energy restitution

    float j = -(1.0f + e) * sgScalarProductVec2(v1ab, n) /
        ((car[0]->Minv + car[1]->Minv) +
        rpn[0] * rpn[0] * car[0]->Iinv.z + rpn[1] * rpn[1] * car[1]->Iinv.z);

    for (i = 0; i < 2; i++) {
        if (car[i]->carElt->_state & RM_CAR_STATE_NO_SIMU) {
            continue;
        }

        // Damage.
        tdble damFactor, atmp;
        atmp = atan2(r[i][1], r[i][0]);
        if (fabs(atmp) < (PI / 3.0)) {
            // Front collision gives more damage.
            damFactor = 1.5f;
        } else {
            // Rear collision gives less damage.
            damFactor = 1.0f;
        }

        if ((car[i]->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
            car[i]->dammage += (int)(CAR_DAMMAGE * fabs(j) * damFactor * rulesDamageFactor * simDammageFactor[car[i]->carElt->_skillLevel]);
        }

        // Compute collision velocity.
        const float ROT_K = 1.0f;

        float js = (i == 0) ? j : -j;
        sgScaleVec2(tmpv, n, js * car[i]->Minv);
        sgVec2 v2a;

        if (car[i]->collision & SEM_COLLISION_CAR) {
            sgAddVec2(v2a, (const float*)&(car[i]->VelColl.x), tmpv);
            car[i]->VelColl.az = car[i]->VelColl.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
        } else {
            sgAddVec2(v2a, (const float*)&(car[i]->DynGCg.vel), tmpv);
            car[i]->VelColl.az = car[i]->DynGCg.vel.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
        }

        static float VELMAX = 3.0f;
        if (fabs(car[i]->VelColl.az) > VELMAX) {
            car[i]->VelColl.az = SIGN(car[i]->VelColl.az) * VELMAX;
        }

        sgCopyVec2((float*)&(car[i]->VelColl.x), v2a);

        // Move the car for the collision lib.
        tCarElt *carElt = car[i]->carElt;
        sgMakeCoordMat4(carElt->pub.posMat, car[i]->DynGCg.pos.x, car[i]->DynGCg.pos.y,
                        car[i]->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
                        RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
        dtSelectObject(car[i]);
        dtLoadIdentity();
        dtMultMatrixf((const float *)(carElt->_posMat));

        car[i]->collision |= SEM_COLLISION_CAR;
    }
}


// Collision response for walls.
// TODO: Separate common code with car-car collision response.
static void SimCarWallCollideResponse(void *clientdata, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
    tCar* car;      // The car colliding with the wall.
    float nsign;    // Normal direction correction for collision plane.
    sgVec2 p;       // Cars collision point delivered by solid.

    // TODO: If other movable objects are added which could collide with the wall, it will be
    // necessary to validate if the object is actually a car.

    if (obj1 == clientdata) {
        car = (tCar*) obj2;
        nsign = -1.0f;
        p[0] = (float) collData->point2[0];
        p[1] = (float) collData->point2[1];
    } else {
        car = (tCar*) obj1;
        nsign = 1.0f;
        p[0] = (float) collData->point1[0];
        p[1] = (float) collData->point1[1];
    }

    sgVec2 n;       // Collision normal delivered by solid, corrected such that it points away from the wall.
    n[0] = nsign * (float) collData->normal[0];
    n[1] = nsign * (float) collData->normal[1];
    float pdist = sgLengthVec2(n);  // Distance of collision points.
    sgNormaliseVec2(n);
    
    // Because of the type conversion and the transformation to 2D the length of the normal might be 0 now, which could cause NaN
    if (isnan(n[0]) || isnan(n[1])) {
        return;
    }

    sgVec2 r;
    sgSubVec2(r, p, (const float*)&(car->statGC));

    tCarElt *carElt = car->carElt;

    sgVec2 vp;      // Speed of car collision point in global frame of reference.
    sgVec2 rg;      // raduis oriented in global coordinates, still relative to CG (rotated aroung CG).

    float sina = sin(carElt->_yaw);
    float cosa = cos(carElt->_yaw);
    rg[0] = r[0]*cosa - r[1]*sina;
    rg[1] = r[0]*sina + r[1]*cosa;

    vp[0] = car->DynGCg.vel.x - car->DynGCg.vel.az * rg[1];
    vp[1] = car->DynGCg.vel.y + car->DynGCg.vel.az * rg[0];

    sgVec2 tmpv;
    static const float CAR_MIN_MOVEMENT = 0.02f;
    static const float CAR_MAX_MOVEMENT = 0.05f;
    sgScaleVec2(tmpv, n, MIN(MAX(pdist, CAR_MIN_MOVEMENT), CAR_MAX_MOVEMENT));
    if (car->blocked == 0) {
        sgAddVec2((float*)&(car->DynGCg.pos), tmpv);
        car->blocked = 1;
    }

    // Doing no dammage and correction if the cars are moving out of each other.
    if (sgScalarProductVec2(vp, n) > 0) {
        return;
    }

    float rp = sgScalarProductVec2(rg, n);

    // Pesudo cross product to find out if we are left or right.
    // TODO: SIGN, scrap value?
    float rpsign = n[0]*rg[1] - n[1]*rg[0];

    const float e = 1.0f;   // energy restitution
    float j = -(1.0f + e) * sgScalarProductVec2(vp, n) / (car->Minv + rp * rp * car->Iinv.z);
    const float ROT_K = 0.5f;

    // Damage.
    tdble damFactor, atmp;
    atmp = atan2(r[1], r[0]);
    if (fabs(atmp) < (PI / 3.0)) {
        // Front collision gives more damage.
        damFactor = 1.5f;
    } else {
        // Rear collision gives less damage.
        damFactor = 1.0f;
    }

    static const float DMGFACTOR = 0.00002f;
    if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
        car->dammage += (int)(CAR_DAMMAGE * (DMGFACTOR*j*j) * damFactor * rulesDamageFactor * simDammageFactor[car->carElt->_skillLevel]);
    }

    sgScaleVec2(tmpv, n, j * car->Minv);
    sgVec2 v2a;

    if (car->collision & SEM_COLLISION_CAR) {
        sgAddVec2(v2a, (const float*)&(car->VelColl.x), tmpv);
        car->VelColl.az = car->VelColl.az + j * rp * rpsign * car->Iinv.z * ROT_K;
    } else {
        sgAddVec2(v2a, (const float*)&(car->DynGCg.vel), tmpv);
        car->VelColl.az = car->DynGCg.vel.az + j * rp * rpsign * car->Iinv.z * ROT_K;
    }

    static float VELMAX = 3.0f;
    if (fabs(car->VelColl.az) > VELMAX) {
        car->VelColl.az = SIGN(car->VelColl.az) * VELMAX;
    }

    sgCopyVec2((float*)&(car->VelColl.x), v2a);

    // Move the car for the collision lib.
    sgMakeCoordMat4(carElt->pub.posMat, car->DynGCg.pos.x, car->DynGCg.pos.y,
                    car->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
                    RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
    dtSelectObject(car);
    dtLoadIdentity();
    dtMultMatrixf((const float *)(carElt->_posMat));

    car->collision |= SEM_COLLISION_CAR;
}


// Remove wrecked cars to avoid glitches in collision detection and to improve performance.
// Called by RemoveCar.
void SimCollideRemoveCar(tCar *car, int nbcars)
{
    // Find the car to remove.
    int i;
    for (i = 0; i < nbcars; i++) {
        if (&SimCarTable[i] == car) {
            break;
        }
    }

    // Remove it.
    if (SimCarTable[i].shape != NULL) {
        dtDeleteObject(&(SimCarTable[i]));
        dtDeleteShape(SimCarTable[i].shape);
        SimCarTable[i].shape = NULL;
    }
}


// Static object and shape list.
// TODO: Dynamic implementation.
static DtShapeRef fixedobjects[100];
// Id to the next free slot in fixedobjects.
static unsigned int fixedid;


void SimCarCollideShutdown(int nbcars)
{
    int i;

    for (i = 0; i < nbcars; i++) {
        // Check if car has not been removed already (wrecked).
        if (SimCarTable[i].shape != NULL) {
            dtDeleteObject(&(SimCarTable[i]));
            dtDeleteShape(SimCarTable[i].shape);
        }
    }

    unsigned int j;
    for (j = 0; j < fixedid; j++) {
        dtClearObjectResponse(&fixedobjects[j]);
        dtDeleteObject(&fixedobjects[j]);
        dtDeleteShape(fixedobjects[j]);
    }
    fixedid = 0;

    dtClearDefaultResponse();
}


// Searching backwards for the first non wall segments to start.
// Used for the creation of collision detection model of the wall.
static tTrackSeg *getFirstWallStart(tTrackSeg *start, int side)
{
    tTrackSeg *first = start;

    // Moving backward out of wall.
    do {
        // A wall is a wall on the track if it is not the barrier at the same time.
        if (first->side[side] != NULL && first->side[side]->style == TR_WALL &&
            first->side[side]->side[side] != NULL)
        {
            first = first->prev;
        } else {
            break;
        }
    } while (first != start);

    // Searching forward for the first wall segment.
    start = first;
    do {
        if (first->side[side] != NULL && first->side[side]->style == TR_WALL &&
            first->side[side]->side[side] != NULL)
        {
            return first;
        } else {
            first = first->next;
        }
    } while (first != start);

    return NULL;
}


// Create the left walls, start must point to a segment with a wall on the left and a leading
// non wall segment.
// FIXME: Does not work for a closed ring "by design".
void buildWalls(tTrackSeg *start, int side) {

    if (start == NULL) {
        return;
    }

    tTrackSeg *current = start;
    bool close = false;

    // Going down the wall.
    do {
        tTrackSeg* s = current->side[side];
        tTrackSeg* p = current->prev->side[side];
        tTrackSeg* n = current->next->side[side];

        // Current segment is a wall?
        if (s != NULL && s->style == TR_WALL && s->side[side] != NULL) {

            float h = s->height;
            t3Dd svl = s->vertex[TR_SL];
            t3Dd svr = s->vertex[TR_SR];
            t3Dd evl = s->vertex[TR_EL];
            t3Dd evr = s->vertex[TR_ER];
            static float weps = 0.01f;

            // Close the start with a ploygon?
            if (p == NULL || p->style != TR_WALL ||
                (fabs(p->vertex[TR_EL].x - svl.x) > weps) ||
                (fabs(p->vertex[TR_ER].x - svr.x) > weps) ||
                (fabs(h - p->height) > weps) ||
                fixedid == 0)
            {
                // Enough space in array?
                if (fixedid >= sizeof(fixedobjects)/sizeof(fixedobjects[0])) {
                    GfError("fixedobjects full in %s, line %d\n", __FILE__, __LINE__);
                    return;
                }

                if (close == true) {
                    dtEndComplexShape();
                    GfError("Shape not closed %s, line %d\n", __FILE__, __LINE__);
                }

                // Start new shape.
                fixedobjects[fixedid] = dtNewComplexShape();
                fixedid++;
                close = true;

                dtBegin(DT_POLYGON);
                    dtVertex(svl.x, svl.y, svl.z);
                    dtVertex(svr.x, svr.y, svr.z);
                    dtVertex(svr.x, svr.y, svr.z + h);
                    dtVertex(svl.x, svl.y, svl.z + h);
                dtEnd();
            }

            // Build sides, left, top, right. Depending on how we want to use it we
            // can remove top perhaps.
            // TODO: do we want the top poly?
            if (close == true) {
                // Left.
                dtBegin(DT_POLYGON);
                    dtVertex(svl.x, svl.y, svl.z);
                    dtVertex(svl.x, svl.y, svl.z + h);
                    dtVertex(evl.x, evl.y, evl.z + h);
                    dtVertex(evl.x, evl.y, evl.z);
                dtEnd();
                /*
                // Top.
                dtBegin(DT_POLYGON);
                    dtVertex(svl.x, svl.y, svl.z + h);
                    dtVertex(svr.x, svr.y, svr.z + h);
                    dtVertex(evr.x, evr.y, evr.z + h);
                    dtVertex(evl.x, evl.y, evl.z + h);
                dtEnd();*/
                // Right.
                dtBegin(DT_POLYGON);
                    dtVertex(svr.x, svr.y, svr.z + h);
                    dtVertex(svr.x, svr.y, svr.z);
                    dtVertex(evr.x, evr.y, evr.z);
                    dtVertex(evr.x, evr.y, evr.z + h);
                dtEnd();
            } else {
                GfError("Shape not open %s, line %d\n", __FILE__, __LINE__);
            }

            // Close the end with a ploygon?
            if (n == NULL || n->style != TR_WALL ||
                (fabs(n->vertex[TR_SL].x - evl.x) > weps) ||
                (fabs(n->vertex[TR_SR].x - evr.x) > weps) ||
                (fabs(h - n->height) > weps))
            {
                if (close == true) {
                    dtBegin(DT_POLYGON);
                        dtVertex(svl.x, svl.y, svl.z);
                        dtVertex(svr.x, svr.y, svr.z);
                        dtVertex(svr.x, svr.y, svr.z + h);
                        dtVertex(svl.x, svl.y, svl.z + h);
                    dtEnd();

                    dtEndComplexShape();
                    close = false;
                } else {
                    GfError("Shape not open %s, line %d\n", __FILE__, __LINE__);
                }
            }

        }

        current = current->next;

    } while (current != start);

}


void
SimCarCollideConfig(tCar *car, tTrack *track)
{
    tCarElt *carElt;

    // Create car collision objects.
    carElt = car->carElt;
    // The current car shape is a box...
    car->shape = dtBox(carElt->_dimension_x, carElt->_dimension_y, carElt->_dimension_z);
    dtCreateObject(car, car->shape);

    car->collisionAware = 1;

    // Create fixed track collision objects (just pit wall for now).
    // TODO: car body/curbs collision.
    // TODO: car body/flat wall collision (e.g. for pavement, sidewalk).
    // TODO: define static objects in XML file/tTrack, collide with them as well.
}


void
SimCarCollideInit(tTrack *track)
{
    dtSetDefaultResponse(SimCarCollideResponse, DT_SMART_RESPONSE, NULL);
    // Hmm, why is caching disabled, are our objects too fast, so it does not work?
    // TODO: understand this and reconsider caching.
    dtDisableCaching();
    dtSetTolerance(0.001);
    
    fixedid = 0;

    if (track != NULL) {
        tTrackSeg *firstleft = getFirstWallStart(track->seg, TR_SIDE_LFT);
        tTrackSeg *firstright = getFirstWallStart(track->seg, TR_SIDE_RGT);

        buildWalls(firstleft, TR_SIDE_LFT);
        buildWalls(firstright, TR_SIDE_RGT);

        unsigned int i;
        for (i = 0; i < fixedid; i++) {
            dtCreateObject(&fixedobjects[i], fixedobjects[i]);
            dtSetObjectResponse(&fixedobjects[i], SimCarWallCollideResponse, DT_SMART_RESPONSE, &fixedobjects[i]);
        }
    }   
}


// TODO: Rename, because actually the dtProceed handles all solid managed objects, e.g. walls.
void
SimCarCollideCars(tSituation *s)
{
    tCar *car;
    tCarElt *carElt;
    int i;

    for (i = 0; i < s->_ncars; i++) {
        carElt = s->cars[i];
        if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
            continue;
        }

        car = &(SimCarTable[carElt->index]);
        dtSelectObject(car);
        // Fit the bounding box around the car, statGC's are the static offsets.
        dtLoadIdentity();
        // Place the bounding box such that it fits the car in the world.
        dtMultMatrixf((const float *)(carElt->_posMat));
        memset(&(car->VelColl), 0, sizeof(tPosd));
    }

    // Running the collision detection. If no collision is detected, call dtProceed.
    // dtProceed just works if all objects are disjoint.
    if (dtTest() == 0) {
        dtProceed();
    }

    for (i = 0; i < s->_ncars; i++) {
        carElt = s->cars[i];
        if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
            continue;
        }
        car = &(SimCarTable[carElt->index]);
        if (car->collision & SEM_COLLISION_CAR) {
            car->DynGCg.vel.x = car->VelColl.x;
            car->DynGCg.vel.y = car->VelColl.y;
            car->DynGCg.vel.az = car->VelColl.az;
        }
    }
}