Vehicle Simulation Algorithms Planning Notes
Below are several outlines for various algorithms that will be needed to construct the traffic simulation applications. You must study each algorithm to see exactly what it is doing before you can begin to implement it in code. All of these are examples of the detail of pseudocode expected in the Functionality Outline document. You are expected to give credit for where these algorithms came from in your documentation.
Main Simulation Loop
Loop For each second of simulation time
Update the state of all traffic lights
Loop For each vehicle in the simulation
Update vehicle (Note: it should adjust its’ speed as needed,
determine its’ new location allowing for turns, and
move itself to the new location.)
End for each vehicle
If time to report simulation status (5 second intervals)
Report:
Total elapsed simulation time
State of traffic lights north/south
State of traffic lights east/west
State of all vehicles
Vehicle type
Location in X,Y coordinates
Road vehicle is on
Direction moving (in degrees measured counter-clockwise from East)
Speed in MPH
Set next time to report status
End for each second
*see in-class slide TrafficSim_05.jpg
Locating the road a vehicle is on*
Information needed:
Vehicle current location (VX,VY)
Vehicle direction of travel (EAST = 0.0, NORTH = 90.0, WEST = 180.0, SOUTH = 270.0)
Search Algorithm:
For each road in the collection of roads
Get road start point, end point, number of lanes and direction of road
orientation (North-South or East-West)
Calculate ULX, ULY, LRX, LRY to define the road rectangle
Set isNSRoad flag TRUE if this road runs North-South
else set isNSRoad flag to FALSE for East-West
Determine if vehicle is on this road
if((VX >= ULX) & (VX <= LRX) & (VY >= ULY) & (VY <= LRY))
//Check in case vehicle is in an intersection and on 2 roads simultaneously
if(isNSRoad & ((dir==NORTH) || (dir==SOUTH)))
// Vehicle is on this road
else if(!isNSRoad & ((dir==EAST) || (dir==WEST)))
// Vehicle is on this road
end For each
This algorithm can also be used to determine which intersection, if any, a vehicle is in.
*see in-class slides TrafficSim_06.jpg and TrafficSim_07.jpg
Locating the next intersection a vehicle will reach on its current path*
Information needed:
Vehicle current location (VX,VY)
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Current road vehicle is traveling on (curRoad)
Define a variable (minDist) to hold closest distance to next intersection
Search Algorithm:
Initialize minDist to some very large value, like 65536.999
Initialize nextIntersec (pointer to an intersection) to NULL
For each intersection in the collection of intersections
Get this intersections mid-point (CenterX, CenterY)
Initialize checkIntersection flag to FALSE
// Is this intersection directly ahead of the vehicle on the current road?
if((vehDir==EAST) &
(thisIntersec.WestRoad == curRoad) &
(VX < thisIntersec.CenterX))
Set checkIntersection flag to TRUE
else if((vehDir==NORTH) &
(thisIntersec.SouthRoad == curRoad) &
(VY > thisIntersec.CenterY))
Set checkIntersection flag to TRUE
else if((vehDir==WEST) &
(thisIntersec.EastRoad == curRoad) &
(VX > thisIntersec.CenterX))
Set checkIntersection flag to TRUE
else if((vehDir==SOUTH) &
(thisIntersec.NorthRoad == curRoad) &
(VY < thisIntersec.CenterY))
Set checkIntersection flag to TRUE
if(checkIntersection is TRUE)
dist = sqrt(pow((VX-thisIntersec.CenterX), 2.0) +
pow((VY-thisIntersec.CenterY), 2.0))
if(dist < minDist)
minDist = dist
nextIntersec = thisIntersec
end For each intersection
Return nextIntersec as pointer to closest intersection on this road
Note: there is one rare instance when this can return NULL. If a vehicle is within an intersection that is the last one on a road in the direction of travel and it has gone just past the center of the intersection then no next intersection will be found. This rare occurrence will be handled in the movement algorithm.
*see in-class slide TrafficSim_09.jpg
Setting Vehicle Speed
Information needed:
Vehicle current location (VX, VY)
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Vehicle speed in MPH and MPS (meters / second)
Vehicle acceleration in meters/second/second.
Distance required to stop vehicle from current speed
Distance required to stop at road speed limit
Distance to the edge of the next intersection to be crossed
Road vehicle is on
Next intersection to be reached
Status of traffic light in the direction the vehicle is traveling
Getting the Information?
The vehicle move algorithm will have the following information to use:
VSmph = vehicle speed in miles per hour
VSmps = vehicle speed in meters per second
VAcc = vehicle acceleration in meters per second per second
Get a reference to the current road the vehicle is on:
SL = speed limit on this road in MPH
Get a reference to the next intersection the vehicle will reach on this road traveling in the current direction:
NextI = next intersection
Get a reference to the next intersection’s traffic light
TLStat = call traffic light to get the status (RED, AMBER, GREEN) of the traffic light. You will need to be able to get the status North/South or East/West depending on the direction the vehicle is traveling.
Calculate distance needed to stop vehicle safely from the current speed
TSDVS (total stopping distance in feet) = 1.1 * VSmph + 0.06 * VSmph2
Convert TSDVSin feet to TSDVSin meters
Calculate distance needed to stop vehicle safely from the speed limit
TSDSL (total stopping distance in feet) = 1.1 * SLmph + 0.06 * SLmph2
Convert TSDSL in feet to TSDSL in meters
Calculate distance to next intersection edge (AB – see diagram below)
Collect data
BC’ = (NumLanesNS == 4) ? (LaneWidth * 2.0) : (LaneWidth * 1.0)
AC’ = CX - VX
AB = AC’- BC’
Get SL from Road object
Get VS from Vehicle object
Get TLS (Traffic light state) from TraficLight object in next intersection
// Handle light being RED or AMBER
If((TLS == RED) OR (TLS == AMBER))
If(TSDSL< AB)// Still a ways from the intersection
// OK to accelerate
If(VSmps< SLmps )// If not at speed limit in meters/sec
VSmps += Vehicle acceleration// Increase speed by accel.
VSmph = VSmps / 0.44704// Set speed in MPH
if(VSmpsSLmps)// Check speed limit
VSmps = SLmps// if over set speed to SL
VSmph = VSmps / 0.44704 // also set MPH
// If dist to stop > dist to intersection then decelerate
Else If(TSDVS> AB) AND (VSmps != 0)
while ((TSDVS> AB) & (AB > LANE_WIDTH) & (VSmps >0)
VSmps –= VehicleAcceleration
VSmph = VSmps / 0.44704 // also set MPH
if (VSmps < 0)
VSmps = 0
VSmph = 0
recalculate new TSDVS
Convert TSDVSin feet to TSDVSin meters
if (VSmps== 0) break // end while loop
if(AB < LANE_WIDTH) // if we are this close then stop
VSmps = 0
VSmph = 0
Else // Handle light being GREEN
If(VSmps < SLmps )// If not at speed limit in meters/sec
VSmps += Vehicle acceleration// Increase speed by accel.
VSmph = VSmps / 0.44704// Set speed in MPH
if(VSmps > SLmps)// Check speed limit
VSmps = SLmps// if over set speed to SL
VSmph = VSmps / 0.44704 // also set MPH
Set new speed (MPH and MPS) in vehicle object
Speed Footnotes
Be extremely careful in how you are measuring speed. The data file gives you the road speed limit in Miles Per Hour (MPH). Vehicles must keep their speed in both MPH for reporting to the user and in Meter Per Second (MPS) for most of the calculations. All distances in the algorithm above are measured in meters and all speeds are given in MPS (meters per second) for speed and meters/second/second (m/s2) for acceleration.
The general formula to calculate the safe distance for a vehicle to stop from a given speed in Miles Per Hour is given as:
TSD (total stopping distance (ft.)) = 1.1 * speedmph + 0.06 * (speedmph)2
After calculating the total stopping distance in feet from the speed in miles per hour you will need to convert this to meters to stop with:
TSD *= 0.3048
Note that in the deceleration while loop the check is for (Total Stopping Distance > Distance to Intersection Edge) AND (Distance to Intersection Edge > 0). This is due to the rare possibility that the vehicle’s current location is between the intersection edge and the center of the intersection. In this situation the distance to the next intersection edge will be a negative distance and the while loop will never be able to exit since the TSD is limited to a minimum value of zero.
*see in-class slides TrafficSim_010.jpg and TrafficSim_11.jpg
Vehicle Movement
Movement Algorithm – Simplified version
Information needed:
Vehicle current X,Y location
Vehicle speed
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Algorithm:
Adjust vehicle speed as needed.
Determine vehicle location at end of next second
if(vehDir == EAST)
NewX = CurX + VehicleSpeedMPS
NewY = CurY
else if(vehDir == NORTH)
NewX = CurX
NewY = CurY - VehicleSpeedMPS
else if(vehDir == WEST)
NewX = CurX - VehicleSpeedMPS
NewY = CurY
else if(vehDir == SOUTH)
NewX = CurX
NewY = CurY + VehicleSpeedMPS
This version of the algorithm illustrates how to calculate the position of a vehicle at each one second interval. This simplified version of the algorithm cannot be implemented in the programming assignments, because it does not take into account turns.
*see in class slide TrafficSim_12.jpg
Movement Algorithm – Handling Left Turns
Information needed:
Vehicle current location (VX, VY)
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Vehicle speed in MPS (meters / second)
Center point of next intersection (CX, CY)
Distance vehicle will travel in one second (AC)
Distance to center of next intersection (AD)
Distance from center of intersection to center of lane after turn (BD)
Distance vehicle will travel after the turn (BC’)
Algorithm:
AC = distInOneSec = Vehicle current speed
BD = (NumLanesNS == 4) ? (LaneWidth * 1.5) : (LaneWidth * 0.5)
AD = CX – CurX
BC = AC – AD – BD
C’X = CX + BD
C’Y = CurY - BC
This must be adapted for the vehicle moving North, West, or South. You are not required to handle moving the vehicle into the left lane for left turns, but you may add this to the final algorithm if you wish.
*see in class slide TrafficSim_13.jpg
Movement Algorithm – Handling Right Turns
Information needed:
Vehicle current location (VX, VY)
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Vehicle speed in MPS (meters / second)
Center point of next intersection (CX, CY)
Distance vehicle will travel in one second (AC)
Distance to center of next intersection (AD)
Distance from center of intersection to center of lane before turn (BD)
Distance vehicle will travel after the turn (BC’)
Algorithm:
AC = distInOneSec = Vehicle current speed
BD = (NumLanesNS == 4) ? (LaneWidth * 1.5) : (LaneWidth * 0.5)
AD = CX – CurX
BC = AC – AD + BD
C’X = CX – BD
C’Y = CurY + BC
This must be adapted for the vehicle moving North, West, or South.
*see in class slide TrafficSim_14.jpg
Movement Algorithm – Complete and in detail
Information needed:
Vehicle current location (VX, VY)
Vehicle direction of travel (vehDir) (EAST = 0.0, NORTH = 90.0,
WEST = 180.0, SOUTH = 270.0)
Vehicle speed in MPS (meters / second)
Center point of next intersection (CX, CY)
Distance vehicle will travel in one second (AC)
Distance to center of next intersection (AD)
Distance from center of intersection to center of lane before turn (BD)
Distance vehicle will travel after the turn (BC’)
Boolean flag TurnDecided set to true if a turn at the next intersection has been decided.
Pointers to current road and road to turn on
Algorithm:
Get reference to current road the vehicle is on (curRoad)
if curRoad is NULL
// This is a very rare exception that can only happen if a
// vehicle has made a turn but is still inside an intersection,
// but has not reached the intersection mid point. Here we
// tweek the location to bring it back on a road just past the
// intersection mid point.
if vehDir == EAST
set VX = VX + LANE_WIDTH * 2.0
else if vehDir == NORTH
set VY = VY – LANE_WIDTH * 2.0
else if vehDir == WEST
set VX = VX – LANE_WIDTH * 2.0
else if vehDir == SOUTH
set VY = VY + LANE_WIDTH * 2.0
// Try again to get the curRoad
Get reference to current road the vehicle is on (curRoad)
if curRoad is NULL
Terminate the application with an error message
This should never happen.
Get speed limit on road in miles per hour (SLmph)
Get speed limit on road in meters per second (SLmps)
Get reference to the next intersection vehicle will reach (nextIntr)
if nextIntr is NULL
Handle location exception (see next algorithm)
return
if(NOT turnDecided) decide on turn direction and set flag TRUE
TLStat = call traffic light to get the status (RED, AMBER, GREEN) of the traffic light. You will need to be able to get the status North/South or East/West depending on the direction the vehicle is traveling.
Adjust vehicle speed (see algorithm above)
Get mid point of next intersection (CX, CY)
if(vehDIR == EAST)
newX = VX + VSmps
newY = VY
if(newX > CX) // passing the center of the intersection so handle turns
if(turn direction == LEFT)
Get road north from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for left turns
Set new vehicle direction to NORTH
else if(turn direction == RIGHT)
Get road south from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for right turns
Set new vehicle direction to SOUTH
Set TurnDecided to FALSE
else if(vehDir == NORTH)
newX = VX
newY = VY - VSmps
if(newY < CY) passing the center of the intersection so handle turns
if(turn direction == LEFT)
Get road west from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for left turns
Set new vehicle direction to WEST
else if(turn direction == RIGHT)
Get road east from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for right turns
Set new vehicle direction to EAST
Set TurnDecided to FALSE
else if(vehDIR == WEST)
newX = VX - VSmps
newY = VY
if(newX < CX) passing the center of the intersection so handle turns
if(turn direction == LEFT)
Get road south from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for left turns
Set new vehicle direction to SOUTH
else if(turn direction == RIGHT)
Get road north from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for right turns
Set new vehicle direction to NORTH
Set TurnDecided to FALSE
else if(vehDir == SOUTH)
newX = VX
newY = VY + VSmps
if(newYCY) passing the center of the intersection so handle turns
if(turn direction == LEFT)
Get road east from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for left turns
Set new vehicle direction to EAST
else if(turn direction == RIGHT)
Get road west from current intersection (tempRoad)
Get number of lanes on tempRoad
Calculate (newX, newY) as in algorithm given for right turns
Set new vehicle direction to WEST
Set TurnDecided to FALSE
Set vehicle location to (newX, newY)
Movement Algorithm – Handling the location exception in the Movement Algorithm, i.e. go ahead and make the turn taking the vehicle out of the intersection.
Information needed:
Reference to current intersection vehicle is in (thisIntr)
Reference to road vehicle will turn onto (tempRoad)
Algorithm:
Get reference to current intersection the vehicle is in (thisIntr)
Get intersection mid-point (CX, CY)
if(vehDIR == EAST)
if(turn direction == LEFT)
Get road north from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesNS)
Get road west from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesEW)
Set newX = CX + ((numLanesNS == 2) ? 0.5 : 1.5) * LANE_WIDTH
Set newY = CY - ((numLanesEW/ 2) * LANE_WIDTH)
Set new vehicle direction to NORTH
else if(turn direction == RIGHT)
Get road south from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesNS)
Get road west from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesEW)
Set newX = CX - ((numLanesNS == 2) ? 0.5 : 1.5) * LANE_WIDTH
Set newY = CY +((numLanesEW/ 2) * LANE_WIDTH)
Set new vehicle direction to SOUTH
else if(vehDir == NORTH)
if(turn direction == LEFT)
Get road west from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesEW)
Get road south from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesNS)
Set newX = CX - ((numLanesNS / 2) * LANE_WIDTH
Set newY = CY - ((numLanesEW == 2) ? 0.5 : 1.5)* LANE_WIDTH)
Set new vehicle direction to WEST
else if(turn direction == RIGHT)
Get road east from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesEW)
Get road south from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesNS)
Set newX = CX + ((numLanesNS / 2) * LANE_WIDTH
Set newY = CY + ((numLanesEW == 2) ? 0.5 : 1.5)*LANE_WIDTH)
Set new vehicle direction to EAST
else if(vehDIR == WEST)
if(turn direction == LEFT)
Get road south from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesNS)
Get road east from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesEW)
Set newX = CX - ((numLanesNS == 2) ? 0.5 : 1.5) * LANE_WIDTH
Set newY = CY + ((numLanesEW/ 2) * LANE_WIDTH)
Set newvehicle direction to SOUTH
else if(turn direction == RIGHT)
Get road north from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesNS)
Get road east from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesEW)
Set newX = CX + ((numLanesNS == 2) ? 0.5 : 1.5) * LANE_WIDTH
Set newY = CY - ((numLanesEW/ 2) * LANE_WIDTH)
Set newvehicle direction to NORTH
else if(vehDir == SOUTH)
if(turn direction == LEFT)
Get road east from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesEW)
Get road north from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesNS)
Set newX = CX + ((numLanesNS / 2) * LANE_WIDTH
Set newY = CY + ((numLanesEW == 2) ? 0.5 : 1.5) *LANE_WIDTH)
Set new vehicle direction to EAST
else if(turn direction == RIGHT)
Get road west from current intersection (tempRoad)
Get number of lanes on tempRoad (numLanesEW)
Get road north from current intersection (tempRoad). The one we are on.
Get number of lanes on tempRoad (numLanesNS)
Set newX = CX - ((numLanesNS / 2) * LANE_WIDTH
Set newY = CY - ((numLanesEW == 2) ? 0.5 : 1.5)*LANE_WIDTH)
Set newvehicle direction to WEST
Set new vehicle location to newX, newY
Set vehicle “turn decided” flag to false