1
$\begingroup$

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?

Improve this question
$\endgroup$
1
  • $\begingroup$ related: Computational Geometry; Algorithms and Applications; by de Berg, van Kreveld, Overmars, Schwarzkopf, especially chapter 13 on construction of configuration space and chapter 2 on boolean operations on polygons. $\endgroup$
    – Bending Unit 22
    May 16, 2016 at 14:14

1 Answer 1

Reset to default
0
$\begingroup$

For your particular question about m = Inf, you should first check if vertex_x(2) = vertex_x(1). Actually, I would implement the check to see if the absolute value of vertex_x(2) - vertex_x(1) < some_very_small_number. If that condition is true, then the equation for that line segment is y = vertex_x(1) and you just need to see if the other line segment contains that value for x. If the condition is false then use the check you already have.

Improve this answer
$\endgroup$
2
  • $\begingroup$ Thanks! This seems to work fine, but now I think I have some problems with the accuracy on the computation. Working with double data seems to be problemating when trying to make a logical '==' operation. In the case when I try to see whether the point lies inside the line (las if-else case) MATLAB doesn't find this expression true although the values (m*pto(1) + b == pto(2))are actually the same, but due to the accuracy the code enter on the '<' case instead of this last one. Do you know how to handle this?? $\endgroup$
    – osuarez
    May 16, 2016 at 19:03
  • $\begingroup$ Yes, you can use that same absolute value trick that I posted in the answer. Or, since you know you're dealing with straight line segments, you can check if one point is less than x and the next point is greater than x. $\endgroup$
    – SteveO
    May 16, 2016 at 19:04

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.