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.

Answer 2

"... 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. ..."

You wouldn't need a rotation direction, just the angle.

For example, (3, 1, w) would mean, "move to 3, 1, and rotate West".

n, e, s, w = 0, 90, 180, 270
p = [(3, 1, w), (3, 0, n), (2, 0, e),
     (2, 1, n), (1, 1, e), (1, 2, e), (1, 3, s), (2, 3, n),
     (1, 3, w), (1, 2, n), (0, 2, s),
     (1, 2, w), (1, 1, n), (0, 0, e), (0, 1, n)]

Answer 3

So you just need to find the angle between each movement. And as each movement is in the unitary directions (+1/-1 on the x or y-axis), you just need to find the angle between each movement.

This can be done like this in python:

import numpy as np
# initailize data
xx = np.array([(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)])

# find the delta or step taken between each row and fix the coordiantes 
vals = ((np.array(xx)[1:] - np.array(xx)[:-1]))
vals[:,[0,1]] = vals[:,[1,0]]
vals[:,1] = - vals[:,1]
vals = np.append([[0,1]], vals, axis=0)

Here vals is just the step taken in the standard coordinates i.e [1,0] means +1 movement on the standard x-axis.

# calculate the angle between each step
# I'm leaving intermediate steps for ease of understanding the code
for i in range(len(vals) -1):
  vector1 = vals[i,:]
  vector2 = vals[i+1,:]
  cosTh = np.dot(vector1,vector2)
  sinTh = np.cross(vector2,vector1)
  angle= np.rad2deg(np.arctan2(sinTh,cosTh))
  xx[i,2] = angle
print(xx)

Output:

array([[  3.,   1., -90.],
       [  3.,   0.,  90.],
       [  2.,   0., 180.],
       [  3.,   0., -90.],
       [  3.,   1.,   0.],
       [  3.,   2., -90.],
       [  2.,   2.,  90.],
       [  2.,   3., -90.],
       [  1.,   3., -90.],
       [  1.,   2.,  90.],
       [  0.,   2., 180.],
       [  1.,   2.,  90.],
       [  1.,   1.,   0.],
       [  1.,   0.,  90.],
       [  0.,   0.,  90.],
       [  0.,   1.,   0.]])

Answer 4

Here is a manual approach. Everything is commented. If you need further clarity please ask and I will update.

from collections import deque

amap = [(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)]

#compass directions in a rotatable container
dirs  = deque(('n', 'e', 's', 'w'))  

#initial facing direction
faced = 'n' 

#container for extended map
nmap = []

#row/colum current - row/column next
for (rc,cc), (rn,cn) in zip(amap, amap[1:]+amap[-1:]):
    #if the current and next columns/rows aren't identical
    #determine which direction "next" is in
    if not all((rc==rn,cc==cn)):
        if   rc==rn: dn = ('w','e')[cc<cn]
        elif cc==cn: dn = ('n','s')[rc<rn]
    
    else: #this is the last index of `map`
        nmap.append((rc, cc, 0))
        continue #or break, this is the very end
    
    #if we aren't already facing the proper direction
    if dn != faced: 
        i = 0
        #count how many times we need to rotate to be facing new direction
        while dirs[0] != dn:
            dirs.rotate(-1)
            i += 1
            
        #if we rotated more than twice, adjust to negative
        rot   = i*90+((0, -360)[i>2])
        #store new direction
        faced = dn
    
    #append to extended map data    
    nmap.append((rc, cc, rot))
        
print(*nmap, sep='\n')

results:

(3, 1, -90)
(3, 0, 90)
(2, 0, 180)
(3, 0, -90)
(3, 1, 0)
(3, 2, -90)
(2, 2, 90)
(2, 3, -90)
(1, 3, -90)
(1, 2, 90)
(0, 2, 180)
(1, 2, 90)
(1, 1, 0)
(1, 0, 90)
(0, 0, 90)
(0, 1, 0)