diff --git a/utils/space_ops.py b/utils/space_ops.py index 40026a1a..3b65b4cd 100644 --- a/utils/space_ops.py +++ b/utils/space_ops.py @@ -2,7 +2,10 @@ import numpy as np from constants import OUT from constants import RIGHT +from constants import PI +from constants import TAU from functools import reduce +from utils.iterables import adjacent_pairs # Matrix operations @@ -164,3 +167,12 @@ def line_intersection(line1, line2): x = det(d, x_diff) / div y = det(d, y_diff) / div return np.array([x, y, 0]) + + +def get_winding_number(points): + total_angle = 0 + for p1, p2 in adjacent_pairs(points): + d_angle = angle_of_vector(p2) - angle_of_vector(p1) + d_angle = ((d_angle + PI) % TAU) - PI + total_angle += d_angle + return total_angle / TAU