79 lines
2.3 KiB
Matlab
79 lines
2.3 KiB
Matlab
function intersect = IsIntersecting (L1, L2, closestPoint, newPoint)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% function path = buildRRT(L1, L2, start, finish)
|
|
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
|
%
|
|
% Inputs:
|
|
% - L1: first length
|
|
% - L2: second length
|
|
% - closestPoint: start point x y
|
|
% - newPoint: end point x y
|
|
%
|
|
% Output:
|
|
% -path: Vector of points
|
|
%
|
|
% author: Marais Lucas
|
|
% date: 22/11/2023
|
|
x3 = [-L2 -L2 L2 L2];
|
|
y3 = [-L2 L2 L2 -L2];
|
|
x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
|
y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2];
|
|
x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
|
y2 = [-L1 L1 L1 -L1];
|
|
% checks if the path is crossed by an obstacle
|
|
crossesObstacle = false;
|
|
for i = 1:length(x1)
|
|
edge1 = [x1(i), y1(i), x1(mod(i, 4) + 1), y1(mod(i, 4) + 1)];
|
|
edge2 = [x2(i), y2(i), x2(mod(i, 4) + 1), y2(mod(i, 4) + 1)];
|
|
edge3 = [x3(i), y3(i), x3(mod(i, 4) + 1), y3(mod(i, 4) + 1)];
|
|
|
|
% Check if the line intersects with any obstacle edge
|
|
if doIntersect(closestPoint, newPoint, edge1(1:2), edge1(3:4)) || ...
|
|
doIntersect(closestPoint, newPoint, edge2(1:2), edge2(3:4)) || ...
|
|
doIntersect(closestPoint, newPoint, edge3(1:2), edge3(3:4))
|
|
crossesObstacle = true;
|
|
break;
|
|
end
|
|
end
|
|
|
|
% Return the result
|
|
intersect = crossesObstacle;
|
|
endfunction
|
|
|
|
function intersects = doIntersect(p1, q1, p2, q2)
|
|
% Function to check if two line segments (p1, q1) and (p2, q2) intersect
|
|
|
|
if (p1 == q1) || (p2 == q2)
|
|
intersects = false; % Degenerate cases, no intersection
|
|
return;
|
|
end
|
|
|
|
% Check if the line segments are not collinear
|
|
if orientation(p1, q1, p2) ~= orientation(p1, q1, q2) && ...
|
|
orientation(p2, q2, p1) ~= orientation(p2, q2, q1)
|
|
intersects = true;
|
|
return;
|
|
end
|
|
|
|
intersects = false; % No intersection
|
|
end
|
|
|
|
|
|
function o = orientation(p, q, r)
|
|
% Function to find the orientation of triplet (p, q, r)
|
|
% Returns:
|
|
% 0 -> Collinear points
|
|
% 1 -> Clockwise points
|
|
% 2 -> Counterclockwise points
|
|
|
|
val = (q(2) - p(2)) * (r(1) - q(1)) - (q(1) - p(1)) * (r(2) - q(2));
|
|
|
|
if val == 0
|
|
o = 0; % Collinear
|
|
elseif val > 0
|
|
o = 1; % Clockwise
|
|
else
|
|
o = 2; % Counterclockwise
|
|
end
|
|
end
|