I'm currently working on a Robotics project. The project objective is to create a nonholonomic robot path finding algorithm to be integrated into an actual STM Controller. I am currently stuck at solving the obstacle detection part using the separating axis theorem.
def has_collided(self, obj_1: List, obj_2: List): polygons = [obj_1, obj_2] for polygon in polygons: for i1 in range(len(polygon)): i2 = (i1 + 1) % len(polygon) p1 = polygon[i1] p2 = polygon[i2] normal = [p2 - p1, p1 - p2] min_1, max_1 = self.check_projection(normal, obj_1) min_2, max_2 = self.check_projection(normal, obj_2) if max_1 < min_2 or max_2 < min_1: return False return True
The process currently takes very long to run when I activate the above obstacle detection but the simulation does not detect the obstacles. Would appreciate some guidance on this :) Thanks in advance!