D:/~Research/Code/C++/EvolveTraffic/Vehicle.cpp

Go to the documentation of this file.

// Vehicle.cpp: implementation of the Vehicle class.
//

#include "stdafx.h"
#include "Vehicle.h"

#include "ConfigData.h"
extern CConfigData g_ConfigData;

Vehicle::Vehicle()
{
        ROAD_END_BUFFER                         = g_ConfigData.Road.ROAD_END_BUFFER;
        
        T_DELAY                                         = g_ConfigData.IDM.T_DELAY;
        MIN_SPACE_FOR_NEXT_VEHICLE      = g_ConfigData.IDM.MIN_SPACE_FOR_NEXT_VEHICLE;
        SAFE_BRAKING                            = g_ConfigData.IDM.SAFE_BRAKING;

        CRANE_AVERAGE_SPACING           = g_ConfigData.VehicleID.CRANE_AVERAGE_SPACING;
        CRANE_MAX_SPACING                       = g_ConfigData.VehicleID.CRANE_MAX_SPACING;
        LOWLOADER_MIN_MAX_SPACING       = g_ConfigData.VehicleID.LOWLOADER_MIN_MAX_SPACING;
        SMALL_TRUCK_NO_AXLES            = g_ConfigData.VehicleID.SMALL_TRUCK_NO_AXLES;

        DAYS_PER_MT                                     = g_ConfigData.Time.DAYS_PER_MT;
        MTS_PER_YR                                      = g_ConfigData.Time.MTS_PER_YR;
}

Vehicle::~Vehicle()
{

}


void Vehicle::update(double step)
{
        if( changeTime(step) )
                setChangeStatus(true);

        double accel = calcAccel();

        updateProperties(step, accel);
}

void Vehicle::update(double step, Vehicle *pV)
{
        bool change = changeTime(step);

        if(change)
                setChangeStatus(true);

        double accel = calcAccel(pV);

        updateProperties(step, accel);
}

double Vehicle::calcAccel()
{
        m_pDriver->setVel(m_Velocity);

        //MAGIC NUMBER: ROAD END
        double accel = m_pDriver->update(m_Velocity, (m_RoadLength + ROAD_END_BUFFER) - m_Position); 

        return accel;
}

double Vehicle::calcAccel(Vehicle *pV)
{
        m_pDriver->setVel(m_Velocity);

        double s = pV->getPos() - pV->getLength() - m_Position;

        double delta_v = m_Velocity - pV->getVelocity();
        double accel = m_pDriver->update(delta_v, s);

        return accel;
}

void Vehicle::updateProperties(double step, double accel)
{
        // Assume average velocity over timestep
        double v_old = m_Velocity;
        double v_new = m_Velocity + accel*step;
        
        // zero speeds may occur when very close to stationary obstacle
        // eliminate as per Treiber's comments to Carpool Tunnel        
        if(v_new < 0)
        {
                //TRACE("*** Backwards move prevented\n");
                v_new = 0.0;
        }

        m_Position += 0.5*(v_old + v_new)*step;

        m_RoadPosition = m_Position;
        m_Velocity = v_new;

        m_Acceleration = accel;
}

bool Vehicle::changeTime(double step)
{
        m_Tdelay += step;

        if (m_Tdelay >= T_DELAY)
        {
                m_Tdelay = m_Tdelay - T_DELAY;
                return true;
        }
        else {return false;}
}

DriverModel* Vehicle::getDriver()
{
         return m_pDriver;
}

void Vehicle::setDriver(IDM driver)
{
        m_IDMDriver = driver;
        m_pDriver = &m_IDMDriver;
}


int Vehicle::DecideLaneChange(  Vehicle* LeftFrontVehicle,      Vehicle* LeftBackVehicle,
                                                                Vehicle* RightFrontVehicle, Vehicle* RightBackVehicle,
                                                                bool LeftLaneExists,            bool RightLaneExists)
{
        // Note that a possible conflict between what is the lane's leftLane and rightLane
        // and the Vehicle's leftLane and Rightlane. For DirPos they are the same, but not 
        // for DirNeg. e.g.
        // DirPos - Lane 1: leftLane = 2: rightLane = 0;
        // DirNeg - Lane 5: leftLane = 6: rightLane = 4
        // Whereas for the Vehicle:
        // DirPos - in lane 1: lane to left = 2: lane to right = 0;
        // DirNeg - in lane 5: lane to left = 4: lane to right = 6;
        // Therefore:
        // THIS FUNCTION NAMES LANES AS VIEWED BY THE VEHICLE
        
        double LeftLaneAdvantage = 0.0;         double RightLaneAdvantage = 0.0;

        if(m_DriveOnRight)
        {       // we are overtaking properly
                if(LeftLaneExists)
                        LeftLaneAdvantage = LaneChangeAdvantage(LeftFrontVehicle, LeftBackVehicle, true);
                // or we are being bold
                if(RightLaneExists)
                        RightLaneAdvantage = LaneChangeAdvantage(RightFrontVehicle, RightBackVehicle, false);
        }
        else
        {       // we're boldly undertaking
                if(LeftLaneExists)
                        LeftLaneAdvantage = LaneChangeAdvantage(LeftFrontVehicle, LeftBackVehicle, false);
                // we are overtaking properly
                if(RightLaneExists)
                        RightLaneAdvantage = LaneChangeAdvantage(RightFrontVehicle, RightBackVehicle, true);
        }

        const double zero = 0.0001;
        if(LeftLaneAdvantage < zero && RightLaneAdvantage < zero)
                return 0;                                                                       // do not change lane
        else if(LeftLaneAdvantage > RightLaneAdvantage)
                return 1;                                                                       // move to the left lane
        else
                return 2;                                                                       // move to the right lane
}

double Vehicle::LaneChangeAdvantage(Vehicle* FrontVehicle, Vehicle* BackVehicle, bool overtake)
{
        // These are IDM properties and may be reomved from Constants.h
        double GapToFront = MIN_SPACE_FOR_NEXT_VEHICLE;                 double GapToBack = MIN_SPACE_FOR_NEXT_VEHICLE; 
        double CurrentBackAccel = SAFE_BRAKING;                                 double ProposedBackAccel= SAFE_BRAKING;
        double FrontChangeAccel = 0.0;

        // calculate the above lane change parameters
        if(FrontVehicle != NULL)
        {
                GapToFront = FrontVehicle->getPos() - FrontVehicle->getLength() - this->getPos();
                FrontChangeAccel = this->calcAccel(FrontVehicle) - this->getAccel();
        }
        else
                FrontChangeAccel = this->calcAccel() - this->getAccel();        // no front Vehicle

        if(BackVehicle != NULL)
        {
                GapToBack = this->getPos() - this->getLength() - BackVehicle->getPos();
                ProposedBackAccel = BackVehicle->calcAccel(this);
                // Since the acceleration of the back Vehicle at the next step
                // is affected by the presence of the Vehicle in front of it
                // we must take that into account and not an open-road acceleration
                // but only if there is a Vehicle in front!
                if(FrontVehicle != NULL)
                        CurrentBackAccel = BackVehicle->calcAccel(FrontVehicle);
                else
                        CurrentBackAccel = BackVehicle->getAccel();
        }

        double advantage =  m_pDriver->LaneChange(GapToFront, GapToBack, FrontChangeAccel, 
                                                                                                CurrentBackAccel, ProposedBackAccel, overtake);
        return advantage;
}



void Vehicle::setVelocity(double vel)
{
        m_Velocity = vel;
}

void Vehicle::setLength(double length)
{
        m_Length = length;
}

void Vehicle::setAccel(double acc)
{
        m_Acceleration = acc;
}

void Vehicle::setLane(int lane)
{
        m_Lane = lane;
}

void Vehicle::setDirection(bool DirPos)
{
        m_DirPos = DirPos;
}


void Vehicle::setPos(double p)
{
        m_Position = p;
}

void Vehicle::setChangeStatus(bool stat)
{
        m_bLaneChange = stat;
}

void Vehicle::setRoadPos(double r)
{
        m_RoadPosition = r;
}


double Vehicle::getPos()
{
        return m_Position;
}

double Vehicle::getDesiredVel()
{
        return m_pDriver->getDesiredVel();
}

double Vehicle::getRoadPos()
{
        return m_RoadPosition;
}

WORD Vehicle::getID()
{
        return m_ID;
}

bool Vehicle::getChangeStatus()
{
        return m_bLaneChange;
}

double Vehicle::getVelocity()
{
        return m_Velocity;
}

double Vehicle::getLength()
{
        return m_Length;
}

double Vehicle::getAccel()
{
        return m_Acceleration;
}

int Vehicle::getLane()
{
        return m_Lane;
}

bool Vehicle::getDirection()
{
        return m_DirPos;
}

double Vehicle::getGVW()
{
        return m_GVW;
}

void Vehicle::setGVW(double weight)
{
        m_GVW = weight;
}

WORD Vehicle::setID()
{
        // Small Truck or large truck
        m_ID = (m_NoAxles <= SMALL_TRUCK_NO_AXLES) ? 
                                                VEH_ID_SMALLTRUCK : VEH_ID_LARGETRUCK;


        double averageSpace = getAS(0);
        double maxSpace = 0;

        // For all the vehicle's axles
        for(int i = 1; i < m_NoAxles; i++)
        {
                // Obtain the average spacing
                averageSpace += getAS(i);

                // Obtain the maximum spacing
                if(getAS(i) > getAS(i-1))
                        maxSpace = getAS(i);
        }

        // If the maximum spacing is less than the maximum spacing for a crane and there are more than 2 axles
        if(maxSpace < CRANE_MAX_SPACING && m_NoAxles > 2)
        {
                if(averageSpace / m_vAxles.size() < CRANE_AVERAGE_SPACING) // And the average spacing is less than the maximum average for a crane
                        m_ID = VEH_ID_CRANE;    // It is a crane
        }

        // If the maximum spacing is at least the maximum for a low-loader and there are more than 2 axles
        if(maxSpace >= LOWLOADER_MIN_MAX_SPACING && m_NoAxles > 2)
                m_ID = VEH_ID_LOWLOADER;        //It is a low-loader

        return m_ID;
}

double Vehicle::getTime() const
{
        // working off a 10 month year & a 25 day month
        int s_per_hr = SECS_PER_HOUR;
        int s_per_day = SECS_PER_HOUR*HOURS_PER_DAY;

        int no_days = (m_Year - 5) * MTS_PER_YR * DAYS_PER_MT + (m_Month - 1) * DAYS_PER_MT + (m_Day-1);
        double time = no_days * s_per_day + m_Hour * s_per_hr + m_Min * SECS_PER_MIN + m_Sec + (double)m_Hndt/100;

        return time;
}

void Vehicle::setTime(double time)
{
        double temp = time;     
        const int mpy = MTS_PER_YR;     const int dpm = DAYS_PER_MT;

        m_Year  = (int)(temp/(MTS_PER_YR * DAYS_PER_MT * HOURS_PER_DAY * SECS_PER_HOUR));
        
        temp    = time - m_Year * MTS_PER_YR * DAYS_PER_MT * HOURS_PER_DAY * SECS_PER_HOUR;     
        m_Month = (int)(temp/(DAYS_PER_MT * HOURS_PER_DAY * SECS_PER_HOUR));
        
        temp    = temp - m_Month * DAYS_PER_MT * HOURS_PER_DAY * SECS_PER_HOUR; 
        m_Day   = (int)(temp/(HOURS_PER_DAY * SECS_PER_HOUR));          
        
        temp    = temp - m_Day * HOURS_PER_DAY * SECS_PER_HOUR;                 
        m_Hour  = (int)(temp/(SECS_PER_HOUR));
        
        temp    = temp - m_Hour * SECS_PER_HOUR;                                        
        m_Min   = (int)(temp/(MINS_PER_HOUR));
        
        temp    = temp - m_Min * MINS_PER_HOUR;                                         
        m_Sec   = (int)(temp);
        
        temp    = temp - m_Sec;                                                 
        m_Hndt  = (int)(100 * temp);

}

void Vehicle::init(int RoadLength, bool DriveOnRight)
{
        m_RoadLength = RoadLength;
        m_DriveOnRight = DriveOnRight;
}

void Vehicle::setRoadLength(int length)
{
        m_RoadLength = length;
}

int Vehicle::getNoAxles()
{
        return m_NoAxles;
}

void Vehicle::setAS(int i, double s)
{
        m_vAxles.at(i)->setSpacing(s);
}

void Vehicle::setAW(int i, double w)
{
        m_vAxles.at(i)->setWeight(w);
}

void Vehicle::setAxles()
{
        for(int i = 0; i < m_NoAxles; i++)
        {
                Axle* axle = new Axle();
                m_vAxles.push_back(axle);
        }
}

double Vehicle::getAS(int i)
{
        return m_vAxles.at(i)->getSpacing();
}

double Vehicle::getAW(int i)
{
        return m_vAxles.at(i)->getWeight();
}


void Vehicle::setTotalNoLanesInRoad(int nLanes)
{
        m_TotalNoLanesInRoad = nLanes;
}

// map global lane to local lane
int Vehicle::getLaneNoInDirection()
{
        if(m_DirPos)            // if pos direction, local = global
                return m_Lane;
        else                            // if neg, it's not
        {
                ASSERT(m_Lane <= m_TotalNoLanesInRoad); // if it isn't we have a negative index returned
                int iLocalLane = m_TotalNoLanesInRoad - m_Lane + 1;     // must be 1-based lane index for file output
                return iLocalLane;
        }
}


void Vehicle::createCASTORVehicle(std::string data)
{
        m_Position = 0.0;
        m_RoadPosition = m_Position;

        m_Head          = atoi( data.substr(0,4).c_str() );
        m_Day           = atoi( data.substr(4,2).c_str() );
        m_Month         = atoi( data.substr(6,2).c_str() );
        m_Year          = atoi( data.substr(8,2).c_str() );
        m_Hour          = atoi( data.substr(10,2).c_str() );
        m_Min           = atoi( data.substr(12,2).c_str() );
        m_Sec           = atoi( data.substr(14,2).c_str() );
        m_Hndt          = atoi( data.substr(16,2).c_str() );
        m_Velocity      = atoi( data.substr(18,3).c_str() );
        m_GVW           = atoi( data.substr(21,4).c_str() );
        m_Length        = atoi( data.substr(25,3).c_str() );
        m_NoAxles       = atoi( data.substr(28,1).c_str() );
        m_intDir        = atoi( data.substr(29,1).c_str() );
        m_Lane          = atoi( data.substr(30,1).c_str() );
        m_Trns          = atoi( data.substr(31,3).c_str() );

        m_DirPos = m_intDir == 1 ? true : false;                // Required for compatibility with legacy

        // Length = length/10 for dm to meters 
        // Vel = vel / 10 for dm/s to meters/second
        // GVW * 0.981 for kg/100 to kN
        // Trns = trns/10 for dm to meters

        m_Length        /= 10;
        m_Velocity      /= 10;
        m_GVW           *= KG100_TO_KN;
        m_Trns          /= 10;

        m_Tdelay        = 0.0;
        
        setAxles();
        double W;
        double S;
        int j = 32;

        for(int i = 0; i < m_NoAxles; i++)
        {
                j += 2;
                W = atoi( data.substr(j,3).c_str() );
                W *= KG100_TO_KN;       // convert to kN
                setAW(i, W);
                
                j += 3;
                S = atoi( data.substr(j,2).c_str() );
                S /= 10;                // convert to m
                setAS(i, S);
        }

        m_ID = setID();
}

void Vehicle::createSAFTVehicle(std::string data)
{
        m_Position = 0.0;
        m_RoadPosition = m_Position;

        m_Order         = atoi( data.substr(0,5).c_str() );
        m_Head          = 20005;
        m_Day           = atoi( data.substr(10,2).c_str() );
        m_Month         = atoi( data.substr(12,2).c_str() );
        m_Year          = atoi( data.substr(14,2).c_str() );
        m_Hour          = atoi( data.substr(16,2).c_str() );
        m_Min           = atoi( data.substr(18,2).c_str() );
        m_Sec           = atoi( data.substr(20,2).c_str() );
        m_Hndt          = atoi( data.substr(22,2).c_str() );
        m_Velocity      = atoi( data.substr(24,3).c_str() );
        m_GVW           = atoi( data.substr(27,4).c_str() );
        m_Length        = atoi( data.substr(31,3).c_str() );
        m_NoAxles       = atoi( data.substr(34,1).c_str() );

        m_Lane = 1;
        m_intDir = 1;   // m_DirPointsToRight = true;

        // Length = length/10 for dm to meters 
        // Vel = vel / 10 for dm/s to meters/second
        m_Length        /= 10;
        m_Velocity      /= 10;
        
        m_Tdelay        = 0.0;

        setAxles();
        double W;
        double S;
        int j = 33;

        for(int i = 0; i < m_NoAxles; i++)
        {
                j += 2;
                W = atoi( data.substr(j,3).c_str() );
                setAW(i, W);

                if(i == m_NoAxles-1) break;

                j += 3;
                S = atoi( data.substr(j,2).c_str() );
                S /= 10;                // convert to m
                setAS(i, S);
        }

        m_ID = setID();         // why is this here
}

void Vehicle::writeCASTORData(char* pVehChar)
{
        // Length = length*10 for meters to decimeters
        // Vel = vel * 10 for metres/second to decimetres/second
        // GVW / 0.981 for KN to KG/100 
        // Trns = trns/10 for metres to decimetres

        int velocity    = Round(m_Velocity*10);
        int grossWeight = Round(m_GVW/KG100_TO_KN);
        int length              = Round(m_Length*10);
        int transPos    = Round(m_Trns*10);

        ostrstream oFile( pVehChar, sizeof(pVehChar)*78 );

        oFile.width(4); oFile << m_Head;
        oFile.width(2); oFile << m_Day;
        oFile.width(2); oFile << m_Month;
        oFile.width(2); oFile << m_Year;
        oFile.width(2); oFile << m_Hour;
        oFile.width(2); oFile << m_Min;
        oFile.width(2); oFile << m_Sec;
        oFile.width(2); oFile << m_Hndt;
        oFile.width(3); oFile << velocity;
        oFile.width(4); oFile << grossWeight;
        oFile.width(3); oFile << length;
        oFile.width(1); oFile << m_NoAxles;
        oFile.width(1); oFile << m_intDir;
        oFile.width(1); oFile << getLaneNoInDirection();        // local lane no in direction
        oFile.width(3); oFile << transPos;
        
        int j = 3;
        for(int i = 0; i < m_NoAxles; i++)
        {
                oFile.width(j); oFile << Round(this->getAW(i)/KG100_TO_KN);     // convert from kN to kg/100
                j = 2;

                if(i == 8) break;

                oFile.width(j); oFile << Round(this->getAS(i)*10);              // convert from m to dm
                j = 3;
        }

        for(i = m_NoAxles; i < CASTOR_MAX_AXLES; i++)
        {
                oFile.width(j); oFile << "0";
                j = 2;

                if(i == 8) break;

                oFile.width(j); oFile << "0";
                j = 3;
        }

        oFile << ends;
}
void Vehicle::writeSAFTData(char *pVehChar)
{
        // Length = length*10 for meters to decimeters
        // Vel = vel * 10 for metres/second to decimetres/second

        int velocity    = Round(m_Velocity*10);
        int length              = Round(m_Length*10);

        ostrstream oFile( pVehChar, sizeof(pVehChar)*78 );

        oFile.fill('0');
        oFile.width(5); oFile << m_Order;
        oFile.width(5); oFile << m_Head;
        oFile.width(2); oFile << m_Day;
        oFile.width(2); oFile << m_Month;
        oFile.width(2); oFile << m_Year;
        oFile.width(2); oFile << m_Hour;
        oFile.width(2); oFile << m_Min;
        oFile.width(2); oFile << m_Sec;
        oFile.width(2); oFile << m_Hndt;
        oFile.fill(' ');
        oFile.width(3); oFile << velocity;
        oFile.width(4); oFile << m_GVW;
        oFile.width(3); oFile << length;
        oFile.width(1); oFile << m_NoAxles;

        int j = 3;
        for(int i = 0; i < m_NoAxles; i++)
        {
                oFile.width(j); oFile << Round(this->getAW(i)/KG100_TO_KN);     // convert from kN to kg/100
                j = 2;

                if(i == m_NoAxles-1) 
                        break;

                oFile.width(j); oFile << Round(this->getAS(i)*10);              // convert from m to dm
                j = 3;
        }

        oFile << ends;
}

CString Vehicle::getDataString()
{
        CString dataStr, tempStr, strSplitter;
        strSplitter = "-----------------------------------\n";
        
        dataStr = "Vehicle Data:\n";
        dataStr += strSplitter;
        dataStr += "Direction: \t";
        tempStr = m_DirPos ? "Positive\n" : "Negative\n";
        dataStr += tempStr;
        
        tempStr.Format("Lane:        \t%d\n",                   m_Lane);                                                                                        dataStr += tempStr;
        tempStr.Format("Position:    \t%4.1f m\n",              m_RoadPosition);                                                                        dataStr += tempStr;
        tempStr.Format("Velocity:    \t%3.1f km/h\n",   m_Velocity*M_PER_S_TO_KM_PER_H);                                        dataStr += tempStr;
        tempStr.Format("Acceleration:\t%2.2f m/s^2\n",  m_Acceleration);                                                                        dataStr += tempStr;
        tempStr.Format("Desired Vel.:\t%3.1f km/h\n",   m_pDriver->getDesiredVel()*M_PER_S_TO_KM_PER_H);        dataStr += tempStr;
        
        dataStr += strSplitter;
        
        tempStr.Format("IDM T: \t\t%1.2f s\n",          m_IDMDriver.get_T() );                                                          dataStr += tempStr;
        tempStr.Format("IDM A: \t\t%2.2f m/s^2\n",      m_IDMDriver.get_A() );                                                          dataStr += tempStr;
        tempStr.Format("IDM B: \t\t%2.2f m/s^2\n",      m_IDMDriver.get_B() );                                                          dataStr += tempStr;
        tempStr.Format("IDM S0:\t\t%2.2f m\n",          m_IDMDriver.get_S0() );                                                         dataStr += tempStr;
        tempStr.Format("IDM S1:\t\t%2.2f m\n",          m_IDMDriver.get_S1() );                                                         dataStr += tempStr;
        tempStr.Format("IDM V0:\t\t%3.1f m/s\n",        m_IDMDriver.get_V0() );                                                         dataStr += tempStr;
        tempStr.Format("IDM Delta:\t%2.1f\n",           m_IDMDriver.get_Delta() );                                                      dataStr += tempStr;
        tempStr.Format("IDM Polite:\t%2.1f\n",          m_IDMDriver.get_Polite() );                                                     dataStr += tempStr;
        tempStr.Format("IDM Bias:\t%2.1f\n",            m_IDMDriver.get_Bias() );                                                       dataStr += tempStr;
        tempStr.Format("IDM DeltaAth:\t%2.1f\n",        m_IDMDriver.get_DeltaAth() );                                           dataStr += tempStr;
        
        dataStr += strSplitter;
        
        tempStr.Format("GVW:         \t%3.1f kN\n",             m_GVW);                                                                                         dataStr += tempStr;
        tempStr.Format("Length:      \t%2.1f m\n",              m_Length);                                                                                      dataStr += tempStr;
        tempStr.Format("No of Axles: \t%d\n",                   m_NoAxles);                                                                                     dataStr += tempStr;

        for(int i = 0; i < m_NoAxles; i++)
        {
                tempStr.Format("Axle %d:\t", i+1);                      dataStr += tempStr;
                tempStr.Format("%2.1f kN",  getAW(i) ); dataStr += tempStr;
                if(i < m_NoAxles - 1)
                {
                        tempStr.Format(" | %2.1f m\n",getAS(i) );       dataStr += tempStr;
                }               
        }

        return dataStr;
}

---