Skip to content

Commit be54b80

Browse files
gnestlingergnestlinger
committed
Create path via frenet-offset from existing path.
1 parent a8fe6e2 commit be54b80

File tree

4 files changed

+149
-26
lines changed

4 files changed

+149
-26
lines changed

‎Path2D.m‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
% Path2D methods (modify path object):
1414
% append - Append paths.
1515
% derivative - Derivative of path.
16+
% frenetOffset - Instantiate PolygonPath from given path and offsets.
1617
% restrict - Restrict path domain.
1718
% reverse - Reverse path.
1819
% rotate - Rotate path.
@@ -90,6 +91,43 @@
9091
s = obj.ArcLengths;
9192
end%fcn
9293

94+
function [objNew,tau] = frenetOffset(obj, sd)
95+
%FRENETOFFSET PolygonPath from path object and offsets.
96+
% POBJ = FRENETOFFSET(OBJ, SD) returns a PolygonPath POBJ from
97+
% frenet offsets D applied at lengths S, specified as an array of
98+
% size N-by-2, to the path OBJ.
99+
%
100+
% POBJ = FRENETOFFSET(OBJ, D) applies the frenet offsets D, of
101+
% size N-by-1, evenly distributed along the domain of OBJ.
102+
%
103+
% See also PolygonPath.
104+
105+
% We instantiate the new path via xy2Path(), which requires at
106+
% least two samples.
107+
[tau0,tau1] = obj.domain();
108+
assert(tau1 > tau0, 'Domain must be non-singular!');
109+
110+
N = size(sd, 1);
111+
if size(sd,2) > 1
112+
tau = obj.s2tau(sd(:,1));
113+
d = sd(:,2);
114+
else % Only normal component d of frenet coordinates is given
115+
tau = linspace(tau0, tau1, N)';
116+
d = sd(:,1);
117+
end
118+
119+
assert(numel(d) == numel(tau))
120+
[x,y,~,h] = obj.eval(tau);
121+
122+
% Instantiate PolygonPath object
123+
if N > 1
124+
objNew = PolygonPath.xy2Path(x - d.*sin(h), y + d.*cos(h));
125+
else
126+
objNew = PolygonPath(x - d.*sin(h), y + d.*cos(h), h, zeros(N,1));
127+
end
128+
129+
end%fcn
130+
93131
function flag = isempty(obj)
94132
% ISEMPTY Check if path is empty.
95133
% FLAG = ISEMPTY(OBJ) returns true if the path's domain is

‎PolygonPath.m‎

Lines changed: 72 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
% PolygonPath methods:
1616
% PolygonPath - Constructor.
1717
% discreteFrechetDist - Discrete Frechet distance.
18+
% findSelfIntersections - Find intersecting path segments.
1819
% fitCircle - Fit circle to path.
1920
% fitStraight - Fit straight line to path.
2021
% interp - Interpolate path.
@@ -293,6 +294,46 @@
293294

294295
end%fcn
295296

297+
function [idxs,taus] = findSelfIntersections(obj, doPlot)
298+
%FINDSELFINTERSECTIONS Find intersecting path segments - Experimental!
299+
% IDXS = FINDSELFINTERSECTIONS(OBJ) returns an array of size
300+
% N-by-2 of indexes IDXS, where each row reports an intersection
301+
% between two segments.
302+
%
303+
% Note: If a segment I has intersections with segments J1 < J2 <
304+
% J3 < ..., only the first intersection [I J1] is returned!
305+
306+
N = obj.numel();
307+
idxs = zeros(0,2);
308+
taus = zeros(0,2);
309+
for i = 1:N-1
310+
P0 = [obj.x(i) obj.y(i)];
311+
P1 = [obj.x(i+1) obj.y(i+1)];
312+
313+
for j = i+1:N
314+
Q0 = [obj.x(j) obj.y(j)];
315+
Q1 = [obj.x(j+1) obj.y(j+1)];
316+
[~,tauIJ] = lineLineIntersection(P0, P1, Q0, Q1);
317+
if all(tauIJ > 0 & tauIJ < 1)
318+
% Segment i has an intersection with segment j
319+
idxs = [idxs; [i j]]; %#ok<AGROW>
320+
taus = [taus; tauIJ(1) + i - 1, tauIJ(2) + j - 1]; %#ok<AGROW>
321+
break
322+
end
323+
end
324+
end
325+
326+
if (nargin > 1) && doPlot
327+
[~,ax] = obj.plot();
328+
npState = get(ax, 'NextPlot');
329+
set(ax, 'NextPlot','add')
330+
[xI,yI] = obj.eval(taus(:,1));
331+
plot(ax, xI, yI, 'o', 'DisplayName','Intersections')
332+
set(ax, 'NextPlot',npState)
333+
end
334+
335+
end%fcn
336+
296337
function [objc,e,xc,yc,R] = fitCircle(obj, N, doPlot)
297338
%FITCIRCLE Fit a circle to PolygonPath.
298339
% [OBJC,E,XC,YC,R] = FITCIRCLE(OBJ,N) fits a circle with
@@ -319,10 +360,11 @@
319360

320361
% Plot if requested
321362
if (nargin > 2) && doPlot
322-
plot(obj, 'b-', 'MarkerSize',10, 'DisplayName','PolygonPath');
323-
hold on
324-
plot(objc, 'Color','k', 'DisplayName','Circle');
325-
hold off
363+
[~,ax] = plot(obj, 'b-', 'MarkerSize',10);
364+
npState = get(ax, 'NextPlot');
365+
set(ax, 'NextPlot','add')
366+
plot(ax, objc, 'Color','k', 'DisplayName','Circle');
367+
set(ax, 'NextPlot',npState)
326368
legend('-DynamicLegend')
327369
end%if
328370

@@ -366,10 +408,11 @@
366408
end
367409

368410
if (nargin > 1) && doPlot % Plot if requested
369-
plot(obj, 'r', 'Marker','.', 'DisplayName','PolygonPath');
370-
hold on
371-
plot(objs, 'b', 'Marker','o', 'DisplayName','Straight');
372-
hold off
411+
[~,ax] = plot(obj, 'r', 'Marker','.');
412+
npState = get(ax, 'NextPlot');
413+
set(ax, 'NextPlot','add');
414+
plot(ax, objs, 'b', 'Marker','o', 'DisplayName','Straight');
415+
set(ax, 'NextPlot',npState);
373416
legend('-DynamicLegend')
374417
end%if
375418

@@ -398,16 +441,17 @@
398441
xy = Q + bsxfun(@times, sd(:,2), [-u(:,2), u(:,1)]);
399442

400443
if (nargin > 2) && doPlot
401-
plot(obj, 'Marker','.', 'MarkerSize',15, 'DisplayName','PolygonPath');
402-
hold on
403-
plot(xy(:,1), xy(:,2), 'o', 'DisplayName','xy');
404-
plot(Q(:,1), Q(:,2), 'kx', 'DisplayName','Q');
405-
hold off
444+
[~,ax] = plot(obj, 'Marker','.', 'MarkerSize',15);
445+
npState = get(ax, 'NextPlot');
446+
set(ax, 'NextPlot','add');
447+
plot(ax, xy(:,1), xy(:,2), 'o', 'DisplayName','xy');
448+
plot(ax, Q(:,1), Q(:,2), 'kx', 'DisplayName','Q');
449+
set(ax, 'NextPlot',npState);
406450
legend('-DynamicLegend')
407451
end%if
408452

409453
end%fcn
410-
454+
411455
function flag = isempty(obj)
412456
% ISEMPTY Check if path is empty.
413457
% FLAG = ISEMPTY(OBJ) returns true if the path contains no
@@ -503,11 +547,12 @@
503547

504548
if (nargin > 3) && doPlot
505549
[~,ax] = plot(obj, 'Marker','.');
506-
hold on
550+
npState = get(ax, 'NextPlot');
551+
set(ax, 'NextPlot','add');
507552
phi = 0:pi/100:2*pi;
508553
plot(ax, r*cos(phi) + C(1), r*sin(phi) + C(2), 'DisplayName','Circle');
509554
plot(ax, xy(:,1), xy(:,2), 'kx', 'DisplayName','Intersections')
510-
hold off
555+
set(ax, 'NextPlot',npState);
511556
end%if
512557

513558
end%fcn
@@ -688,16 +733,17 @@
688733
dphi = zeros(numel(idx), 1);
689734

690735
if (nargin > 3) && doPlot
691-
plot(obj, 'Marker','.', 'MarkerSize',8, 'DisplayName','RefPath');
692-
hold on
693-
plot(obj.x(1), obj.y(1), 'g.', 'MarkerSize',18, 'DisplayName','Initial point');
694-
plot(poi(1), poi(2), 'ro', 'DisplayName','PoI')
695-
plot(Q(:,1), Q(:,2), 'kx', 'DisplayName','Q')
736+
[~,ax] = plot(obj, 'Marker','.', 'MarkerSize',8, 'DisplayName','RefPath');
737+
npState = get(ax, 'NextPlot');
738+
set(ax, 'NextPlot','add');
739+
plot(ax, obj.x(1), obj.y(1), 'g.', 'MarkerSize',18, 'DisplayName','Initial point');
740+
plot(ax, poi(1), poi(2), 'ro', 'DisplayName','PoI')
741+
plot(ax, Q(:,1), Q(:,2), 'kx', 'DisplayName','Q')
696742
legend('-DynamicLegend');
697-
plot(...
743+
plot(ax, ...
698744
[Q(:,1)'; repmat([poi(1) NaN], size(Q,1),1)'],...
699745
[Q(:,2)'; repmat([poi(2) NaN], size(Q,1),1)'], 'k:');
700-
hold off
746+
set(ax, 'NextPlot',npState);
701747
end%if
702748

703749
end%fcn
@@ -932,7 +978,7 @@
932978
P1 = [obj.x(end); obj.y(end)];
933979
end
934980
end%fcn
935-
981+
936982
function write2file(obj, fn)
937983
%WRITE2FILE Write path to file.
938984
% WRITE2FILE(OBJ,FN) writes waypoints OBJ to file with filename
@@ -1014,9 +1060,9 @@ function write2file(obj, fn)
10141060

10151061
function obj = clothoid(L, curv01, N, MODE)
10161062
%CLOTHOID Create clothoid path.
1017-
% OBJ = POLYGONPATH.CLOTHOID(L, PHI01, N) creates a clothoid path
1063+
% OBJ = POLYGONPATH.CLOTHOID(L,CURV01,N) creates a clothoid path
10181064
% of length L, initial curvature CURV01(1) and end curvature
1019-
% CURV01(2) at N equidistant sample points.
1065+
% CURV01(2), at N equidistant sample points.
10201066
%
10211067
% OBJ = POLYGONPATH.CLOTHOID(___,MODE) allows to select different
10221068
% calculation methods. Possible values are 'Heald', 'Quad'.

‎private/lineLineIntersection.m‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
function [POI,tau] = lineLineIntersection(P0, P1, Q0, Q1, doPlot)
2+
%LINELINEINTERSECTION Intersection of two lines.
3+
% POI = LINELINEINTERSECTION(P0,P1,Q0,Q1) returns the point of
4+
% intersection POI of the two lines connecting the points P0/P1 and
5+
% Q0/Q1.
6+
%
7+
% [___,TAU] = LINELINEINTERSECTION(___) returns the path parameters TAU =
8+
% [TAUP; TAUQ] for the corresponding line segments P0/P1 and Q0/Q1.
9+
%
10+
11+
dP = P1(:) - P0(:);
12+
A = [dP Q0(:)-Q1(:)];
13+
14+
if rcond(A) > eps
15+
tau = A\(Q0(:) - P0(:));
16+
POI = P0(:) + dP*tau(1);
17+
else
18+
tau = [NaN; NaN];
19+
POI = [NaN; NaN];
20+
end
21+
22+
23+
if (nargin > 4) && doPlot
24+
ax = gca;
25+
npState = get(ax, 'NextPlot');
26+
plot(ax, [P0(1) P1(1)], [P0(2) P1(2)], 'DisplayName','Line P0-P1')
27+
set(ax, 'NextPlot','add')
28+
plot(ax, [Q0(1) Q1(1)], [Q0(2) Q1(2)], 'DisplayName','Line Q0-Q1')
29+
plot(ax, POI(1), POI(2), 'rx')
30+
set(ax, 'XGrid','on', 'YGrid','on', 'NextPlot',npState);
31+
end%if
32+
33+
end%fcn

‎resources/project/Root.type.Files/private.type.File/lineLineIntersection.m.type.File.xml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<Info>
3+
<Category UUID="FileClassCategory">
4+
<Label UUID="design" />
5+
</Category>
6+
</Info>

0 commit comments

Comments
 (0)