batch_functions.py

from sympy import Point, Line
import numpy as np
import math
import replicate_Kountouriotis_et_al_2016
import calculate_parallel_curves

# Functions to Run Batch Mode
def get_agent_trace(dict,time_array):
    agent_x_array = []
    agent_y_array = []

    for i in range(0,len(time_array)):
        agent_x = dict[i]["agent_x_loc"]
        agent_y = dict[i]["agent_y_loc"]
        agent_x_array.append(agent_x)
        agent_y_array.append(agent_y)
    return(agent_x_array,agent_y_array)

def assign_arrays_to_manipulated_vars(var_type,var,num_of_conditions,**manipulated_var_range):
    if var_type == "consistent":
        var_array = np.full(shape = num_of_conditions, fill_value = var, dtype = float)
    
    elif var_type == "changes":
        manipulated_var_range_as_list = list(manipulated_var_range.values())
        min_value,max_value = manipulated_var_range_as_list[0],manipulated_var_range_as_list[1]
        var_array = np.linspace(min_value,max_value,num_of_conditions)
    
    return(var_array)

def get_farpoint_distance_array(velocity,look_ahead_distance_array):
    farpoint_distance_array = []
    for i in range(0,len(look_ahead_distance_array)):
        current_farpoint_distance = velocity * look_ahead_distance_array[i]
        farpoint_distance_array.append(current_farpoint_distance)
    return(farpoint_distance_array)

# Functions to plot RMSE over Time

# Error calculation functions ====================================================================================

find_distance_to_closest_point_on_circle = lambda x,y,circle_center_x,circle_center_y,radius: abs((np.sqrt((x - circle_center_x)**2 + (y - circle_center_y)**2)) - radius)

def find_distance_to_closest_point_on_straight(agent_x,agent_y,firstPointOnStraightLine_x,firstPointOnStraightLine_y,lastPointOnStraightLine_x,lastPointOnStraightLine_y):
    p1,p2 = Point(firstPointOnStraightLine_x,firstPointOnStraightLine_y), Point(lastPointOnStraightLine_x,lastPointOnStraightLine_y)
    s = Line(p1,p2)
    agent_loc = agent_x,agent_y
    shortest_distance = s.distance(agent_loc)
    return(shortest_distance)

def bias_inside_or_outside_of_straight(agent_x,agent_y,straight_array_x,straight_array_y,center_circle_x,center_circle_y,radius):
    agent_loc = agent_x,agent_y
    circle_center = center_circle_x,center_circle_y

    firstPointOnStraightLine_x,firstPointOnStraightLine_y = straight_array_x[0],straight_array_y[0]
    lastPointOnStraightLine_x,lastPointOnStraightLine_y = straight_array_x[-1],straight_array_y[-1]
    bias = find_distance_to_closest_point_on_straight(agent_x,agent_y,firstPointOnStraightLine_x,firstPointOnStraightLine_y,lastPointOnStraightLine_x,lastPointOnStraightLine_y)
    agent_to_center_distance = math.dist(agent_loc,circle_center)

    if agent_y > 0:
        error = bias*-1
        return(error)
    
    elif agent_y < 0:
        error = bias
        return(error)
    
    else:
        return(0)

def get_straight_bias_over_time(x_array,y_array,circle_center_x,circle_center_y,radius,straight_array_x,straight_array_y):
    error_over_time = []
    for i in range(0,len(x_array)):
        agent_x = x_array[i]
        agent_y = y_array[i]

        error = bias_inside_or_outside_of_straight(agent_x,agent_y,straight_array_x,straight_array_y,circle_center_x,circle_center_y,radius)
        error_over_time.append(error)
    return(error_over_time)


def bias_inside_or_outside_of_curve(agent_x,agent_y,circle_center_x,circle_center_y,radius):
    bias = find_distance_to_closest_point_on_circle(agent_x,agent_y,circle_center_x,circle_center_y,radius)
    agent_loc = agent_x,agent_y
    circle_center = circle_center_x,circle_center_y
    agent_to_center_distance = math.dist(agent_loc,circle_center)

    if agent_to_center_distance > radius:
        error = bias*-1
        return(error)
    
    elif agent_to_center_distance < radius:
        error = bias
        return(error)
    
    else:
        return(0)

def get_agent_location(dict):
    x_array = []
    y_array = []
    for i in range(0,len(dict)):
        x_loc = dict[i]['agent_x_loc']
        y_loc = dict[i]['agent_y_loc']
        x_array.append(x_loc)
        y_array.append(y_loc)
    return(x_array,y_array)

def get_bias_over_time(x_array,y_array,circle_center_x,circle_center_y, radius):
    error_over_time = []
    for i in range(0,len(x_array)):
        agent_x = x_array[i]
        agent_y = y_array[i]

        error = bias_inside_or_outside_of_curve(agent_x,agent_y,circle_center_x,circle_center_y,radius)
        error_over_time.append(error)
    return(error_over_time)

# Function that will generate error over time for all trials

def get_error_across_trials(agent_x_array,agent_y_array):
    # Get the center line, as well as the locations of the agent throughout the simulation
    center_arc_x,center_arc_y = replicate_Kountouriotis_et_al_2016.center_curve_x_array,replicate_Kountouriotis_et_al_2016.center_curve_y_array
    full_road_x, full_road_y = replicate_Kountouriotis_et_al_2016.center_x_array,replicate_Kountouriotis_et_al_2016.center_y_array
    first_straight_x,first_straight_y = replicate_Kountouriotis_et_al_2016.center_initial_straight_x_array,replicate_Kountouriotis_et_al_2016.center_initial_straight_y_array
    last_straight_x,last_straight_y = replicate_Kountouriotis_et_al_2016.center_last_straight_x_array,replicate_Kountouriotis_et_al_2016.center_last_straight_y_array

    # Get the center line, as well as the locations of the agent throughout the simulation
    center_arc_x,center_arc_y = replicate_Kountouriotis_et_al_2016.center_curve_x_array,replicate_Kountouriotis_et_al_2016.center_curve_y_array
    full_road_x, full_road_y = replicate_Kountouriotis_et_al_2016.center_x_array,replicate_Kountouriotis_et_al_2016.center_y_array
    first_straight_x,first_straight_y = replicate_Kountouriotis_et_al_2016.center_initial_straight_x_array,replicate_Kountouriotis_et_al_2016.center_initial_straight_y_array
    last_straight_x,last_straight_y = replicate_Kountouriotis_et_al_2016.center_last_straight_x_array,replicate_Kountouriotis_et_al_2016.center_last_straight_y_array

    # Adjust arrays to isolate agent_x and y coordinates to specific road segments (Note: last straight segment isn't analyzed here because the agent doesn't travel along it, only waypoints do)
    first_straight_only_agent_x,first_straight_only_agent_y = calculate_parallel_curves.slice_agentArray_wrt_roadSegment(agent_x_array,agent_y_array,"first_straight",replicate_Kountouriotis_et_al_2016.firstStraightResolution)
    arc_only_agent_x,arc_only_agent_y  = calculate_parallel_curves.slice_agentArray_wrt_roadSegment(agent_x_array,agent_y_array,"middle_arc",replicate_Kountouriotis_et_al_2016.firstStraightResolution)

    # Calculate error for road segments seperately
    error_over_time_for_curve = get_bias_over_time(arc_only_agent_x,arc_only_agent_y,0,(-1*replicate_Kountouriotis_et_al_2016.centerCircleRadius), replicate_Kountouriotis_et_al_2016.centerCircleRadius)
    error_over_time_for_first_straight = get_straight_bias_over_time(first_straight_only_agent_x, first_straight_only_agent_y,0,(-1*replicate_Kountouriotis_et_al_2016.centerCircleRadius), replicate_Kountouriotis_et_al_2016.centerCircleRadius,first_straight_x,first_straight_y)

    # Bring together total error across full road
    error_over_time = np.concatenate((error_over_time_for_first_straight,error_over_time_for_curve),axis=None)

    return(error_over_time)