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

Go to the documentation of this file.

#include "stdafx.h"
#include "IDM.h"

#include "ConfigData.h"
extern CConfigData g_ConfigData;

IDM::IDM()
{
        LANECHANGE_MIN_GAP      = g_ConfigData.IDM.LANECHANGE_MIN_GAP;
        SAFE_BRAKING            = g_ConfigData.IDM.SAFE_BRAKING;
        
        A_FLAG          = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.A_FLAG;
        A_MIN           = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.A_MIN;
        B_FLAG          = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.B_FLAG;
        T_SLOPE         = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.T_SLOPE;
        V0_MIN          = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.V0_MIN;
        V0_SLOPE        = g_ConfigData.Road.RoadFeatures.Gradient.IDM_Modifiers.V0_SLOPE;

        m_T                     = 0.0;
        m_A                     = 0.0;
        m_B                     = 0.0;
        m_S0            = 0.0;
        m_S1            = 0.0;
        m_V0            = 0.0;
        m_Delta         = 0.0;
        m_Polite        = 0.0;
        m_Bias          = 0.0;
        m_DeltaAth      = 0.0;
        
        m_V0_BeforeSpeedLimit   = 0.0;
        m_V0_BeforeGradient             = 0.0;
        m_T_BeforeGradient              = 0.0;
        m_A_BeforeGradient              = 0.0;
        m_B_BeforeGradient              = 0.0;

        m_V0_Initial                    = 0.0;
}

IDM::~IDM()
{

}

// IDM driver behaviours

double IDM::update(double vel, double dist)
{
        double accel = dvdt(vel, dist);
        return accel;
}

double IDM::dvdt(double dv, double s)
{
        double s_str = s_star(dv);
        double intel_acc = m_A*(1-pow(m_speed/m_V0,m_Delta));
        double intel_dec = -m_A*pow(s_str/s,2);

        double net_acc = intel_acc + intel_dec;

        // THIS NEEDS TO BE CAPPED OR BREAKDOWN OCCURS (net_acc ~ -30000)
        // Test value of -100 given
        if(net_acc < SAFE_BRAKING)
                net_acc = SAFE_BRAKING;

        return net_acc;
}

double IDM::s_star(double dv)
{
        // Desired gap
        ASSERT(m_V0 > 0);       // it has to be because of the division & sqrt below
        ASSERT(m_A > 0);
        ASSERT(m_B > 0);
        double s_str = m_S0 + m_S1 * sqrt(m_speed/m_V0) 
                + m_T*m_speed + (m_speed*dv)/(2*sqrt(m_A*m_B));

        return s_str;
}

// For compatibility with the DriverModel base class

void IDM::setVel(double vel)
{
        m_speed = vel;
}

double IDM::getDesiredVel()
{
        return m_V0;
}


double IDM::LaneChange(double GapToFront, double GapToBack, double FrontChangeAccel, 
                                           double CurrentBackAccel, double ProposedBackAccel, bool overtake)
{
        double advantage = 0.0;
        double othersDisadv = 0.0;
        double netAdvantage = 0.0;

        if(GapToFront >= LANECHANGE_MIN_GAP && GapToBack >= LANECHANGE_MIN_GAP && ProposedBackAccel >= SAFE_BRAKING)
        {
                advantage = FrontChangeAccel + (overtake ? -1 : 1) * m_Bias;
                othersDisadv = CurrentBackAccel - ProposedBackAccel;
        }

        if(othersDisadv < 0.0)
                othersDisadv = 0.0;

        netAdvantage = advantage - othersDisadv * m_Polite;

        if(netAdvantage <= m_DeltaAth)  // if it's under the threshold
                netAdvantage = 0.0;

        return netAdvantage;
}


// Parameter sets and gets

// Note: unit conversions to be done in here

// Safe time headway, T

void IDM::set_T(double val)
{
        // Unit - input: secs
        //              - output: secs
        m_T = val;
}

double IDM::get_T()
{
        // Unit - input: secs
        //              - output: secs
        return m_T;
}

// Maximum acceleration, A

void IDM::set_A(double val)
{
        // Unit - input: m/s^2
        //              - output: m/s^2
        m_A = val;
        ASSERT(m_A > 0);
}

double IDM::get_A()
{
        // Unit - input: m/s^2
        //              - output: m/s^2 
        return m_A;
}

// Desired decceleration, B

void IDM::set_B(double val)
{
        // Unit - input: m/s^2
        //              - output: m/s^2 
        m_B = val;
        ASSERT(m_B > 0);
}

double IDM::get_B()
{
        // Unit - input: m/s^2
        //              - output: m/s^2 
        return m_B;
}

// Elastic jam distance, S0

void IDM::set_S0(double val)
{
        // Unit - input: m
        //              - output: m     
        m_S0 = val;
}

double IDM::get_S0()
{
        // Unit - input: m
        //              - output: m     
        return m_S0;
}

// Minimum jam distance, S1

void IDM::set_S1(double val)
{
        // Unit - input: m
        //              - output: m     
        m_S1 = val;
}

double IDM::get_S1()
{
        // Unit - input: m
        //              - output: m     
        return m_S1;
}

// Desired velocity, V0

void IDM::set_V0(double val)
{
        // Unit - input: km/h
        //              - output: m/s   
        m_V0 = val * KM_PER_H_TO_M_PER_S;
        m_V0_Initial = m_V0;
        ASSERT(m_V0 > 0);
}

double IDM::get_V0()
{
        // Unit - input: m/s
        //              - output: m/s //km/h    
        return m_V0;// * M_PER_S_TO_KM_PER_H;
}

// Acceleration exponent, Delta

void IDM::set_Delta(double val)
{
        // Unit - input: none
        //              - output: none
        m_Delta = val;
}

double IDM::get_Delta()
{
        // Unit - input: none
        //              - output: none
        return m_Delta;
}

// MOBIL politeness factor

void IDM::set_Polite(double val)
{
        // Unit - input: none
        //              - output: none
        m_Polite = val;
}

double IDM::get_Polite()
{
        // Unit - input: none
        //              - output: none
        return m_Polite;
}

// MOBIL right lane bias

void IDM::set_Bias(double val)
{
        // Unit - input: none
        //              - output: none
        m_Bias = val;
}

double IDM::get_Bias()
{
        // Unit - input: none
        //              - output: none
        return m_Bias;
}

// MOBIL overtaking threshold

void IDM::set_DeltaAth(double val)
{
        // Unit - input: none
        //              - output: none
        m_DeltaAth = val;
}

double IDM::get_DeltaAth()
{
        // Unit - input: none
        //              - output: none
        return m_DeltaAth;
}

void IDM::SetSpeedLimit(double newV0)
{
        m_bInSpeedLimit = true;
        m_V0_BeforeSpeedLimit = m_V0;

        m_V0 = newV0;
        ASSERT(m_V0 > 0);
        ASSERT(m_V0_BeforeSpeedLimit > 0);
}

// gradient is in percent, i.e 4.3% means gradient = 4.3
// gradient may be positive or negative
// POSITIVE gradient means UPHILL
void IDM::SetGradient(double gradient)
{
        m_bInGradient = true;   
        m_A_BeforeGradient = m_A;       m_T_BeforeGradient = m_T;
        m_B_BeforeGradient = m_B;       m_V0_BeforeGradient = m_V0;
        
        // increase T - regardless of uphill or downhill
        double ABSgradient = gradient < 0 ? -gradient : gradient;
        m_T += ABSgradient/T_SLOPE;     
        
        // Reduce A - for uphill
        // MAGIC NUMBER 10 accounts for component of gravity acceleration
        // parallel to road surface - sin of small angle
        if(A_FLAG)
        {
                m_A -= gradient/10;
                m_A = m_A < A_MIN ? A_MIN : m_A;        // set to min at least
        }
        
        // Increase B for uphill only - MAGIC NUMBER 10 as for A
        if(B_FLAG && gradient > 0)
                m_B += gradient/10;     
        
        // Reduce V0 for uphill only
        if(gradient > 0)
        {
                // reduce V0 from the 'free' desired velocity
                // so if not in speed limit zone, we're free to reduce to gradient, else
                if(!m_bInSpeedLimit)
                        m_V0 -= (gradient/V0_SLOPE)*KM_PER_H_TO_M_PER_S;        
                else    // we're in a speed limit so see if speed limit or gradient governs V0
                {
                        double FreeV0 = m_V0_Initial;   // Do NOT change value of m_V0_BeforeSpeedLimit here!
                        double SpeedLimitV0 = m_V0;
                        double GradientV0 = FreeV0 - (gradient/V0_SLOPE)*KM_PER_H_TO_M_PER_S;
                        m_V0 = SpeedLimitV0 < GradientV0 ? SpeedLimitV0 : GradientV0;
                }
                if(m_V0 < V0_MIN)
                {
                        //TRACE("*** V0 of: %f\t set to: %f\n",m_V0, V0_MIN);
                        m_V0 = V0_MIN;  // set to min at least
                }
        }       // end if gradient uphill
}

void IDM::ClearSpeedLimit()
{
        if(!m_bInSpeedLimit)
                TRACE("***SL: Removed but not in\n");
        ASSERT(m_V0_BeforeSpeedLimit > 0);      // if this happens it means vehicle wasn't added to segment but was removed from it
        m_bInSpeedLimit = false;
        m_V0 = m_V0_BeforeSpeedLimit;
}

void IDM::ClearGradient()
{
        if(!m_bInGradient)
                TRACE("***Grad: Removed but not in\n");
        ASSERT(m_V0_BeforeGradient > 0);        // if this happens it means vehicle wasn't added to segment but was removed from it

        m_bInGradient = false;
        m_T = m_T_BeforeGradient;
        m_A = m_A_BeforeGradient;
        m_B = m_B_BeforeGradient;
        m_V0 = m_V0_BeforeGradient;
}

void IDM::ClearRestriction()
{
        // This is only called if we are entering a clear road and
        // have already been removed from all segments.
        m_V0 = m_V0_Initial;
}

---