I need to calculate the configuration space obstacle to planning a path with a mobile robot. The idea is to divide the obstacles and the robot in triangles and test whether is there any triangle from the robot colliding with any triangle from the obstacles.
The approach to solve this is to test this between two triangles each time so I need to look if any of the 6 edges (3 for each triangle) divide the triangles so 3 vertex from one of them lie in one side and the other 3 vertex lie on the other side of the line.
I wrote some code to calculate the line equation (y = m*x + b) and I think it is correct, but I am having problems when the line is vertical (this means that m = -Inf) because MATLAB gives me a NaN when I calculate the equation for it. I am not sure how to handle this.
Here you can see a snippet from the code where I test the 3 edges from the robot triangle:
for i = 1:1:3 vertex_x = P1(edge(i,:),1); vertex_y = P1(edge(i,:),2); m = (vertex_y(2) - vertex_y(1))/(vertex_x(2) - vertex_x(1)); b = -m*vertex_x(1) + vertex_y(1); for j = 1:1:6 % For each vertex... pto = listaVertices(j,:); if (m*pto(1) + b > pto(2)) % Vertex lies below the edge... cont1 = cont1 + 1; elseif (m*pto(1) + b < pto(2)) % Vertex lies above the edge... cont2 = cont2 + 1; else % Vertex lie inside the edge... % Do nothing end end % 3 vertex on one side and 1 on the others side means they do not % collide. Two of the vertex always lie inside the line (the two vertex % of each edge). if (cont1 == 1 && cont2 == 3) || (cont1 == 3 && cont2 == 1) flag_aux = false; % Do not collide... end % Reset the counters for the 3 next edges... cont1 = 0; cont2 = 0; end
Anyone could help with this issue?