calculate_visual_angles.py

import numpy as np
from numpy import linalg


def determine_heading_terminal_point(prev_agent_x,prev_agent_y,agent_x,agent_y):
    delta_x = agent_x - prev_agent_x
    delta_y= agent_y - prev_agent_y
    terminal_x = agent_x + delta_x
    terminal_y = agent_y + delta_y
    return(terminal_x,terminal_y)

def calculate_vector(initial_x,initial_y,terminal_x,terminal_y):
    x_component = terminal_x - initial_x
    y_component = terminal_y - initial_y
    return([x_component,y_component])

def angle_between_vectors_using_dot(vector1,vector2):
    norm_vector1 = linalg.norm(vector1)
    norm_vector2 = linalg.norm(vector2)
    angle = np.arccos(np.dot(vector1,vector2)/(norm_vector1*norm_vector2))
    return(angle)

def angle_between_vectors_using_cross(vector1,vector2):
    norm_vector1 = linalg.norm(vector1)
    norm_vector2 = linalg.norm(vector2)
    angle = np.arcsin(np.cross(vector1,vector2)/(norm_vector1*norm_vector2))
    return(angle)


def calculate_visual_angle(i,dictionary,currentAgent_x,currentAgent_y,visual_point_x,visual_point_y):
    # calculate vector for visual point (e.g. NP angle)
    agent_to_visual_point_vector = calculate_vector(currentAgent_x,currentAgent_y,visual_point_x,visual_point_y)
    # if after the first frame, past_agent is the previous position of the agent, and future_agent is the agent's current position
    pastAgent_x = dictionary[i-1]['agent_x_loc']
    pastAgent_y = dictionary[i-1]['agent_y_loc']
    # calculate the terminal point of the heading vector
    heading_terminal_x,heading_terminal_y = determine_heading_terminal_point(pastAgent_x,pastAgent_y,currentAgent_x,currentAgent_y)
    # calculate vector for heading
    agent_to_heading_vector = calculate_vector(currentAgent_x,currentAgent_y,heading_terminal_x,heading_terminal_y)
    # find angle between heading vector and visual point vector
    visual_angle = angle_between_vectors_using_cross(agent_to_visual_point_vector,agent_to_heading_vector)
    return(visual_angle)