Determining rotation between two points in 2D array

Question

Let's say I have a 4 by 4 maze as shown below, and I've written a program that returns a possible path from start to finish as shown below:

[(3, 1), (3, 0), (2, 0), (3, 0), (3, 1), (3, 2), (2, 2), (2, 3), (1, 3), (1, 2), (0, 2), (1, 2), (1, 1), (1, 0), (0, 0), (0, 1)]

The start point is the first element, and the end point is the last element.

How could I add a third value to each of the tuple containing a rotation in degrees in increments of 90? If a rotation occurred, it should return the rotation, and direction, with - indicating a counter-clockwise turn. Assume you start facing north.

For example, for the first two points,

[(3, 1), (3, 0), ... ]

it should return,

[(3, 1, -90), (3, 0, 90), ... ]

as on (3, 1) you must turn to face west in order to move onto the next point, and on (3, 0) you must turn to face north to move onto the third point.

For a set of points where no rotation would occur, and a 180 degree rotation would occur,

[ ... (0, 2, 180), (1, 2, 90), (1, 1, 0), (1, 0, 90), ... ]

as at (0, 2) you need to turn from facing north to facing south in order to move on, and for (1, 1) you are already facing the right direction.

Answer 1

To add a third value to each tuple containing a rotation in degrees in increments of 90, you can use the following Python code:

import math

def normalize_vector(vector):
    length = math.sqrt(vector[0] ** 2 + vector[1] ** 2)
    if length == 0:
        return [0, 0]
    else:
        return [vector[0] / length, vector[1] / length]

def calculate_angle(vector1, vector2):
    angle_radians = math.atan2(vector2[1], vector2[0]) - math.atan2(vector1[1], vector1[0])
    
    # Convert radians to degrees
    angle_degrees = math.degrees(angle_radians)
    
    # Ensure the angle is in the range of -180 to 180 degrees
    if angle_degrees > 180:
        angle_degrees -= 360
    elif angle_degrees < -180:
        angle_degrees += 360
    
    return angle_degrees

# Example usage:
input_points = [
    [3, 1],
    [3, 0],
    [2, 0],
    [3, 0],
    [3, 1],
    [3, 2],
    [2, 2],
    [2, 3],
    [1, 3],
    [1, 2],
    [0, 2],
    [1, 2],
    [1, 1],
    [1, 0],
    [0, 0],
    [0, 1],
]

current_direction = [-1, 0]

# Store the results in a list
result = []

for i, point in enumerate(input_points[:-1]):
    current_direction = normalize_vector(current_direction)
    next_direction = normalize_vector([input_points[i + 1][0] - point[0], input_points[i + 1][1] - point[1]])
    angle = -calculate_angle(current_direction, next_direction)#negative sign (-) indicating a counter-clockwise turn
    

    # Ensure the angle is in the range of -180 to 180 degrees
    if angle > 180:
        angle -= 360
    elif angle < -180:
        angle += 360

    result.append([point[0], point[1], angle])
    current_direction = next_direction

# Print the results
print(result)

Output of the code: [[3, 1, -90.0], [3, 0, 90.0], [2, 0, 180.0], [3, 0, -90.0], [3, 1, -0.0], [3, 2, -90.0], [2, 2, 90.0], [2, 3, -90.0], [1, 3, -90.0], [1, 2, 90.0], [0, 2, 180.0], [1, 2, 90.0], [1, 1, -0.0], [1, 0, 90.0], [0, 0, 90.0]]

This code will take a list of points as input, where each point is represented as [x, y]. It calculates the rotation angle between consecutive points and appends the angle to each tuple in the format [x, y, rotation_angle]. The rotation_angle is in increments of 90 degrees, with a negative sign (-) indicating a counter-clockwise turn.