# closestProjections

Find orthogonal projections between path tangent vector and query point

Since R2022a

## Description

[arclengths,distances] = closestProjections(refPath,points) attempts to project each xy point in the points matrix, onto each clothoid segment contained in the reference path, refPath, such that the projection vector is orthogonal to the path tangent-angle. Returns closest orthogonal projection between the curve and query point in each segment as a pair of two cell arrays, arclengths and distances containing the arclengths and distances respectively.

example

[___,projPoints] = closestProjections(refPath,points) optionally returns the projected points, projPoints as a cell array containing path data evaluated at the corresponding arclengths element.

[___] = closestProjections(refPath,points,bestN) returns the nearest projections, bestNfor each xy point in points.

[___] = closestProjections(refPath,points,intervals) accepts an optional matrix of arclengths intervals, intervals, where each row contains a lower and upper arclength bounds.

[___] = closestProjections(refPath,points,intervals,bestN) returns up to the nearest projections bestN for each xy point in points.

## Examples

collapse all

Create a reference path with multiple intersections.

leftSideAngles  = [linspace(-pi/6,pi/6,4) linspace(pi/6,-pi/6,4)]';
rightSideAngles = [linspace(-pi/6,pi/6,4) linspace(-pi/6,pi/6,4)]';
waypoints = zeros(numel(leftSideAngles)*2,3);
width = 10;
height = 20;
waypoints(1:2:end,:) = [zeros(numel(leftSideAngles),1) linspace(0,height,numel(leftSideAngles))' leftSideAngles]; ...
waypoints(2:2:end,:) = [width*ones(numel(leftSideAngles),1) linspace(0,height,numel(leftSideAngles))' rightSideAngles];
refPath = referencePathFrenet(waypoints);

Create a set of random XY points around the path.

queryPoints = [width height]/2+(rand(10,2)-.5).*[width height]*1.5;

Retrieve the nearest valid projection of each query point on each segment in the path.

[allArclenth,allDistance,allProjection] = closestProjections(refPath,queryPoints);
pLength = refPath.PathLength;
breaks = [refPath.SegmentParameters(:,end); pLength];
allInterval = [breaks(1:end) [breaks(2:end); pLength]];

Return the three best projections.

maxResult = 3;
[best3Arclength,best3Distance,best3Projection] = closestProjections(refPath,queryPoints,maxResult);

Define a custom set of arclength-intervals.

everyThreeMerged = [breaks(1:3:end-1) [breaks(4:3:end-1); breaks(end)]];

Find the best projection of each query-point onto each custom interval that exists.

[allArclengthCustom,allDistanceCustom,allProjectionCustom] = closestProjections(refPath,queryPoints,everyThreeMerged);

Return the single best projection in the first and last quarter of the path.

endQuarterIntervals = [0 1/4; 3/4 1]*refPath.PathLength;
[bestQuarterArclength,bestQuarterDistance,bestQuarterProjection] = closestProjections(refPath,queryPoints,endQuarterIntervals, 1);

Display the results.

% Pack iterable containers
intervalSets = {allInterval, allInterval, everyThreeMerged, endQuarterIntervals};
S = {allArclenth best3Arclength allArclengthCustom bestQuarterArclength};
D = {allDistance best3Distance allDistanceCustom bestQuarterDistance};
PP = {allProjection best3Projection allProjectionCustom bestQuarterProjection};
titles = ["All Projections","Best 3, All Segments","Best In Merged Segments","First vs Last Quarter"];
cOrder = colororder;

Define the helper functions.

mergeFcn = @(v1,v2)reshape([v1 v2 nan(size(v1,1),size(v2,2))]',[],1);
plotFcn = @(L1,L2,linespec)plot(mergeFcn(L1(:,1),L2(:,1:min(1,size(L2,2)))),mergeFcn(L1(:,2),L2(:,2:min(2,size(L2,2)))),linespec{:});
intervalPlotter = @(bounds,nPt,linespec)plotFcn(interpolate(refPath,linspace(bounds(1),bounds(2),nPt)'),[],linespec);

Create in-loop handles.

setupFcns = {};
setupFcns{1} = @(figIdx)hold(show(refPath,"Parent",subplot(2,2,figIdx)),"on");
setupFcns{2} = @(figIdx)axis(subplot(2,2,figIdx),"equal");
setupFcns{3} = @(figIdx)title(subplot(2,2,figIdx),titles(figIdx));
setupFcns{4} = @(figIdx)plotFcn(queryPoints,[],{"Xk","MarkerSize",5});
setupFcns{5} = @(figIdx)arrayfun(@(i)intervalPlotter(intervalSets{figIdx}(i,:),100,{"Color",cOrder(mod(i,size(cOrder,1)-1)+1,:),"LineWidth",2'}),1:size(intervalSets{figIdx},1));
setupFcns{6} = @(figIdx)cellfun(@(p,projPts)plotFcn(repmat(queryPoints(p,:),size(projPts,1),1),projPts,{"Color",cOrder(mod(p,size(cOrder,1)-1)+1,:)}),num2cell(1:size(queryPoints,1))',PP{figIdx});

Display the results.

arrayfun(@(idx)cellfun(@(f)f(idx),setupFcns),1:4)

## Input Arguments

collapse all

Reference path, specified as a referencePathFrenet object.

Global points, specified as a P-by-2 numeric matrix with rows of the form [x y]. P is the number of points. Positions are in meters.

Best N projections, specified as a scalar in the range [1,N], where N is the number of segments in the path.

Arclength intervals, specified as a N-by-2 matrix, where each row is of the form [minimum_arclength, maximum_arclength] in meters, and N is the number of segments in the path.

## Output Arguments

collapse all

Arclengths between curve and query points, returned as an M-element cell array, where M is the number of query points in the points input. Each cell contains a P-element column vector, where P is in the range [0,N] and N is the number of segments in the path.

Distances between curve and query points, returned as an M-element cell array, where M is the number of query points in the points input. Each cell contains a P-element column vector, where P is in the range [0,N] and N is the number of segments in the path.

Projected points, returned as an M-element cell array, where M is the number of query points in the points input. Each cell contains a P-by-6 matrix, where P is in the range [0,N] and each row is in the form [x y theta k dk s]. x, y, theta, k, dk, s, are the x and y positions, tangent angle, curvature, change in curvature, at the arclength, s, respectively.

## Version History

Introduced in R2022a