calculate_nearpoint_farpoint.py

import matplotlib.pyplot as plt
import numpy as np
from matplotlib import animation
import math
import calculate_parallel_curves

# CALCULATE NP and FP
# define patches that will move at every time step for NP and FP
def get_instantaneous_heading(x1,y1,x2,y2):
    delta_x = x2 - x1
    delta_y= y2 - y1
    slope = delta_y/delta_x
    return(slope)

def get_intercept(x1, y1, slope):
    intercept = y1 - (slope * x1)
    return(intercept)

def get_midpoint(x1,y1,x2,y2):
    midpoint_x = (x1+x2)/2
    midpoint_y = (y1+y2)/2
    return(midpoint_x,midpoint_y)

def get_distance(x1,y1,x2,y2):
    distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
    return(distance)

# Calculate center of the road by looking at points of road edges. 
# Consider the instantaneous slope of the edge line to get proper midpoint

def get_idx(agent_x,center_xlist):
    center_xlist = np.asarray(center_xlist)
    idx = (np.abs(center_xlist - agent_x)).argmin()
    return(idx)

# get_NP 
def get_distances(agent_x,agent_y,center_xlist,center_ylist, d):
    distance_list = []
    start_idx = get_idx(agent_x,center_xlist)
    for i in range(start_idx,len(center_xlist)):
        current_x = center_xlist[i]
        current_y = center_ylist[i]
        current_distance = get_distance(agent_x,agent_y,current_x,current_y)
        distance_list.append(current_distance)
        if current_distance > d:
            break
    return(distance_list,start_idx)

def get_coordinates(center_xlist,center_ylist, d, distance_list, start):
    distance_list = np.asarray(distance_list)
    idx = (np.abs(distance_list - d)).argmin()
    np_x = center_xlist[start+idx]
    np_y = center_ylist[start+idx]
    return(np_x,np_y)

def get_point(agent_x,agent_y,Road,point):
    if point == "nearpoint":
        d = Road.nearpoint_distance
    elif point == "farpoint":
        d = Road.farpoint_distance
    
    distance_list, start = get_distances(agent_x,agent_y,Road.center_x,Road.center_y, d)
    np_x, np_y = get_coordinates(Road.center_x,Road.center_y, d, distance_list, start)
    return(np_x,np_y)