From a6bc57123c9ab433e981501fc7e5979ddb8795ff Mon Sep 17 00:00:00 2001 From: Georg <84226458+georgathome@users.noreply.github.com> Date: Sun, 28 Jul 2024 21:21:49 +0200 Subject: [PATCH 01/16] Initial commit. --- DubinsPath.m | 504 ++++++++++++++++++ private/circleLeft.m | 12 + private/circleRight.m | 12 + private/mod2pi.m | 3 + .../DubinsPath.m.type.File.xml | 6 + .../circleLeft.m.type.File.xml | 6 + .../circleRight.m.type.File.xml | 6 + .../private.type.File/mod2pi.m.type.File.xml | 6 + .../DubinsPath.type.File.xml | 2 + .../1.type.DIR_SIGNIFIER.xml | 2 + .../ConnectTest.m.type.File.xml | 6 + ...-40a5-bd0e-de27774f0431.type.Reference.xml | 2 + xUnitTests/DubinsPath/ConnectTest.m | 174 ++++++ 13 files changed, 741 insertions(+) create mode 100644 DubinsPath.m create mode 100644 private/circleLeft.m create mode 100644 private/circleRight.m create mode 100644 private/mod2pi.m create mode 100644 resources/project/Root.type.Files/DubinsPath.m.type.File.xml create mode 100644 resources/project/Root.type.Files/private.type.File/circleLeft.m.type.File.xml create mode 100644 resources/project/Root.type.Files/private.type.File/circleRight.m.type.File.xml create mode 100644 resources/project/Root.type.Files/private.type.File/mod2pi.m.type.File.xml create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File.xml create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/1.type.DIR_SIGNIFIER.xml create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml create mode 100644 resources/project/Root.type.ProjectPath/ff97ac0a-3bbe-40a5-bd0e-de27774f0431.type.Reference.xml create mode 100644 xUnitTests/DubinsPath/ConnectTest.m diff --git a/DubinsPath.m b/DubinsPath.m new file mode 100644 index 0000000..c1574fc --- /dev/null +++ b/DubinsPath.m @@ -0,0 +1,504 @@ +classdef (InferiorClasses = {?matlab.graphics.axis.Axes}) DubinsPath < Path2D +%DUBINSPATH Dubins path. +% Path representation using circular arcs and straight line segments. +% +% DubinsPath properties: +% TurningRadius - Radius of circular arc segments. +% SegmentLengths - Length of each path segment. +% SegmentTypes - Type of each path segment. +% +% +% DubinsPath methods: +% DubinsPath - Constructor. +% +% DubinsPath static methods: +% connect - Create Dubins path from initial/target configuration. +% See superclasses. +% +% See also PATH2D. + + + + properties (SetAccess = private) + % TurningRadius - Turning radius + TurningRadius = 1 + + % SegmentTypes - Segment types + % 1 ... Left turn + % 0 ... Straight line + % -1 ... Right turn + SegmentTypes = zeros(3,1, 'int8') + + % SegmentLengths - Segment lengths + SegmentLengths = zeros(3,1) + + % InitialPos - Initial position + InitialPos = zeros(2,1) + + % InitialAng - Initial orientation angle + InitialAng = 0 + end + + properties (Access = private) + % ArcLengths - Cumulative length of path segments + Arclengths = zeros(0, 1) + end + + properties (Constant, Hidden) + % AdmissiblePaths - Admissible paths of the dubins set + % The six admissible paths are {LSL, RSR, RSL, LSR, RLR, LRL}, + % where L means turning left, R turning right and S going + % straight. + AdmissiblePaths = int8([... + 1 0 1; + -1 0 -1; + -1 0 1; + 1 0 -1; + -1 1 -1; + 1 -1 1]); + + % MapNum2Char - Segment type map from numeric to character + % + MapType2Char = 'RSL' + + LEFT = int8(1) + STRAIGHT = int8(0) + RIGHT = int8(-1) + end + + + methods + + function obj = DubinsPath(startPose, types, lengths, R) + %DUBINSPATH Create Dubins path object. + % OBJ = DUBINSPATH() creates an empty path. + % + % OBJ = DUBINSPATH(C0, TYPES, LENGTHS, R) + % + + % OBJ = DUBINSPATH(___,ISCIRCUIT) set to true if the path is a + % circuit. + % + + if nargin < 1 + % Return with default values + return + end + + obj.InitialPos = startPose(1:2); + obj.InitialAng = mod2pi(startPose(3)); + + assert(isequal(numel(types), numel(lengths)), ... + 'DubinsPath:Constructor:numelTypesLengths', ... + 'Number of path property elements mismatch!'); + obj.SegmentTypes = types; + obj.SegmentLengths = lengths; + obj.TurningRadius = R; + + obj.Arclengths = [0; cumsum(obj.SegmentLengths)']; + +% if nargin < 5 +% obj = obj.setIsCircuit(1e-5); +% else +% obj.IsCircuit = isCircuit; +% end%if + + end%Constructor + + function obj = append(obj, obj2) + error('Not implemented!') + end%fcn + + function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, ~, doPlot) + error('Not implemented!') + end%fcn + + function c = convertSegmentType2Char(obj) + + c = obj.MapType2Char(obj.SegmentTypes + 2); + end%fcn + + function s = cumlengths(obj) + s = obj.Arclengths; + end%fcn + + function [tauL,tauU] = domain(obj) + + if isempty(obj) + tauL = NaN; + tauU = NaN; + else + tauL = 0; + tauU = obj.numel() - 1; + end + end%fcn + + function [x,y,tau,head,curv,curvDs] = eval(obj, tau, ~) + %EVAL Evaluate path at path parameter. + % + +% if nargin < 3 % Not supported +% extrap = false; +% end + + N = obj.numel(); + + if nargin < 2 + % M samples per L/R segment; 1 sample per S segment; 1 + % additional sample for final segment of any type + M = 100; + types = obj.SegmentTypes; + lengths = obj.SegmentLengths; + Nlr = sum(abs(types(lengths > 0))); + Ns = sum(types == 0 & lengths > 0); + tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); + xyhc = zeros(numel(tau), 4); + + x0 = obj.InitialPos(1); + y0 = obj.InitialPos(2); + h0 = obj.InitialAng; + R = obj.TurningRadius; + tau0 = 0; + + jj = 1; + for i = 1:N + si = lengths(i); + if si < eps + continue + end + + ii = jj; + jj = ii + M; + + typei = types(i); + if typei == obj.LEFT % Left turn + % S = R*PHI -> PHI = S/R + taui = linspace(0, 1, M+1)'; + hi = linspace(h0, h0+si/R, M+1)'; + [xi,yi,ci] = circleLeft(R, hi); + elseif typei == obj.RIGHT % Right turn + taui = linspace(0, 1, M+1)'; + hi = linspace(h0, h0-si/R, M+1)'; + [xi,yi,ci] = circleRight(R, hi); + else % Straight + jj = ii + 1; + taui = [0; 1]; + xi = [0; si/R*cos(h0)]; + yi = [0; si/R*sin(h0)]; + hi = [h0; h0]; + ci = [0; 0]; + end + xi = xi - xi(1) + x0; + yi = yi - yi(1) + y0; + taui = taui + tau0; + x0 = xi(end); + y0 = yi(end); + h0 = hi(end); + tau0 = taui(end); + + xyhc(ii:jj,:) = [xi yi hi ci]; + tau(ii:jj) = taui; +% plot(xi, yi) + end + + else % nargin > 1 + error('ToDo!!!') + end%if + + x = xyhc(:,1); + y = xyhc(:,2); + head = xyhc(:,3); + curv = xyhc(:,4); + curvDs = zeros(numel(tau), 1); + + end%fcn + + function tau = findZeroCurvature(obj, ths) + + if nargin < 2 + ths = eps; + end + error('Not implemented!') + + end%fcn + + function [xy,Q,idx,tau] = frenet2cart(obj, sd, doPlot) + error('Not implemented!') + end%fcn + + function obj = interp(obj, tau, varargin) + %INTERP Interpolate path. + % OBJ = INTERP(OBJ,TAU) interpolate path OBJ w.r.t. path + % parameter TAU. + % + % OBJ = INTERP(__,ARGS) specify interpolation settings via ARGS. + % + % See also INTERP1. + + narginchk(2, 4) % object, query points, method, extrapolation + + error('Not implemented!') + end%fcn + + function [xy,tau,errFlag] = intersectCircle(obj, C, r, doPlot) + error('Not implemented!') + end%fcn + + function [xy,tau,errFlag] = intersectLine(obj, O, psi, doPlot) + error('Not implemented!') + end%fcn + + function flag = isempty(obj) + flag = ~any(obj.SegmentLengths ~= 0); + end%fcn + + function s = length(obj) + s = obj.ArcLengths(end); + end%fcn + + function n = numel(obj) + n = numel(obj.SegmentTypes); + end%fcn + + function [Q,idx,tau,dphi] = pointProjection(obj, poi, ~, doPlot) + + error('Not implemented!') + end%fcn + + function [obj,tau0,tau1] = restrict(obj, tau0, tau1) + + error('Not implemented!') + end%fcn + + function obj = reverse(obj) + + error('Not implemented!') + end%fcn + + function obj = rotate(obj, phi) + + error('Not implemented!') + end%fcn + + function obj = select(obj, idxs) + error('Not implemented!') + end%fcn + + function obj = shift(obj, P) + + error('Not implemented!') + end%fcn + + function [tau,idx,ds] = s2tau(obj, s) + + error('Not implemented!') + end%fcn + + function [P0,P1] = termPoints(obj) + + if isempty(obj) + P0 = [NaN; NaN]; + P1 = [NaN; NaN]; + else + P0 = obj.InitialPos(:); + [x,y] = obj.eval(obj.numel() - 1); + P1 = [x; y]; + end + + end%fcn + + function write2file(obj, fn) + %WRITE2FILE Write path to file. + % WRITE2FILE(OBJ,FN) writes waypoints OBJ to file with filename + % FN (specify extension!). + % + error('Not implemented!') + end%fcn + + function s = toStruct(obj) + error('Not implemented!') + end%fcn + + %%% Set methods + function obj = set.SegmentLengths(obj, val) + obj.SegmentLengths = double(val(:)'); + end%fcn + + function obj = set.SegmentTypes(obj, val) + obj.SegmentTypes = int8(val(:)'); + end%fcn + + function obj = set.TurningRadius(obj, val) + assert(isscalar(val) && isnumeric(val) && val > 0); + obj.TurningRadius = double(val); + end%fcn + end%methods + + + methods (Static) + + function obj = fromStruct(s) + end%fcn + + function c = getBusDef() + end%fcn + + function obj = connect(C0, C1, R) + %CONNECT Dubins path from initial and target configuration. + % OBJ = CONNECT(C0, C1, R) create a Dubins path OBJ with turning + % radius R connecting the initial/end configuration C0/C1, where + % Ci = [Xi; Yi; PHIi]. + % + + narginchk(3, 3) + + % Adjust the problem so that P0 and P1 are on the x-axis a + % distance d apart + dx = C1(1) - C0(1); + dy = C1(2) - C0(2); + d = hypot(dx, dy)/R; + theta = atan2(dy, dx); + phi0 = mod2pi(C0(3)) - theta; + phi1 = mod2pi(C1(3)) - theta; + + T = coder.nullcopy(zeros(3, 6, 'int8')); + L = coder.nullcopy(zeros(3, 6)); + S = coder.nullcopy(zeros(6, 1)); + + [T(:,1),S(1),L(:,1)] = dubinsLRL(d, phi0, phi1); + [T(:,2),S(2),L(:,2)] = dubinsLSL(d, phi0, phi1); + [T(:,3),S(3),L(:,3)] = dubinsLSR(d, phi0, phi1); + [T(:,4),S(4),L(:,4)] = dubinsRLR(d, phi0, phi1); + [T(:,5),S(5),L(:,5)] = dubinsRSL(d, phi0, phi1); + [T(:,6),S(6),L(:,6)] = dubinsRSR(d, phi0, phi1); + + [~,minIdx] = min(S); + assert(sum(L(:, minIdx)) == S(minIdx)) + obj = DubinsPath(C0, T(:, minIdx), L(:, minIdx)*R, R); + + end%fcn + + end%methods + +end%class + + +function [w,s,l] = dubinsLSL(d, a, b) + +w = coder.const(uint8([1;0;1])); +p2 = 2 + d^2 - 2*cos(a-b) + 2*d*(sin(a)-sin(b)); +if p2 < 0 + s = NaN; + l = [0;0;0]; + return +end + +p = sqrt(p2); +tmp = atan2(cos(b)-cos(a), d+sin(a)-sin(b)); +t = mod2pi(tmp - a); +q = mod2pi(b - tmp); +l = [t; p; q]; + +% s = -a + b + p; +s = sum(l); + +end%fcn + +function [w,s,l] = dubinsRSR(d, a, b) + +w = coder.const(int8([-1;0;-1])); +p2 = 2 + d^2 - 2*cos(a-b) + 2*d*(sin(b)-sin(a)); +if p2 < 0 + s = NaN; + l = [0;0;0]; + return +end + +p = sqrt(p2); +tmp = atan2(cos(a)-cos(b), d-sin(a)+sin(b)); +l = [mod2pi(a - tmp); p; mod2pi(tmp - mod2pi(b))]; + +% s = a - b + p; +s = sum(l); + +end%fcn + +function [w,s,l] = dubinsLSR(d, a, b) + +w = coder.const(int8([1;0;-1])); +p2 = -2 + d^2 + 2*cos(a-b) + 2*d*(sin(a)+sin(b)); +if p2 < 0 + s = NaN; + l = [0;0;0]; + return +end + +p = sqrt(p2); +tmp = atan2(-cos(b)-cos(a), d+sin(a)+sin(b)) - atan2(-2, p); +t = mod2pi(tmp - a); +l = [t; p; mod2pi(tmp - mod2pi(b))]; + +% s = a - b + 2*t + p; +s = sum(l); + +end%fcn + +function [w,s,l] = dubinsRSL(d, a, b) + +w = int8([-1;0;1]); +p2 = d^2 - 2 + 2*cos(a-b) - 2*d*(sin(a)+sin(b)); +if p2 < 0 + s = NaN; + l = [0;0;0]; + return +end + +tmp1 = atan2(cos(a)+cos(b), d-sin(a)-sin(b)); +p = sqrt(p2); +tmp2 = atan2(2, p); +t = mod2pi(a - tmp1 + tmp2); +q = mod2pi(mod2pi(b) - tmp1 + tmp2); +l = [t; p; q]; +% s = b - a + 2*t + p; +s = sum(l); + +end%fcn + +function [w,s,l] = dubinsRLR(d, a, b) + +w = int8([-1;1;-1]); +p2 = 0.125*(6 - d^2 + 2*cos(a-b) + 2*d*(sin(a)-sin(b))); +if abs(p2) > 1 % Outside domain of acos() + s = NaN; + l = [0;0;0]; + return +end + +p = mod2pi(2*pi - acos(p2)); +t = mod2pi(a - atan2(cos(a)-cos(b), d-sin(a)+sin(b)) + p/2); +q = mod2pi(a - b - t + p); + +l = [t; p; q]; +% s = a - b + 2*p; +s = sum(l); + +end%fcn + +function [w,s,l] = dubinsLRL(d, a, b) + +w = int8([1;-1;1]); +p2 = 0.125*(6 - d^2 + 2*cos(a-b) + 2*d*(sin(b)-sin(a))); +if abs(p2) > 1 % Outside domain of acos() + s = NaN; + l = [0;0;0]; + return +end + +p = mod2pi(2*pi - acos(p2)); +t = mod2pi(-atan2(cos(a)-cos(b), d+sin(a)-sin(b)) + p/2 - a); +q = mod2pi(mod2pi(b) - a - t + p); +l = [t; p; q]; +% s = b - a + 2*p; +s = sum(l); + +end%fcn diff --git a/private/circleLeft.m b/private/circleLeft.m new file mode 100644 index 0000000..48f1fd2 --- /dev/null +++ b/private/circleLeft.m @@ -0,0 +1,12 @@ +function [x,y,c] = circleLeft(r, head) +%UNTITLED Summary of this function goes here +% Detailed explanation goes here + +% We can avoid calculating phi = head - pi/2 by using the identities +% cos(x - pi/2) = sin(x) and +% sin(x - pi/2) = -cos(x) +x = r*sin(head); +y = r*-cos(head); +c = 1/r*ones(size(x)); + +end%fcn diff --git a/private/circleRight.m b/private/circleRight.m new file mode 100644 index 0000000..b55263f --- /dev/null +++ b/private/circleRight.m @@ -0,0 +1,12 @@ +function [x,y,c] = circleRight(r, head) +%UNTITLED Summary of this function goes here +% Detailed explanation goes here + +% We can avoid calculating phi = head + pi/2 by using the identities +% cos(x + pi/2) = -sin(x) and +% sin(x + pi/2) = cos(x) +x = r*-sin(head); +y = r*cos(head); +c = -1/r*ones(size(x)); + +end%fcn diff --git a/private/mod2pi.m b/private/mod2pi.m new file mode 100644 index 0000000..c8615c3 --- /dev/null +++ b/private/mod2pi.m @@ -0,0 +1,3 @@ +function x = mod2pi(x) +x = mod(x, 2*pi); +end%fcn diff --git a/resources/project/Root.type.Files/DubinsPath.m.type.File.xml b/resources/project/Root.type.Files/DubinsPath.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/DubinsPath.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/private.type.File/circleLeft.m.type.File.xml b/resources/project/Root.type.Files/private.type.File/circleLeft.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/private.type.File/circleLeft.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/private.type.File/circleRight.m.type.File.xml b/resources/project/Root.type.Files/private.type.File/circleRight.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/private.type.File/circleRight.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/private.type.File/mod2pi.m.type.File.xml b/resources/project/Root.type.Files/private.type.File/mod2pi.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/private.type.File/mod2pi.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/1.type.DIR_SIGNIFIER.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml new file mode 100644 index 0000000..d8fadf3 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.ProjectPath/ff97ac0a-3bbe-40a5-bd0e-de27774f0431.type.Reference.xml b/resources/project/Root.type.ProjectPath/ff97ac0a-3bbe-40a5-bd0e-de27774f0431.type.Reference.xml new file mode 100644 index 0000000..c154133 --- /dev/null +++ b/resources/project/Root.type.ProjectPath/ff97ac0a-3bbe-40a5-bd0e-de27774f0431.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/xUnitTests/DubinsPath/ConnectTest.m b/xUnitTests/DubinsPath/ConnectTest.m new file mode 100644 index 0000000..354e05c --- /dev/null +++ b/xUnitTests/DubinsPath/ConnectTest.m @@ -0,0 +1,174 @@ +classdef ConnectTest < matlab.unittest.TestCase + + properties (TestParameter) + ConfigsRight = {... + {[0 0 0], [0 -2 -pi], 1}; + {[1 1 pi/2], [3 1 -pi/2], 1}; + {[1 1 pi/2], [5 1 -pi/2], 2}; + }; + + ConfigsLeft = {... + {[0 0 0], [0 2 pi], 1}; + {[3 1 pi/2], [1 1 3*pi/2], 1}; + {[3 1 pi/2], [-1 1 3*pi/2], 2}; + }; + + ConfigsXSY = {... + {[2 3 pi/4], [-2 3 -pi/4], 1, 'LSL'}; + {[2 3 pi/4], [-2 3 +pi/4], 1, 'LSR'}; + {[-2 3 pi/4], [2 3 +pi/4], 1, 'RSL'}; + {[-2 3 pi/4], [2 3 -pi/4], 1, 'RSR'}; + }; + + ConfigsLRL = {... + {[1 2 pi/2], [5 2 -pi/2], 3}} + + ConfigsRLR = {... + {[5 2 pi/2], [1 2 -pi/2], 3}} + end + + methods (Test, ParameterCombination='sequential') + function testRightTurn(testCase, ConfigsRight) + + C0 = ConfigsRight{1}; + C1 = ConfigsRight{2}; + R = ConfigsRight{3}; + dub = DubinsPath.connect(C0, C1, R); + [x,y,~,h] = dub.eval(); + + +% checkAgainstMatlabImpl(dub, C0, C1, R); + + % Check terminal points + testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(end) y(end) h(end)], C1, 'AbsTol',1e-15); + + % Check segment types/lengths + testCase.verifyEqual(dub.SegmentTypes, [dub.LEFT dub.RIGHT dub.LEFT]); + S = abs(C1(end)-C0(end))*R; + testCase.verifyEqual(dub.SegmentLengths, [0 S 0]); + + testCase.verifyFalse(dub.IsCircuit) + end%fcn + + function testLeftTurn(testCase, ConfigsLeft) + + C0 = ConfigsLeft{1}; + C1 = ConfigsLeft{2}; + R = ConfigsLeft{3}; + dub = DubinsPath.connect(C0, C1, R); + [x,y,~,h] = dub.eval(); + + % Check against Matlab implementation +% checkAgainstMatlabImpl(dub, C0, C1, R); + + % Check terminal points + testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(end) y(end) h(end)], C1, 'AbsTol',1e-15); + + % Check segment types/lengths + testCase.verifyEqual(dub.SegmentTypes, [dub.LEFT dub.RIGHT dub.LEFT]); + S = abs(C1(end)-C0(end))*R; + testCase.verifyEqual(dub.SegmentLengths, [S/2 0 S/2]); + + testCase.verifyFalse(dub.IsCircuit) + end%fcn + + function testXSY(testCase, ConfigsXSY) + + C0 = ConfigsXSY{1}; + C1 = ConfigsXSY{2}; + R = ConfigsXSY{3}; + T = ConfigsXSY{4}; + dub = DubinsPath.connect(C0, C1, R); + [x,y,~,h] = dub.eval(); + + % Check against Matlab implementation +% ml = checkAgainstMatlabImpl(dub, C0, C1, R); + + % Check terminal points + testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); + testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); + + % Check segment types/lengths + testCase.verifyEqual(dub.convertSegmentType2Char(), T); +% testCase.verifyEqual(dub.SegmentLengths, [0 0 0]); + + testCase.verifyFalse(dub.IsCircuit) + end%fcn + + function testLRL(testCase, ConfigsLRL) + + C0 = ConfigsLRL{1}; + C1 = ConfigsLRL{2}; + R = ConfigsLRL{3}; + dub = DubinsPath.connect(C0, C1, R); + [x,y,~,h] = dub.eval(); + + % Check against Matlab implementation +% ml = checkAgainstMatlabImpl(dub, C0, C1, R); + + % Check terminal points + testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); + testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); + + % Check segment types/lengths + testCase.verifyEqual(dub.convertSegmentType2Char(), 'LRL'); + + testCase.verifyFalse(dub.IsCircuit) + end%fcn + + function testRLR(testCase, ConfigsRLR) + + C0 = ConfigsRLR{1}; + C1 = ConfigsRLR{2}; + R = ConfigsRLR{3}; + dub = DubinsPath.connect(C0, C1, R); + [x,y,~,h] = dub.eval(); + + % Check against Matlab implementation +% ml = checkAgainstMatlabImpl(dub, C0, C1, R); + + % Check terminal points + testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); + testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); + + % Check segment types/lengths + testCase.verifyEqual(dub.convertSegmentType2Char(), 'RLR'); + + testCase.verifyFalse(dub.IsCircuit) + end%fcn + + end + +end%class + + +function ds = checkAgainstMatlabImpl(dub, C0, C1, R) +% Check against Matlab implementation +% Requires R2019b! + +dc = dubinsConnection('MinTurningRadius',R); +ds = dc.connect(C0, C1); +ds = ds{1}; +ds.show(); + +if ~isempty(dub) + hold on + dub.plot('MarkerIndices',1, 'Marker','o') + hold off +end + +end%fcn + +function val = mod2Pi(val) +val = mod(val, 2*pi); +end%fcn From aca516966f5e86354201d6c9594209fee52bac22 Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Tue, 30 Jul 2024 20:59:34 +0200 Subject: [PATCH 02/16] Dubins path: fixes; methods rotate() and shift() implemented; tests added. --- DubinsPath.m | 110 +++++++++++------- ....xml => ConnectTestDubins.m.type.File.xml} | 0 xUnitTests/ConstructorEmptyTest.m | 5 +- .../{ConnectTest.m => ConnectTestDubins.m} | 24 ++-- xUnitTests/IsEmptyTest.m | 14 ++- xUnitTests/RotateTest.m | 4 +- xUnitTests/ShiftTest.m | 5 +- 7 files changed, 102 insertions(+), 60 deletions(-) rename resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/{ConnectTest.m.type.File.xml => ConnectTestDubins.m.type.File.xml} (100%) rename xUnitTests/DubinsPath/{ConnectTest.m => ConnectTestDubins.m} (85%) diff --git a/DubinsPath.m b/DubinsPath.m index c1574fc..e6160b4 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -10,6 +10,7 @@ % % DubinsPath methods: % DubinsPath - Constructor. +% convertSegmentType2Char - ... % % DubinsPath static methods: % connect - Create Dubins path from initial/target configuration. @@ -21,16 +22,17 @@ properties (SetAccess = private) % TurningRadius - Turning radius + % The radius of circular arc path segments. TurningRadius = 1 % SegmentTypes - Segment types % 1 ... Left turn % 0 ... Straight line % -1 ... Right turn - SegmentTypes = zeros(3,1, 'int8') + SegmentTypes = zeros(1,0, 'int8') % SegmentLengths - Segment lengths - SegmentLengths = zeros(3,1) + SegmentLengths = zeros(1,0) % InitialPos - Initial position InitialPos = zeros(2,1) @@ -41,7 +43,7 @@ properties (Access = private) % ArcLengths - Cumulative length of path segments - Arclengths = zeros(0, 1) + ArcLengths = zeros(0, 1) end properties (Constant, Hidden) @@ -55,7 +57,7 @@ -1 0 1; 1 0 -1; -1 1 -1; - 1 -1 1]); + 1 -1 1]); % MapNum2Char - Segment type map from numeric to character % @@ -69,11 +71,13 @@ methods - function obj = DubinsPath(startPose, types, lengths, R) + function obj = DubinsPath(startPose, types, lengths, R, isCircuit) %DUBINSPATH Create Dubins path object. % OBJ = DUBINSPATH() creates an empty path. % - % OBJ = DUBINSPATH(C0, TYPES, LENGTHS, R) + % OBJ = DUBINSPATH(C0, T, L, R) creates a Dubins-like path with + % initial pose C0, consisting of path segment types T with + % individual lengths L. Arc segments have a radius R. % % OBJ = DUBINSPATH(___,ISCIRCUIT) set to true if the path is a @@ -85,8 +89,9 @@ return end - obj.InitialPos = startPose(1:2); - obj.InitialAng = mod2pi(startPose(3)); + P0 = startPose(1:2); + obj.InitialPos = P0(:); + obj.InitialAng = startPose(3); assert(isequal(numel(types), numel(lengths)), ... 'DubinsPath:Constructor:numelTypesLengths', ... @@ -95,13 +100,13 @@ obj.SegmentLengths = lengths; obj.TurningRadius = R; - obj.Arclengths = [0; cumsum(obj.SegmentLengths)']; + obj.ArcLengths = [0; cumsum(obj.SegmentLengths)']; -% if nargin < 5 -% obj = obj.setIsCircuit(1e-5); -% else -% obj.IsCircuit = isCircuit; -% end%if + % if nargin < 5 + % obj = obj.setIsCircuit(1e-5); + % else + % obj.IsCircuit = isCircuit; + % end%if end%Constructor @@ -114,22 +119,25 @@ end%fcn function c = convertSegmentType2Char(obj) + %CONVERTSEGMENTTYPE2CHAR Convert segment type to character + % C = CONVERTSEGMENTTYPE2CHAR(OBJ) converts numeric property + % SegmentTypes to character representation. c = obj.MapType2Char(obj.SegmentTypes + 2); end%fcn function s = cumlengths(obj) - s = obj.Arclengths; + s = obj.ArcLengths; end%fcn function [tauL,tauU] = domain(obj) - + if isempty(obj) tauL = NaN; tauU = NaN; else tauL = 0; - tauU = obj.numel() - 1; + tauU = obj.numel(); end end%fcn @@ -144,6 +152,16 @@ N = obj.numel(); if nargin < 2 + if obj.isempty() + x = zeros(0,1); + y = zeros(0,1); + tau = zeros(0,1); + head = zeros(0,1); + curv = zeros(0,1); + curvDs = zeros(0,1); + return + end + % M samples per L/R segment; 1 sample per S segment; 1 % additional sample for final segment of any type M = 100; @@ -183,8 +201,8 @@ else % Straight jj = ii + 1; taui = [0; 1]; - xi = [0; si/R*cos(h0)]; - yi = [0; si/R*sin(h0)]; + xi = [0; si*cos(h0)]; + yi = [0; si*sin(h0)]; hi = [h0; h0]; ci = [0; 0]; end @@ -226,20 +244,6 @@ error('Not implemented!') end%fcn - function obj = interp(obj, tau, varargin) - %INTERP Interpolate path. - % OBJ = INTERP(OBJ,TAU) interpolate path OBJ w.r.t. path - % parameter TAU. - % - % OBJ = INTERP(__,ARGS) specify interpolation settings via ARGS. - % - % See also INTERP1. - - narginchk(2, 4) % object, query points, method, extrapolation - - error('Not implemented!') - end%fcn - function [xy,tau,errFlag] = intersectCircle(obj, C, r, doPlot) error('Not implemented!') end%fcn @@ -248,10 +252,6 @@ error('Not implemented!') end%fcn - function flag = isempty(obj) - flag = ~any(obj.SegmentLengths ~= 0); - end%fcn - function s = length(obj) s = obj.ArcLengths(end); end%fcn @@ -261,23 +261,29 @@ end%fcn function [Q,idx,tau,dphi] = pointProjection(obj, poi, ~, doPlot) - error('Not implemented!') end%fcn function [obj,tau0,tau1] = restrict(obj, tau0, tau1) - error('Not implemented!') end%fcn function obj = reverse(obj) - error('Not implemented!') end%fcn function obj = rotate(obj, phi) - error('Not implemented!') + if nargin < 2 + phi = -obj.InitialAng; + end%if + + R = rotmat2D(phi); + for i = 1:builtin('numel', obj) + obj(i).InitialPos = R*obj(i).InitialPos; + obj(i).InitialAng = obj(i).InitialAng + phi; + end%for + end%fcn function obj = select(obj, idxs) @@ -286,7 +292,18 @@ function obj = shift(obj, P) - error('Not implemented!') + % Handle input arguments + narginchk(1, 2); + + if nargin < 2 + P = -obj(1).termPoints(); + end%if + + % BUILTIN is supported for code-generation starting with R2017b + for i = 1:builtin('numel', obj) + obj(i).InitialPos = obj(i).InitialPos + P; + end%for + end%fcn function [tau,idx,ds] = s2tau(obj, s) @@ -320,6 +337,11 @@ function write2file(obj, fn) end%fcn %%% Set methods + function obj = set.InitialAng(obj, val) + assert(isscalar(val) && isnumeric(val)); + obj.InitialAng = mod2pi(val); + end%fcn + function obj = set.SegmentLengths(obj, val) obj.SegmentLengths = double(val(:)'); end%fcn @@ -332,6 +354,7 @@ function write2file(obj, fn) assert(isscalar(val) && isnumeric(val) && val > 0); obj.TurningRadius = double(val); end%fcn + end%methods @@ -361,10 +384,10 @@ function write2file(obj, fn) phi0 = mod2pi(C0(3)) - theta; phi1 = mod2pi(C1(3)) - theta; + % Calculate all admissible paths T = coder.nullcopy(zeros(3, 6, 'int8')); L = coder.nullcopy(zeros(3, 6)); S = coder.nullcopy(zeros(6, 1)); - [T(:,1),S(1),L(:,1)] = dubinsLRL(d, phi0, phi1); [T(:,2),S(2),L(:,2)] = dubinsLSL(d, phi0, phi1); [T(:,3),S(3),L(:,3)] = dubinsLSR(d, phi0, phi1); @@ -372,6 +395,7 @@ function write2file(obj, fn) [T(:,5),S(5),L(:,5)] = dubinsRSL(d, phi0, phi1); [T(:,6),S(6),L(:,6)] = dubinsRSR(d, phi0, phi1); + % Find the shortest one [~,minIdx] = min(S); assert(sum(L(:, minIdx)) == S(minIdx)) obj = DubinsPath(C0, T(:, minIdx), L(:, minIdx)*R, R); diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTestDubins.m.type.File.xml similarity index 100% rename from resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTest.m.type.File.xml rename to resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTestDubins.m.type.File.xml diff --git a/xUnitTests/ConstructorEmptyTest.m b/xUnitTests/ConstructorEmptyTest.m index f342800..b7102fd 100644 --- a/xUnitTests/ConstructorEmptyTest.m +++ b/xUnitTests/ConstructorEmptyTest.m @@ -3,12 +3,13 @@ properties (TestParameter) obj = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()); + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()); end methods (Test) - + function testConstructorInputLength(testCase, obj) testCase.verifyEqual(obj.numel(), 0) end%fcn diff --git a/xUnitTests/DubinsPath/ConnectTest.m b/xUnitTests/DubinsPath/ConnectTestDubins.m similarity index 85% rename from xUnitTests/DubinsPath/ConnectTest.m rename to xUnitTests/DubinsPath/ConnectTestDubins.m index 354e05c..379dc4d 100644 --- a/xUnitTests/DubinsPath/ConnectTest.m +++ b/xUnitTests/DubinsPath/ConnectTestDubins.m @@ -1,4 +1,4 @@ -classdef ConnectTest < matlab.unittest.TestCase +classdef ConnectTestDubins < matlab.unittest.TestCase properties (TestParameter) ConfigsRight = {... @@ -14,7 +14,7 @@ }; ConfigsXSY = {... - {[2 3 pi/4], [-2 3 -pi/4], 1, 'LSL'}; + {[2 3 pi/4], [-2 3 -pi/4], 2, 'LSL'}; {[2 3 pi/4], [-2 3 +pi/4], 1, 'LSR'}; {[-2 3 pi/4], [2 3 +pi/4], 1, 'RSL'}; {[-2 3 pi/4], [2 3 -pi/4], 1, 'RSR'}; @@ -34,13 +34,15 @@ function testRightTurn(testCase, ConfigsRight) C1 = ConfigsRight{2}; R = ConfigsRight{3}; dub = DubinsPath.connect(C0, C1, R); +% [tau0,tau1] = dub.domain(); +% [x,y,~,h] = dub.eval([tau0 tau1]); [x,y,~,h] = dub.eval(); % checkAgainstMatlabImpl(dub, C0, C1, R); % Check terminal points - testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) testCase.verifyEqual([x(1) y(1) h(1)], C0); testCase.verifyEqual([x(end) y(end) h(end)], C1, 'AbsTol',1e-15); @@ -58,13 +60,15 @@ function testLeftTurn(testCase, ConfigsLeft) C1 = ConfigsLeft{2}; R = ConfigsLeft{3}; dub = DubinsPath.connect(C0, C1, R); +% [tau0,tau1] = dub.domain(); +% [x,y,~,h] = dub.eval([tau0 tau1]); [x,y,~,h] = dub.eval(); % Check against Matlab implementation % checkAgainstMatlabImpl(dub, C0, C1, R); % Check terminal points - testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) testCase.verifyEqual([x(1) y(1) h(1)], C0); testCase.verifyEqual([x(end) y(end) h(end)], C1, 'AbsTol',1e-15); @@ -83,13 +87,15 @@ function testXSY(testCase, ConfigsXSY) R = ConfigsXSY{3}; T = ConfigsXSY{4}; dub = DubinsPath.connect(C0, C1, R); +% [tau0,tau1] = dub.domain(); +% [x,y,~,h] = dub.eval([tau0 tau1]); [x,y,~,h] = dub.eval(); % Check against Matlab implementation % ml = checkAgainstMatlabImpl(dub, C0, C1, R); % Check terminal points - testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) testCase.verifyEqual([x(1) y(1) h(1)], C0); testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); @@ -107,13 +113,15 @@ function testLRL(testCase, ConfigsLRL) C1 = ConfigsLRL{2}; R = ConfigsLRL{3}; dub = DubinsPath.connect(C0, C1, R); +% [tau0,tau1] = dub.domain(); +% [x,y,~,h] = dub.eval([tau0 tau1]); [x,y,~,h] = dub.eval(); % Check against Matlab implementation % ml = checkAgainstMatlabImpl(dub, C0, C1, R); % Check terminal points - testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) testCase.verifyEqual([x(1) y(1) h(1)], C0); testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); @@ -130,13 +138,15 @@ function testRLR(testCase, ConfigsRLR) C1 = ConfigsRLR{2}; R = ConfigsRLR{3}; dub = DubinsPath.connect(C0, C1, R); +% [tau0,tau1] = dub.domain(); +% [x,y,~,h] = dub.eval([tau0 tau1]); [x,y,~,h] = dub.eval(); % Check against Matlab implementation % ml = checkAgainstMatlabImpl(dub, C0, C1, R); % Check terminal points - testCase.verifyEqual([dub.InitialPos dub.InitialAng], C0) + testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) testCase.verifyEqual([x(1) y(1) h(1)], C0); testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); diff --git a/xUnitTests/IsEmptyTest.m b/xUnitTests/IsEmptyTest.m index 4ef2ccb..170f637 100644 --- a/xUnitTests/IsEmptyTest.m +++ b/xUnitTests/IsEmptyTest.m @@ -3,24 +3,26 @@ properties (TestParameter) objEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()) - + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) + objNonEmpty = struct(... 'PolygonPath',PolygonPath(1, 2, 3, 4, 5), ... - 'SplinePath', SplinePath([0 1], zeros(2,1,2))) + 'SplinePath', SplinePath([0 1], zeros(2,1,2)), ... + 'DubinsPath', DubinsPath([0 0 0], 0, 1, 3)) end methods (Test) - + function testIsEmptyTrue(testCase, objEmpty) testCase.verifyTrue(objEmpty.isempty()) end%fcn - + function testIsEmptyFalse(testCase, objNonEmpty) testCase.verifyFalse(objNonEmpty.isempty()) end%fcn - + end end%class diff --git a/xUnitTests/RotateTest.m b/xUnitTests/RotateTest.m index 0de6d7a..09d927d 100644 --- a/xUnitTests/RotateTest.m +++ b/xUnitTests/RotateTest.m @@ -5,7 +5,9 @@ 'PolygonPathEmpty', PolygonPath(), ... 'PolygonPathNonEmpty', PolygonPath.xy2Path(1:10, repmat(2, [10,1])), ... 'SplinePathEmpty', SplinePath(), ... - 'SplinePathNonEmpty', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2))); + 'SplinePathNonEmpty', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2)), ... + 'DubinsPathEmpty', DubinsPath(), ...) + 'DubinsPathNonEmpty', DubinsPath([-1 1 pi/2], [1 -1 1], [1.4455 9.1741 1.4455], 2)); end diff --git a/xUnitTests/ShiftTest.m b/xUnitTests/ShiftTest.m index 0eb820e..66145d9 100644 --- a/xUnitTests/ShiftTest.m +++ b/xUnitTests/ShiftTest.m @@ -6,7 +6,10 @@ 'PolygonPathOneElm', PolygonPath(1, 2, pi/4, 0), ... 'PolygonPathNonEmpty', PolygonPath.xy2Path(10:-1:0, zeros(1,11)), ... 'SplinePathEmpty', SplinePath(), ... - 'SplinePathOneElm', SplinePath.pp2Path(mkpp([-1 2], [0 1 -1; 1 0 2], 2))); + 'SplinePathOneElm', SplinePath.pp2Path(mkpp([-1 2], [0 1 -1; 1 0 2], 2)), ... + 'DubinsPathEmpty', DubinsPath(), ...) + 'DubinsPathOneElm',DubinsPath([-1 1 pi/2], 1, 2, 2), ... + 'DubinsPathNonEmpty', DubinsPath([-1 1 pi/2], [1 -1 1], [1.4455 9.1741 1.4455], 2)); dP = {[1;1], [-1;-1], [10;-20]} end From 35988cd3f5db5e4baafd2b13495360fa3e538587 Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Sun, 4 Aug 2024 15:05:02 +0200 Subject: [PATCH 03/16] Method eval() implemented; tests added (extrapolation missing). --- DubinsPath.m | 263 ++++++++++++++++------ xUnitTests/DubinsPath/ConnectTestDubins.m | 6 +- xUnitTests/EvalTest.m | 12 +- xUnitTests/ShiftTest.m | 4 +- 4 files changed, 213 insertions(+), 72 deletions(-) diff --git a/DubinsPath.m b/DubinsPath.m index e6160b4..80ee285 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -137,22 +137,21 @@ tauU = NaN; else tauL = 0; - tauU = obj.numel(); + tauU = sum(obj.SegmentLengths > 0); end end%fcn - function [x,y,tau,head,curv,curvDs] = eval(obj, tau, ~) + function [x,y,tau,head,curv,curvDs] = eval(obj, tau, extrap) %EVAL Evaluate path at path parameter. % -% if nargin < 3 % Not supported -% extrap = false; -% end - - N = obj.numel(); + if nargin < 3 + extrap = false; + end - if nargin < 2 - if obj.isempty() + objs = obj.simplify(); + if nargin < 2 % tau is undefined -> set-up + if objs.isempty() x = zeros(0,1); y = zeros(0,1); tau = zeros(0,1); @@ -161,73 +160,102 @@ curvDs = zeros(0,1); return end - - % M samples per L/R segment; 1 sample per S segment; 1 - % additional sample for final segment of any type + + % M samples per L/R segment, 1 sample per S segment and 1 + % additional sample for the final segment of any type M = 100; - types = obj.SegmentTypes; - lengths = obj.SegmentLengths; - Nlr = sum(abs(types(lengths > 0))); - Ns = sum(types == 0 & lengths > 0); + types = objs.SegmentTypes; + lengths = objs.SegmentLengths; + Nnz = numel(lengths(lengths > 0)); % Nonzero length segments + Ns = sum((types == objs.STRAIGHT) & (lengths > 0)); + Nlr = Nnz - Ns; tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); - xyhc = zeros(numel(tau), 4); - - x0 = obj.InitialPos(1); - y0 = obj.InitialPos(2); - h0 = obj.InitialAng; - R = obj.TurningRadius; - tau0 = 0; - jj = 1; - for i = 1:N + xyhc = zeros(numel(tau), 4); +% x0 = objs.InitialPos(1); +% y0 = objs.InitialPos(2); +% h0 = objs.InitialAng; +% R = objs.TurningRadius; + i1 = 1; + for i = 1:objs.numel() si = lengths(i); if si < eps continue end - ii = jj; - jj = ii + M; - - typei = types(i); - if typei == obj.LEFT % Left turn - % S = R*PHI -> PHI = S/R - taui = linspace(0, 1, M+1)'; - hi = linspace(h0, h0+si/R, M+1)'; - [xi,yi,ci] = circleLeft(R, hi); - elseif typei == obj.RIGHT % Right turn - taui = linspace(0, 1, M+1)'; - hi = linspace(h0, h0-si/R, M+1)'; - [xi,yi,ci] = circleRight(R, hi); - else % Straight - jj = ii + 1; - taui = [0; 1]; - xi = [0; si*cos(h0)]; - yi = [0; si*sin(h0)]; - hi = [h0; h0]; - ci = [0; 0]; + i0 = i1; + tau0 = i - 1; + if types(i) == objs.STRAIGHT + i1 = i0 + 1; + taui = [tau0; tau0 + 1]; +% xi = [0; si*cos(h0)]; +% yi = [0; si*sin(h0)]; +% hi = [h0; h0]; +% ci = [0; 0]; + else % Left/right turn + i1 = i0 + M; + taui = linspace(tau0, tau0 + 1, M+1)'; +% if types(i) == objs.LEFT +% % S = R*PHI -> PHI = S/R +% % taui = linspace(0, 1, M+1)'; +% hi = linspace(h0, h0+si/R, M+1)'; +% [xi,yi,ci] = circleLeft(R, hi); +% else +% % taui = linspace(0, 1, M+1)'; +% hi = linspace(h0, h0-si/R, M+1)'; +% [xi,yi,ci] = circleRight(R, hi); +% end end - xi = xi - xi(1) + x0; - yi = yi - yi(1) + y0; - taui = taui + tau0; - x0 = xi(end); - y0 = yi(end); - h0 = hi(end); - tau0 = taui(end); - - xyhc(ii:jj,:) = [xi yi hi ci]; - tau(ii:jj) = taui; -% plot(xi, yi) - end - - else % nargin > 1 - error('ToDo!!!') + +% % Shift to match current starting position +% xi = xi - xi(1) + x0; +% yi = yi - yi(1) + y0; + +% % The next segment starts at the end point of the +% % current segment +% x0 = xi(end); +% y0 = yi(end); +% h0 = hi(end); + + tau(i0:i1) = taui; +% xyhc(i0:i1,:) = [xi yi hi ci]; % plot(xi, yi, 'LineWidth',1) + end%for + +% x = xyhc(:,1); +% y = xyhc(:,2); +% head = xyhc(:,3); +% curv = xyhc(:,4); +% curvDs = zeros(numel(tau), 1); +% return + else + tau = tau(:); end%if + if objs.isempty() + N = numel(tau); + tau(:) = NaN; + x = NaN(N, 1); + y = NaN(N, 1); + head = NaN(N, 1); + curv = NaN(N, 1); + curvDs = NaN(N, 1); + return + end + + +% if nargin < 2 +% [x2,y2,tau2,head2,curv2] = objs.evalImpl(tau(:), extrap); +% assert(max(abs(x - x2)) < 1e-14) +% assert(max(abs(y - y2)) < 1e-14) +% assert(all(tau == tau2)) +% assert(max(abs(head - head2)) < 1e-14) +% assert(all(curv == curv2)) +% end%if + [xyhc,tau,curvDs] = objs.evalImpl(tau(:), extrap); x = xyhc(:,1); y = xyhc(:,2); head = xyhc(:,3); curv = xyhc(:,4); - curvDs = zeros(numel(tau), 1); end%fcn @@ -307,7 +335,6 @@ end%fcn function [tau,idx,ds] = s2tau(obj, s) - error('Not implemented!') end%fcn @@ -354,12 +381,120 @@ function write2file(obj, fn) assert(isscalar(val) && isnumeric(val) && val > 0); obj.TurningRadius = double(val); end%fcn - + end%methods + methods (Access = private) + + function [xyhc,tau,curvDs] = evalImpl(obj, tau, extrap) + + N = obj.numel(); + + xyhc = coder.nullcopy(zeros(numel(tau), 4)); + x0 = obj.InitialPos(1); + y0 = obj.InitialPos(2); + h0 = obj.InitialAng; + R = obj.TurningRadius; + + for i = 1:N + si = obj.SegmentLengths(i); + if si < eps +% continue + end + + % Evaluate the first N-1 pieces on half open intervals + % [t0,t1) and the Nth piece on the closed interval [t0,t1] + if i < N + logIdxi = (tau >= i-1) & ( tau < i); + else + logIdxi = (tau >= i-1) & ( tau <= i); + end + taui = tau(logIdxi); + if isempty(taui) + continue + end + + typei = obj.SegmentTypes(i); + dtau = taui - taui(1); + if typei == obj.LEFT + % Linear interpolation from h0 to h1 = h0 + si/R: + hi = h0 + si/R*dtau; + [xi,yi,ci] = circleLeft(R, hi); + + % Explicitly calculate the end pointsince it may not be + % included in the path parameter dtau + hEnd = h0 + si/R; + [xEnd,yEnd] = circleLeft(R, hEnd); + + elseif typei == obj.RIGHT + % Linear interpolation from h0 to h1 = h0 - si/R: + hi = h0 - si/R*dtau; + [xi,yi,ci] = circleRight(R, hi); + hEnd = h0 - si/R; + [xEnd,yEnd] = circleRight(R, hEnd); + + else % Straight segment + % Linear interpolation x0 + (x1-x0)*tau, where x0 = 0 + xi = si*cos(h0)*dtau; + yi = si*sin(h0)*dtau; + hi = repmat(h0, [numel(taui) 1]); + ci = zeros(size(xi)); + hEnd = h0; + xEnd = si*cos(h0); + yEnd = si*sin(h0); + end%if + + % Shift to match current starting position + dx = x0 - xi(1); + dy = y0 - yi(1); + xi = xi + dx; + yi = yi + dy; + + % The next segment starts at the end point of the current + % segment + x0 = xEnd + dx; + y0 = yEnd + dy; + h0 = hEnd; + + xyhc(logIdxi, :) = [xi yi hi ci]; + end%for + + curvDs = zeros(numel(tau), 1); + + % Set return values to NaN outside path domain + if ~extrap + [tau0,tau1] = obj.domain(); + isOutsideDomain = (tau < tau0) | (tau > tau1); + tau(isOutsideDomain) = NaN; + xyhc(isOutsideDomain,:) = NaN; + curvDs(isOutsideDomain,:) = NaN; + end + + end%fcn + + function objs = simplify(obj) + %SIMPLIFY Get rid of zero-length path segments. + % + + hasNZeroLength = (obj.SegmentLengths > 0); + if ~isempty(hasNZeroLength) && ~any(hasNZeroLength) + % Keep at least one path segment even if it has length + % zero. -> Path that is only defined at the initial point! + hasNZeroLength(1) = true; + end + + objs = DubinsPath(... + [obj.InitialPos; obj.InitialAng], ... + obj.SegmentTypes(hasNZeroLength), ... + obj.SegmentLengths(hasNZeroLength), ... + obj.TurningRadius); + + end%fcn + end + methods (Static) - + function obj = fromStruct(s) end%fcn diff --git a/xUnitTests/DubinsPath/ConnectTestDubins.m b/xUnitTests/DubinsPath/ConnectTestDubins.m index 379dc4d..6a53516 100644 --- a/xUnitTests/DubinsPath/ConnectTestDubins.m +++ b/xUnitTests/DubinsPath/ConnectTestDubins.m @@ -69,7 +69,7 @@ function testLeftTurn(testCase, ConfigsLeft) % Check terminal points testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) - testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(1) y(1) h(1)], C0, 'AbsTol',1e-15); % Check tol testCase.verifyEqual([x(end) y(end) h(end)], C1, 'AbsTol',1e-15); % Check segment types/lengths @@ -122,7 +122,7 @@ function testLRL(testCase, ConfigsLRL) % Check terminal points testCase.verifyEqual([dub.InitialPos' dub.InitialAng], C0) - testCase.verifyEqual([x(1) y(1) h(1)], C0); + testCase.verifyEqual([x(1) y(1) h(1)], C0, 'AbsTol',1e-15); % Check tol testCase.verifyEqual([x(end) y(end)], C1(1:2), 'AbsTol',1e-15); testCase.verifyEqual(mod2Pi(h(end)), mod2Pi(C1(end)), 'AbsTol',1e-15); @@ -173,7 +173,7 @@ function testRLR(testCase, ConfigsRLR) if ~isempty(dub) hold on - dub.plot('MarkerIndices',1, 'Marker','o') + dub.plot('LineWidth',1, 'MarkerIndices',1, 'Marker','o') hold off end diff --git a/xUnitTests/EvalTest.m b/xUnitTests/EvalTest.m index d983835..b5da5b8 100644 --- a/xUnitTests/EvalTest.m +++ b/xUnitTests/EvalTest.m @@ -4,10 +4,13 @@ obj = struct(... 'PolygonPathEmpty', PolygonPath([], [], [], []), ... 'PolygonPathZeroLength', PolygonPath(1, 2, pi/4, 0), ... - 'PolygonPathNonEmpty', PolygonPath.xy2Path(0:10, zeros(1,11)), ... + 'PolygonPathNonEmpty', PolygonPath.xy2Path(0:10, ones(1,11)), ... 'SplinePathEmpty', SplinePath(), ... 'SplinePathZeroLength', SplinePath([0 0], reshape([1 1; 1 2], [2 1 2])), ... - 'SplinePathNonEmpty', SplinePath([0 10], reshape([1 0; 0 0], [2 1 2]))); + 'SplinePathNonEmpty', SplinePath([0 10], reshape([1 0; 0 1], [2 1 2])), ... + 'DubinsPathEmpty', DubinsPath(), ... + 'DubinsPathZeroLength', DubinsPath([1 2 pi/4], 0, 0, 2), ... + 'DubinsPathNonEmpty', DubinsPath([0 1 0], [0 0 0], [1 1 1], 10)); tau = struct(... 'empty', zeros(0,1), ... @@ -67,8 +70,11 @@ function testReturnValues(testCase, obj) testCase.verifyEqual(c, [NaN NaN NaN NaN 0 NaN NaN NaN NaN]'); testCase.verifyEqual(dc, [NaN NaN NaN NaN 0 NaN NaN NaN NaN]'); else + % Assume the path is a straight line at (x,1) for x = + % 0,..,10. Since the path parameter is hard-coded, the path + % must be defined accordingly testCase.verifyEqual(x, [NaN NaN NaN NaN 0.0 0.5 1.0 1.5 2.0]'); - testCase.verifyEqual(y, [NaN NaN NaN NaN 0.0 0.0 0.0 0.0 0.0]'); + testCase.verifyEqual(y, [NaN NaN NaN NaN 1.0 1.0 1.0 1.0 1.0]'); testCase.verifyEqual(h, [NaN NaN NaN NaN 0.0 0.0 0.0 0.0 0.0]'); testCase.verifyEqual(c, [NaN NaN NaN NaN 0.0 0.0 0.0 0.0 0.0]'); testCase.verifyEqual(dc, [NaN NaN NaN NaN 0.0 0.0 0.0 0.0 0.0]'); diff --git a/xUnitTests/ShiftTest.m b/xUnitTests/ShiftTest.m index 66145d9..296a1b8 100644 --- a/xUnitTests/ShiftTest.m +++ b/xUnitTests/ShiftTest.m @@ -7,7 +7,7 @@ 'PolygonPathNonEmpty', PolygonPath.xy2Path(10:-1:0, zeros(1,11)), ... 'SplinePathEmpty', SplinePath(), ... 'SplinePathOneElm', SplinePath.pp2Path(mkpp([-1 2], [0 1 -1; 1 0 2], 2)), ... - 'DubinsPathEmpty', DubinsPath(), ...) + 'DubinsPathEmpty', DubinsPath(), ... 'DubinsPathOneElm',DubinsPath([-1 1 pi/2], 1, 2, 2), ... 'DubinsPathNonEmpty', DubinsPath([-1 1 pi/2], [1 -1 1], [1.4455 9.1741 1.4455], 2)); @@ -27,7 +27,7 @@ function testShift(testCase, obj, dP) testCase.verifyTrue(all(isnan([P0; P1; Q0; Q1]))) else testCase.verifyEqual(Q0, P0+dP); - testCase.verifyEqual(Q1, P1+dP); + testCase.verifyEqual(Q1, P1+dP, 'AbsTol',3e-16); % Tol added for Dubins path end end%fcn From 8ba1308495ba333350d67c8962e6270c738a794f Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Tue, 6 Aug 2024 17:43:41 +0200 Subject: [PATCH 04/16] Fix method eval(); additional test. --- DubinsPath.m | 181 ++++++++++++++++------------------------- xUnitTests/EvalTest.m | 20 ++++- xUnitTests/ShiftTest.m | 4 +- 3 files changed, 89 insertions(+), 116 deletions(-) diff --git a/DubinsPath.m b/DubinsPath.m index 80ee285..5c8de53 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -150,108 +150,28 @@ end objs = obj.simplify(); - if nargin < 2 % tau is undefined -> set-up + + % Make sure that tau is defined + if nargin < 2 if objs.isempty() - x = zeros(0,1); - y = zeros(0,1); tau = zeros(0,1); - head = zeros(0,1); - curv = zeros(0,1); - curvDs = zeros(0,1); - return + else + tau = objs.sampleTau(100); end - - % M samples per L/R segment, 1 sample per S segment and 1 - % additional sample for the final segment of any type - M = 100; - types = objs.SegmentTypes; - lengths = objs.SegmentLengths; - Nnz = numel(lengths(lengths > 0)); % Nonzero length segments - Ns = sum((types == objs.STRAIGHT) & (lengths > 0)); - Nlr = Nnz - Ns; - tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); - - xyhc = zeros(numel(tau), 4); -% x0 = objs.InitialPos(1); -% y0 = objs.InitialPos(2); -% h0 = objs.InitialAng; -% R = objs.TurningRadius; - i1 = 1; - for i = 1:objs.numel() - si = lengths(i); - if si < eps - continue - end - - i0 = i1; - tau0 = i - 1; - if types(i) == objs.STRAIGHT - i1 = i0 + 1; - taui = [tau0; tau0 + 1]; -% xi = [0; si*cos(h0)]; -% yi = [0; si*sin(h0)]; -% hi = [h0; h0]; -% ci = [0; 0]; - else % Left/right turn - i1 = i0 + M; - taui = linspace(tau0, tau0 + 1, M+1)'; -% if types(i) == objs.LEFT -% % S = R*PHI -> PHI = S/R -% % taui = linspace(0, 1, M+1)'; -% hi = linspace(h0, h0+si/R, M+1)'; -% [xi,yi,ci] = circleLeft(R, hi); -% else -% % taui = linspace(0, 1, M+1)'; -% hi = linspace(h0, h0-si/R, M+1)'; -% [xi,yi,ci] = circleRight(R, hi); -% end - end - -% % Shift to match current starting position -% xi = xi - xi(1) + x0; -% yi = yi - yi(1) + y0; - -% % The next segment starts at the end point of the -% % current segment -% x0 = xi(end); -% y0 = yi(end); -% h0 = hi(end); - - tau(i0:i1) = taui; -% xyhc(i0:i1,:) = [xi yi hi ci]; % plot(xi, yi, 'LineWidth',1) - end%for - -% x = xyhc(:,1); -% y = xyhc(:,2); -% head = xyhc(:,3); -% curv = xyhc(:,4); -% curvDs = zeros(numel(tau), 1); -% return else tau = tau(:); end%if - if objs.isempty() + if objs.isempty() % Return all NaN's if path is empty N = numel(tau); tau(:) = NaN; - x = NaN(N, 1); - y = NaN(N, 1); - head = NaN(N, 1); - curv = NaN(N, 1); + xyhc = NaN(N, 4); curvDs = NaN(N, 1); - return + else % Otherwise, evaluate path definition + [xyhc,tau,curvDs] = objs.evalImpl(tau(:), extrap); end - -% if nargin < 2 -% [x2,y2,tau2,head2,curv2] = objs.evalImpl(tau(:), extrap); -% assert(max(abs(x - x2)) < 1e-14) -% assert(max(abs(y - y2)) < 1e-14) -% assert(all(tau == tau2)) -% assert(max(abs(head - head2)) < 1e-14) -% assert(all(curv == curv2)) -% end%if - [xyhc,tau,curvDs] = objs.evalImpl(tau(:), extrap); + % Unpack data x = xyhc(:,1); y = xyhc(:,2); head = xyhc(:,3); @@ -267,7 +187,7 @@ error('Not implemented!') end%fcn - + function [xy,Q,idx,tau] = frenet2cart(obj, sd, doPlot) error('Not implemented!') end%fcn @@ -398,12 +318,7 @@ function write2file(obj, fn) R = obj.TurningRadius; for i = 1:N - si = obj.SegmentLengths(i); - if si < eps -% continue - end - - % Evaluate the first N-1 pieces on half open intervals + % Evaluate the first N-1 pieces on half-open intervals % [t0,t1) and the Nth piece on the closed interval [t0,t1] if i < N logIdxi = (tau >= i-1) & ( tau < i); @@ -411,28 +326,30 @@ function write2file(obj, fn) logIdxi = (tau >= i-1) & ( tau <= i); end taui = tau(logIdxi); - if isempty(taui) - continue - end +% if isempty(taui) +% continue +% end + si = obj.SegmentLengths(i); typei = obj.SegmentTypes(i); - dtau = taui - taui(1); + dtau = taui - i + 1; if typei == obj.LEFT % Linear interpolation from h0 to h1 = h0 + si/R: hi = h0 + si/R*dtau; [xi,yi,ci] = circleLeft(R, hi); - % Explicitly calculate the end pointsince it may not be - % included in the path parameter dtau + % Explicitly calculate the terminal points of the + % current segment.. since they may not be included in + % the path parameter dtau hEnd = h0 + si/R; - [xEnd,yEnd] = circleLeft(R, hEnd); + [xT,yT] = circleLeft(R, [h0 hEnd]); elseif typei == obj.RIGHT % Linear interpolation from h0 to h1 = h0 - si/R: hi = h0 - si/R*dtau; [xi,yi,ci] = circleRight(R, hi); hEnd = h0 - si/R; - [xEnd,yEnd] = circleRight(R, hEnd); + [xT,yT] = circleRight(R, [h0 hEnd]); else % Straight segment % Linear interpolation x0 + (x1-x0)*tau, where x0 = 0 @@ -441,20 +358,23 @@ function write2file(obj, fn) hi = repmat(h0, [numel(taui) 1]); ci = zeros(size(xi)); hEnd = h0; - xEnd = si*cos(h0); - yEnd = si*sin(h0); + xT = [0 si*cos(h0)]; + yT = [0 si*sin(h0)]; end%if - % Shift to match current starting position - dx = x0 - xi(1); - dy = y0 - yi(1); + % Shift to match current starting position. This assumes + % the current segment is evaluated at tau(1) = 0! + % dx = x0 - xi(1); + % dy = y0 - yi(1); + dx = x0 - xT(1); + dy = y0 - yT(1); xi = xi + dx; yi = yi + dy; % The next segment starts at the end point of the current % segment - x0 = xEnd + dx; - y0 = yEnd + dy; + x0 = xT(2) + dx; + y0 = yT(2) + dy; h0 = hEnd; xyhc(logIdxi, :) = [xi yi hi ci]; @@ -473,6 +393,38 @@ function write2file(obj, fn) end%fcn + function tau = sampleTau(obj, M) + + % M samples per L/R segment, 1 sample per S segment and 1 + % additional sample for the final segment of any type + types = obj.SegmentTypes; + lengths = obj.SegmentLengths; + Nnz = numel(lengths(lengths > 0)); % Nonzero length segments + Ns = sum((types == obj.STRAIGHT) & (lengths > 0)); + Nlr = Nnz - Ns; + tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); + + i1 = 1; + for i = 1:obj.numel() + si = lengths(i); + if si < eps + continue + end + + i0 = i1; + tau0 = i - 1; + if types(i) == obj.STRAIGHT + i1 = i0 + 1; + taui = [tau0; tau0 + 1]; + else % Left/right turn + i1 = i0 + M; + taui = linspace(tau0, tau0 + 1, M+1)'; + end + tau(i0:i1) = taui; + end%for + + end%fcn + function objs = simplify(obj) %SIMPLIFY Get rid of zero-length path segments. % @@ -484,11 +436,14 @@ function write2file(obj, fn) hasNZeroLength(1) = true; end + % Create simplified Dubins path with explicitly specified + % IsCircuit property to avoid an infinit recursion. objs = DubinsPath(... [obj.InitialPos; obj.InitialAng], ... obj.SegmentTypes(hasNZeroLength), ... obj.SegmentLengths(hasNZeroLength), ... - obj.TurningRadius); + obj.TurningRadius, ... + obj.IsCircuit); end%fcn end diff --git a/xUnitTests/EvalTest.m b/xUnitTests/EvalTest.m index b5da5b8..11de52d 100644 --- a/xUnitTests/EvalTest.m +++ b/xUnitTests/EvalTest.m @@ -20,7 +20,7 @@ 'nd', randn(10,3,4)*10); end - + methods (Test) function testReturnSizeNoTau(testCase, obj) @@ -191,6 +191,24 @@ function testExtrapolationSpline(testCase, obj) testCase.verifyEqual(d, dSet); end%fcn + + function testReturnValuesDubins(testCase) + % Make sure eval() returns the same values irrespective of the path + % segments adressed by the path parameter argument. I.e., the + % (i+1)-th segment starts at the end point of the i-th segment. + + dub = DubinsPath([0 0 0], [-1 0 1], [1 2 3], 2); + + [x1,y1,tau1,h1,c1] = dub.eval(2); + [x2,y2,tau2,h2,c2] = dub.eval([0 1 2]); + + verifyEqual(testCase, x2(end), x1); + verifyEqual(testCase, y2(end), y1); + verifyEqual(testCase, tau2(end), tau1); + verifyEqual(testCase, h2(end), h1); + verifyEqual(testCase, c2(end), c1); + + end%fcn end diff --git a/xUnitTests/ShiftTest.m b/xUnitTests/ShiftTest.m index 296a1b8..0483e4c 100644 --- a/xUnitTests/ShiftTest.m +++ b/xUnitTests/ShiftTest.m @@ -27,7 +27,7 @@ function testShift(testCase, obj, dP) testCase.verifyTrue(all(isnan([P0; P1; Q0; Q1]))) else testCase.verifyEqual(Q0, P0+dP); - testCase.verifyEqual(Q1, P1+dP, 'AbsTol',3e-16); % Tol added for Dubins path + testCase.verifyEqual(Q1, P1+dP, 'AbsTol',4e-15); % Tol added for Dubins path end end%fcn @@ -41,7 +41,7 @@ function testDefaultArg(testCase, obj) [Q0,Q1] = objs.termPoints(); if ~isempty(obj) testCase.verifyEqual(Q0, [0;0]); - testCase.verifyEqual(Q1, P1-P0); + testCase.verifyEqual(Q1, P1-P0, 'AbsTol',5e-16); % Tol added for Dubins path end end%fcn From ac2ba54fa5e8dc7303a83e56d71480ec97c22b0e Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Tue, 6 Aug 2024 20:57:41 +0200 Subject: [PATCH 05/16] Set property IsCircuit at construction, fix method termPoints() and add tests. --- DubinsPath.m | 16 ++++---- .../IsCircuitTestDubins.m.type.File.xml | 6 +++ xUnitTests/DubinsPath/IsCircuitTestDubins.m | 24 +++++++++++ xUnitTests/TermPointsTest.m | 40 ++++++++++++------- 4 files changed, 64 insertions(+), 22 deletions(-) create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IsCircuitTestDubins.m.type.File.xml create mode 100644 xUnitTests/DubinsPath/IsCircuitTestDubins.m diff --git a/DubinsPath.m b/DubinsPath.m index 5c8de53..4b03be3 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -95,18 +95,18 @@ assert(isequal(numel(types), numel(lengths)), ... 'DubinsPath:Constructor:numelTypesLengths', ... - 'Number of path property elements mismatch!'); + 'Number of path property elements must be equal!'); obj.SegmentTypes = types; obj.SegmentLengths = lengths; obj.TurningRadius = R; obj.ArcLengths = [0; cumsum(obj.SegmentLengths)']; - % if nargin < 5 - % obj = obj.setIsCircuit(1e-5); - % else - % obj.IsCircuit = isCircuit; - % end%if + if nargin < 5 + obj = obj.setIsCircuit(1e-5); + else + obj.IsCircuit = isCircuit; + end%if end%Constructor @@ -264,8 +264,8 @@ P0 = [NaN; NaN]; P1 = [NaN; NaN]; else - P0 = obj.InitialPos(:); - [x,y] = obj.eval(obj.numel() - 1); + P0 = obj.InitialPos; + [x,y] = obj.eval(obj.numel()); P1 = [x; y]; end diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IsCircuitTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IsCircuitTestDubins.m.type.File.xml new file mode 100644 index 0000000..d8fadf3 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IsCircuitTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/xUnitTests/DubinsPath/IsCircuitTestDubins.m b/xUnitTests/DubinsPath/IsCircuitTestDubins.m new file mode 100644 index 0000000..4075e08 --- /dev/null +++ b/xUnitTests/DubinsPath/IsCircuitTestDubins.m @@ -0,0 +1,24 @@ +classdef IsCircuitTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + end + + methods (Test) + + function testCircuitTrue(testCase) + R = 2; + d = 3; + dub = DubinsPath([0 0 0], [1 0 1 0], [R*pi d R*pi d], R); + verifyTrue(testCase, dub.IsCircuit); + end%fcn + + function testCircuitFalse(testCase) + R = 2; + d = 3; + dub = DubinsPath([0 0 0], [1 0 1 0], [R*pi d R*pi d*0.99], R); + verifyFalse(testCase, dub.IsCircuit); + end%fcn + + end + +end%class diff --git a/xUnitTests/TermPointsTest.m b/xUnitTests/TermPointsTest.m index 350d86f..f43f436 100644 --- a/xUnitTests/TermPointsTest.m +++ b/xUnitTests/TermPointsTest.m @@ -1,26 +1,38 @@ classdef TermPointsTest < matlab.unittest.TestCase properties (TestParameter) - obj = struct(... - 'PolygonPathEmpty', PolygonPath(), ... - 'PolygonPathNonEmpty', PolygonPath.xy2Path(0:10, zeros(1,11)), ... - 'SplinePathEmpty', SplinePath(), ... - 'SplinePathNonEmpty', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2))); + PathEmpty = struct(... + 'PolygonPath', PolygonPath(), ... + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) + + PathNonEmpty = {... + {PolygonPath.xy2Path(0:10, zeros(1,11)), [0;0], [10;0]}; + {SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2)), [0;0], [2;1]}; + {DubinsPath([1 -1 0], [1 -1 0], [2*pi/2 2*pi 2], 2), [1;-1], [7;-1]}; + } end methods (Test) - function testSize(testCase, obj) + + function testSizeEmpty(testCase, PathEmpty) + [P0,P1] = PathEmpty.termPoints(); + + excpected = [NaN; NaN]; + verifyEqual(testCase, P0, excpected) + verifyEqual(testCase, P1, excpected) + end%fcn + + function testSizeNonEmpty(testCase, PathNonEmpty) + obj = PathNonEmpty{1}; + P0Set = PathNonEmpty{2}; + P1Set = PathNonEmpty{3}; [P0,P1] = obj.termPoints(); - testCase.verifySize(P0, [2 1]); - testCase.verifySize(P1, [2 1]); - - if isempty(obj) - set = [NaN; NaN]; - testCase.verifyEqual(P0, set) - testCase.verifyEqual(P1, set) - end + verifyEqual(testCase, P0, P0Set); + verifyEqual(testCase, P1, P1Set, 'AbsTol',3e-16); end%fcn + end end%class From 2ddf70be1e4fbcae0227a5846aefe37fafa4d13b Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Fri, 16 Aug 2024 21:53:35 +0200 Subject: [PATCH 06/16] Adding basic tests for DubinsPath. --- DubinsPath.m | 29 ++++++----------------------- xUnitTests/CumLengthsTest.m | 7 +++++-- xUnitTests/DomainTest.m | 9 ++++++--- xUnitTests/LengthTest.m | 5 +++-- xUnitTests/NumelTest.m | 5 ++++- xUnitTests/SampleDomainTest.m | 4 +++- 6 files changed, 27 insertions(+), 32 deletions(-) diff --git a/DubinsPath.m b/DubinsPath.m index 4b03be3..977e9b1 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -10,7 +10,7 @@ % % DubinsPath methods: % DubinsPath - Constructor. -% convertSegmentType2Char - ... +% convertSegmentType2Char - Convert numeric segment type to character. % % DubinsPath static methods: % connect - Create Dubins path from initial/target configuration. @@ -41,11 +41,6 @@ InitialAng = 0 end - properties (Access = private) - % ArcLengths - Cumulative length of path segments - ArcLengths = zeros(0, 1) - end - properties (Constant, Hidden) % AdmissiblePaths - Admissible paths of the dubins set % The six admissible paths are {LSL, RSR, RSL, LSR, RLR, LRL}, @@ -75,11 +70,10 @@ %DUBINSPATH Create Dubins path object. % OBJ = DUBINSPATH() creates an empty path. % - % OBJ = DUBINSPATH(C0, T, L, R) creates a Dubins-like path with - % initial pose C0, consisting of path segment types T with + % OBJ = DUBINSPATH(C0, T, L, R) creates a path consisting of + % Dubins segments with initial pose C0, segment types T with % individual lengths L. Arc segments have a radius R. % - % OBJ = DUBINSPATH(___,ISCIRCUIT) set to true if the path is a % circuit. % @@ -100,7 +94,7 @@ obj.SegmentLengths = lengths; obj.TurningRadius = R; - obj.ArcLengths = [0; cumsum(obj.SegmentLengths)']; + obj.ArcLengths = cumsum(obj.SegmentLengths)'; if nargin < 5 obj = obj.setIsCircuit(1e-5); @@ -119,17 +113,13 @@ end%fcn function c = convertSegmentType2Char(obj) - %CONVERTSEGMENTTYPE2CHAR Convert segment type to character + %CONVERTSEGMENTTYPE2CHAR Convert segment type to character. % C = CONVERTSEGMENTTYPE2CHAR(OBJ) converts numeric property % SegmentTypes to character representation. c = obj.MapType2Char(obj.SegmentTypes + 2); end%fcn - function s = cumlengths(obj) - s = obj.ArcLengths; - end%fcn - function [tauL,tauU] = domain(obj) if isempty(obj) @@ -200,10 +190,6 @@ error('Not implemented!') end%fcn - function s = length(obj) - s = obj.ArcLengths(end); - end%fcn - function n = numel(obj) n = numel(obj.SegmentTypes); end%fcn @@ -326,9 +312,6 @@ function write2file(obj, fn) logIdxi = (tau >= i-1) & ( tau <= i); end taui = tau(logIdxi); -% if isempty(taui) -% continue -% end si = obj.SegmentLengths(i); typei = obj.SegmentTypes(i); @@ -485,7 +468,7 @@ function write2file(obj, fn) [T(:,5),S(5),L(:,5)] = dubinsRSL(d, phi0, phi1); [T(:,6),S(6),L(:,6)] = dubinsRSR(d, phi0, phi1); - % Find the shortest one + % Find the shortest path [~,minIdx] = min(S); assert(sum(L(:, minIdx)) == S(minIdx)) obj = DubinsPath(C0, T(:, minIdx), L(:, minIdx)*R, R); diff --git a/xUnitTests/CumLengthsTest.m b/xUnitTests/CumLengthsTest.m index f8920bf..6bd417c 100644 --- a/xUnitTests/CumLengthsTest.m +++ b/xUnitTests/CumLengthsTest.m @@ -3,13 +3,16 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()) + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) PathNonEmpty = struct(... 'PolygonPathZeroLength', PolygonPath(0, 0, 0, 0, 0), ... 'SplinePathZeroLength', SplinePath([1 1], reshape([1 0;1 1], [2 1 2])), ... + 'DubinsPathZeroLength', DubinsPath([1 0 pi], 0, 0, 2), ... 'PolygonPath', PolygonPath.xy2Path(0:10, zeros(1,11)), ... - 'SplinePath', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2))) + 'SplinePath', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2)), ... + 'DubinsPath', DubinsPath([1 0 pi], [1 -1 0], [2*pi/2 2*pi 2], 2)) end diff --git a/xUnitTests/DomainTest.m b/xUnitTests/DomainTest.m index 557b35f..23df7ea 100644 --- a/xUnitTests/DomainTest.m +++ b/xUnitTests/DomainTest.m @@ -3,15 +3,18 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()) + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) PathZeroLength = struct(... % Non-empty but zero length 'PolygonPath', PolygonPath(1, 1, 0, 0), ... - 'SplinePath', SplinePath([0 0], reshape([1 0; 0 0], [2 1 2]))) + 'SplinePath', SplinePath([0 0], reshape([1 0; 0 0], [2 1 2])), ... + 'DubinsPathZeroLength', DubinsPath([1 0 pi], 0, 0, 2)) PathNonEmpty = struct(... 'PolygonPath', PolygonPath.xy2Path(0:10, zeros(1,11)), ... - 'SplinePath', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], [2 2 2]))) + 'SplinePath', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], [2 2 2])), ... + 'DubinsPath', DubinsPath([1 0 pi], [1 -1 0], [2*pi/2 2*pi 2], 2)) end diff --git a/xUnitTests/LengthTest.m b/xUnitTests/LengthTest.m index 7c5ed31..97b6164 100644 --- a/xUnitTests/LengthTest.m +++ b/xUnitTests/LengthTest.m @@ -3,14 +3,15 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath() ... - ) + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) PathNonEmpty = {... {PolygonPath.xy2Path(0:10, zeros(1,11)), 10}; {SplinePath.pp2Path(... mkpp([-1 0 1 2], [0 1 -1; 1 -2 1; 0 1 0; 1 0 0; 0 1 1; 0 0 1], 2)), ... 0.5*(2*sqrt(5) + asinh(2)) + 1}; % via wolframalpha ... + {DubinsPath([1 2 pi], [1 -1 0], [1 2 3], 2), 6} } end diff --git a/xUnitTests/NumelTest.m b/xUnitTests/NumelTest.m index 531cc6a..663b6fe 100644 --- a/xUnitTests/NumelTest.m +++ b/xUnitTests/NumelTest.m @@ -3,13 +3,16 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()) + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) PathNonEmpty = {% {Path object, number of segments} {PolygonPath(1, 2, 3, 4), 0}; {PolygonPath([0 1], [1 1], [0 0], [0 0]), 1}; {PolygonPath.xy2Path(0:10, zeros(1,11)), 10}; {SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2)), 2}; + {DubinsPath([1 2 0], 0, 0, 2), 1} + {DubinsPath([1 0 pi], [1 -1 0], [1 1 1], 2), 3}; } end diff --git a/xUnitTests/SampleDomainTest.m b/xUnitTests/SampleDomainTest.m index 09ad06c..6f82a6f 100644 --- a/xUnitTests/SampleDomainTest.m +++ b/xUnitTests/SampleDomainTest.m @@ -5,7 +5,9 @@ 'PolygonPathEmpty', PolygonPath(), ... 'PolygonPathNonEmpty', PolygonPath.xy2Path(1:10, 1:10), ... 'SplinePathEmpty', SplinePath(), ... - 'SplinePathNonEmpty', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2))); + 'SplinePathNonEmpty', SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], 2,2,2)), ... + 'DubinsPathEmpty', DubinsPath(), ... + 'DubinsPathNonEmpty', DubinsPath([1 0 pi], [1 -1 0], [2*pi/2 2*pi 2], 2)); uintx = {'uint8','uint16','uint32','uint64'}; float = {'single', 'double'}; end From 07979558b90e999a0e7ac8ad99009174cbcf288e Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Wed, 28 Aug 2024 19:44:08 +0200 Subject: [PATCH 07/16] Basic implementation of s2tau() and frenet2cart(). --- DubinsPath.m | 52 ++++++++++++++- .../Frenet2CartTestDubins.m.type.File.xml | 6 ++ .../S2TauTestDubins.m.type.File.xml | 6 ++ xUnitTests/DubinsPath/Frenet2CartTestDubins.m | 66 +++++++++++++++++++ xUnitTests/DubinsPath/S2TauTestDubins.m | 65 ++++++++++++++++++ 5 files changed, 192 insertions(+), 3 deletions(-) create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Frenet2CartTestDubins.m.type.File.xml create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/S2TauTestDubins.m.type.File.xml create mode 100644 xUnitTests/DubinsPath/Frenet2CartTestDubins.m create mode 100644 xUnitTests/DubinsPath/S2TauTestDubins.m diff --git a/DubinsPath.m b/DubinsPath.m index 977e9b1..f3a5c58 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -179,7 +179,31 @@ end%fcn function [xy,Q,idx,tau] = frenet2cart(obj, sd, doPlot) - error('Not implemented!') + + % Get the indexes referring to the path segments according to + % the frenet coordinates s-value + [tau,idx] = obj.s2tau(sd(:,1)); + + [x,y,~,head] = obj.eval(tau, true); + Q = [x,y]; + + % Tangent vector already has length 1 - > no need to normalize + tHandle = @(phi) [-sin(phi) cos(phi)]; + t = tHandle(head-pi/2); + + xy = Q + bsxfun(@times, [-t(:,2), t(:,1)], sd(:,2)); + + if (nargin > 2) && doPlot + obj.plot('DisplayName',class(obj)); + hold on + % [xb,yb] = obj.eval(obj.Breaks); + % plot(xb, yb, 'b.', 'MarkerSize',10, 'DisplayName','Breaks'); + plot(xy(:,1), xy(:,2), 'o', 'DisplayName','xy'); + plot(Q(:,1), Q(:,2), 'kx', 'DisplayName','Q'); + hold off + legend('-DynamicLegend') + end%if + end%fcn function [xy,tau,errFlag] = intersectCircle(obj, C, r, doPlot) @@ -240,8 +264,30 @@ end%fcn - function [tau,idx,ds] = s2tau(obj, s) - error('Not implemented!') + function [tau,idx] = s2tau(obj, s) + + if obj.length() < eps % Zero-length path + tau = nan(size(s)); + idx = zeros(size(s), 'uint32'); + if ~obj.isempty() + theIdx = abs(s) < eps; + tau(theIdx) = 0; + idx(theIdx) = 1; + end + return + end + + if obj.IsCircuit + s = mod(s, obj.length()); + end + + S = obj.ArcLengths; + [~,tmp] = histc(s, [0;S;inf]); %#ok + idx = min(max(uint32(tmp), 1), numel(S)); + + S = [0; S]; + ds = s - reshape(S(idx), size(s)); + tau = double(idx-1) + ds./reshape(S(idx+1) - S(idx), size(s)); end%fcn function [P0,P1] = termPoints(obj) diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Frenet2CartTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Frenet2CartTestDubins.m.type.File.xml new file mode 100644 index 0000000..d8fadf3 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Frenet2CartTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/S2TauTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/S2TauTestDubins.m.type.File.xml new file mode 100644 index 0000000..d8fadf3 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/S2TauTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m new file mode 100644 index 0000000..1415c3c --- /dev/null +++ b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m @@ -0,0 +1,66 @@ +classdef Frenet2CartTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + PathObj = {DubinsPath([0 0 0], [0 1 -1], [1 pi/2 pi/2], 2)} + DSet = {-1 +1 3 -4} + end + + + + methods (Test) + + function testFrenet2Cart(testCase, PathObj) + + s = [0 0.5 linspace(1, PathObj.length(), 6)]; + d = zeros(size(s)); + [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); + + % Test the distance from Q to xy + dAct = hypot1Arg(xy - Q); + verifyEqual(testCase, dAct, d(:)); + + % Hard + xySet = [0 0; 0.5 0; 1 0; 1.6180 0.0979; 2.1756 0.382; ... + 2.6529 0.7896; 3.2104 1.0737; 3.8284 1.1716]; + verifyEqual(testCase, xy, xySet, 'AbsTol',5e-5); + verifyEqual(testCase, Q, xy); + verifyEqual(testCase, idx, uint32([1 1 2 2 2 3 3 3])'); + verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); + end%fcn + + function testFrenet2CartVariedD(testCase, PathObj, DSet) + + s = [0 0.5 linspace(1, PathObj.length(), 6)]; + d = DSet*ones(size(s)); + [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); + + % Test the distance from Q to xy + dAct = hypot1Arg(xy - Q); + verifyEqual(testCase, dAct, abs(d(:)), 'AbsTol',1e-15); + + % Segment index and tau are independent of DSet + verifyEqual(testCase, idx, uint32([1 1 2 2 2 3 3 3])'); + verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); + end%fcn + + function testOutOfBound(testCase, PathObj) + + % Pre-path/post-path solution + [xy,Q,idx,tau] = PathObj.frenet2cart([-1 1; 5 1], true); + testCase.verifyEqual(xy, [-1 2; 3 0]); + testCase.verifyEqual(Q, [-1 1; 3 1]); + testCase.verifyEqual(idx, uint32([1; 2])); + testCase.verifyEqual(tau, [-1; 3]); + end%fcn + + function testCircuit(testCase) + % TODO + end%fcn + + end + +end%class + +function d = hypot1Arg(x) +d = hypot(x(:,1), x(:,2)); +end%fcn \ No newline at end of file diff --git a/xUnitTests/DubinsPath/S2TauTestDubins.m b/xUnitTests/DubinsPath/S2TauTestDubins.m new file mode 100644 index 0000000..c5d5c58 --- /dev/null +++ b/xUnitTests/DubinsPath/S2TauTestDubins.m @@ -0,0 +1,65 @@ +classdef S2TauTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + s = struct(... + 'emptyTau', [], ... + 'scalarTau', 0, ... + 'vectorTau', linspace(-1,4,26), ... + 'ndTau', [ones(10,2,3); zeros(1,2,3)]) + end + + methods (Test) + function testReturnValuesEmptyPath(testCase, s) + % Empty path + obj = DubinsPath(); + [tau,idx] = obj.s2tau(s); + verifyEqual(testCase, tau, nan(size(s))); + verifyEqual(testCase, idx, zeros(size(s), 'uint32')); + end%fcn + + function testReturnValuesZeroLengthPath(testCase, s) + + % For non-empty s, at least one value should equal zero! + assert(isempty(s) || any(s(:)==0)) + + % Non-empty but zero-length path + obj = DubinsPath([0 0 0], 0, 0, 2); + [tau,idx] = obj.s2tau(s); + + tauSet = nan(size(s)); + tauSet(s==0) = 0; + verifyEqual(testCase, tau, tauSet); + + idxSet = zeros(size(s), 'uint32'); + idxSet(s==0) = uint32(1); + verifyEqual(testCase, idx, idxSet); + end%fcn + + function testReturnValuesNonEmptyPath(testCase) + + obj = DubinsPath([0 0 0], [1 0 -1], [pi/4 1 pi/4], 2); + s = [-1 -0.2 0 0.1 100 2 1.5 5 9 20]; %#ok<*PROP> + [tau,idx] = obj.s2tau(s); + + [tau0,tau1] = obj.domain(); + tauSet = interp1([0; obj.cumlengths()], tau0:tau1, s, 'linear','extrap'); + verifyEqual(testCase, tau, tauSet, 'AbsTol',2e-14); + + verifyEqual(testCase, idx, uint32([1 1 1 1 3 3 2 3 3])); + end%fcn + + function testReturnValuesOutOfBounds(testCase) + obj = DubinsPath([0 0 0], [1 0 -1], [2 1 2], 2); + + s = [-1 7]; + N = obj.numel(); + [tau0,tau1] = obj.domain(); + tauSet = interp1([0; obj.cumlengths()], tau0:tau1, s, 'linear','extrap'); + + [tau,idx] = obj.s2tau(s); + verifyEqual(testCase, tau, tauSet); + verifyEqual(testCase, idx, uint32([1 N])); + end%fcn + end + +end%class From 007cfba9ab768da72290bdffb01c8447346a7ace Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Fri, 27 Sep 2024 19:18:26 +0200 Subject: [PATCH 08/16] Fix method eval()/frenet2cart(). Additional test cases for Frenet2CartTestDubins.m, EvalTest.m (Dubins path evaluation with extrapolation). --- DubinsPath.m | 53 +++++++++++------ xUnitTests/DubinsPath/Frenet2CartTestDubins.m | 32 +++++------ xUnitTests/DubinsPath/S2TauTestDubins.m | 22 +++---- xUnitTests/EvalTest.m | 57 ++++++++++++++++++- xUnitTests/S2TauTest.m | 8 ++- 5 files changed, 123 insertions(+), 49 deletions(-) diff --git a/DubinsPath.m b/DubinsPath.m index f3a5c58..3df1921 100644 --- a/DubinsPath.m +++ b/DubinsPath.m @@ -152,13 +152,18 @@ tau = tau(:); end%if - if objs.isempty() % Return all NaN's if path is empty + if objs.isempty() % Empty path: return all NaN's/no extrapolation N = numel(tau); tau(:) = NaN; - xyhc = NaN(N, 4); - curvDs = NaN(N, 1); + xyhc = NaN(N, 5); + elseif objs.length() < eps % Zero length path: no extrapolation + xyhc = repmat([objs.InitialPos(:)', objs.InitialAng, ... + double(objs.SegmentTypes(1))/objs.TurningRadius, 0], ... + numel(tau), 1); + xyhc(tau ~= 0, :) = NaN; + tau(tau ~= 0) = NaN; else % Otherwise, evaluate path definition - [xyhc,tau,curvDs] = objs.evalImpl(tau(:), extrap); + [xyhc,tau] = objs.evalImpl(tau(:), extrap); end % Unpack data @@ -166,6 +171,7 @@ y = xyhc(:,2); head = xyhc(:,3); curv = xyhc(:,4); + curvDs = xyhc(:,5); end%fcn @@ -194,8 +200,9 @@ xy = Q + bsxfun(@times, [-t(:,2), t(:,1)], sd(:,2)); if (nargin > 2) && doPlot - obj.plot('DisplayName',class(obj)); + h = obj.plot(linspace(min(tau),max(tau),1e3), '--','DisplayName','Extrap.'); hold on + obj.plot('Color',get(h,'Color'), 'DisplayName',class(obj)); % [xb,yb] = obj.eval(obj.Breaks); % plot(xb, yb, 'b.', 'MarkerSize',10, 'DisplayName','Breaks'); plot(xy(:,1), xy(:,2), 'o', 'DisplayName','xy'); @@ -339,24 +346,41 @@ function write2file(obj, fn) methods (Access = private) - function [xyhc,tau,curvDs] = evalImpl(obj, tau, extrap) + function [xyhc,tau] = evalImpl(obj, tau, extrap) N = obj.numel(); - xyhc = coder.nullcopy(zeros(numel(tau), 4)); + xyhc = coder.nullcopy(zeros(numel(tau), 5)); x0 = obj.InitialPos(1); y0 = obj.InitialPos(2); h0 = obj.InitialAng; R = obj.TurningRadius; + % Assign values of tau to path segments + [a,b] = obj.domain(); + [~,segIdxs] = histc(tau, [a:b, inf]); %#ok + segIdxs = max(min(segIdxs, N), 1); + + % Assume extrap=true for path evaluation for i = 1:N % Evaluate the first N-1 pieces on half-open intervals % [t0,t1) and the Nth piece on the closed interval [t0,t1] - if i < N - logIdxi = (tau >= i-1) & ( tau < i); - else - logIdxi = (tau >= i-1) & ( tau <= i); - end +% if i == 1 +% if N < 2 +% logIdxi = (tau == 0); +% else +% logIdxi = (tau < 1); +% end +% elseif i == N +% logIdxi = (tau >= N-1); +% else % i < N +% logIdxi = (tau >= i-1) & (tau < i); +% % else +% % logIdxi = (tau >= i-1) & (tau <= i); +% end +% % taui = tau(logIdxi); + + logIdxi = (segIdxs == i); taui = tau(logIdxi); si = obj.SegmentLengths(i); @@ -406,18 +430,15 @@ function write2file(obj, fn) y0 = yT(2) + dy; h0 = hEnd; - xyhc(logIdxi, :) = [xi yi hi ci]; + xyhc(logIdxi, :) = [xi yi hi ci zeros(size(xi))]; end%for - curvDs = zeros(numel(tau), 1); - % Set return values to NaN outside path domain if ~extrap [tau0,tau1] = obj.domain(); isOutsideDomain = (tau < tau0) | (tau > tau1); tau(isOutsideDomain) = NaN; xyhc(isOutsideDomain,:) = NaN; - curvDs(isOutsideDomain,:) = NaN; end end%fcn diff --git a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m index 1415c3c..855e32e 100644 --- a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m +++ b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m @@ -4,22 +4,22 @@ PathObj = {DubinsPath([0 0 0], [0 1 -1], [1 pi/2 pi/2], 2)} DSet = {-1 +1 3 -4} end - - + + methods (Test) function testFrenet2Cart(testCase, PathObj) - + s = [0 0.5 linspace(1, PathObj.length(), 6)]; d = zeros(size(s)); [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); - + % Test the distance from Q to xy dAct = hypot1Arg(xy - Q); verifyEqual(testCase, dAct, d(:)); - - % Hard + + % Hard coded solution xySet = [0 0; 0.5 0; 1 0; 1.6180 0.0979; 2.1756 0.382; ... 2.6529 0.7896; 3.2104 1.0737; 3.8284 1.1716]; verifyEqual(testCase, xy, xySet, 'AbsTol',5e-5); @@ -27,30 +27,30 @@ function testFrenet2Cart(testCase, PathObj) verifyEqual(testCase, idx, uint32([1 1 2 2 2 3 3 3])'); verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); end%fcn - + function testFrenet2CartVariedD(testCase, PathObj, DSet) - + s = [0 0.5 linspace(1, PathObj.length(), 6)]; d = DSet*ones(size(s)); [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); - + % Test the distance from Q to xy dAct = hypot1Arg(xy - Q); verifyEqual(testCase, dAct, abs(d(:)), 'AbsTol',1e-15); - + % Segment index and tau are independent of DSet verifyEqual(testCase, idx, uint32([1 1 2 2 2 3 3 3])'); verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); end%fcn function testOutOfBound(testCase, PathObj) - + % Pre-path/post-path solution - [xy,Q,idx,tau] = PathObj.frenet2cart([-1 1; 5 1], true); - testCase.verifyEqual(xy, [-1 2; 3 0]); - testCase.verifyEqual(Q, [-1 1; 3 1]); - testCase.verifyEqual(idx, uint32([1; 2])); - testCase.verifyEqual(tau, [-1; 3]); + [xy,Q,idx,tau] = PathObj.frenet2cart([-1 1; 5 1], false); + testCase.verifyEqual(xy, [-1 1; 5.076867634387 1.899465155730], 'AbsTol',1e-12); + testCase.verifyEqual(Q, [-1 0; 4.660720797840 0.990167728905], 'AbsTol',1e-12); + testCase.verifyEqual(idx, uint32([1; 3])); + testCase.verifyEqual(tau, [-1; 3.546479089470], 'AbsTol',1e-12); end%fcn function testCircuit(testCase) diff --git a/xUnitTests/DubinsPath/S2TauTestDubins.m b/xUnitTests/DubinsPath/S2TauTestDubins.m index c5d5c58..609c044 100644 --- a/xUnitTests/DubinsPath/S2TauTestDubins.m +++ b/xUnitTests/DubinsPath/S2TauTestDubins.m @@ -8,20 +8,14 @@ 'ndTau', [ones(10,2,3); zeros(1,2,3)]) end + + methods (Test) - function testReturnValuesEmptyPath(testCase, s) - % Empty path - obj = DubinsPath(); - [tau,idx] = obj.s2tau(s); - verifyEqual(testCase, tau, nan(size(s))); - verifyEqual(testCase, idx, zeros(size(s), 'uint32')); - end%fcn - function testReturnValuesZeroLengthPath(testCase, s) - + % For non-empty s, at least one value should equal zero! assert(isempty(s) || any(s(:)==0)) - + % Non-empty but zero-length path obj = DubinsPath([0 0 0], 0, 0, 2); [tau,idx] = obj.s2tau(s); @@ -36,18 +30,18 @@ function testReturnValuesZeroLengthPath(testCase, s) end%fcn function testReturnValuesNonEmptyPath(testCase) - + obj = DubinsPath([0 0 0], [1 0 -1], [pi/4 1 pi/4], 2); s = [-1 -0.2 0 0.1 100 2 1.5 5 9 20]; %#ok<*PROP> [tau,idx] = obj.s2tau(s); - + [tau0,tau1] = obj.domain(); tauSet = interp1([0; obj.cumlengths()], tau0:tau1, s, 'linear','extrap'); verifyEqual(testCase, tau, tauSet, 'AbsTol',2e-14); - verifyEqual(testCase, idx, uint32([1 1 1 1 3 3 2 3 3])); + verifyEqual(testCase, idx, uint32([1 1 1 1 3 3 2 3 3 3])); end%fcn - + function testReturnValuesOutOfBounds(testCase) obj = DubinsPath([0 0 0], [1 0 -1], [2 1 2], 2); diff --git a/xUnitTests/EvalTest.m b/xUnitTests/EvalTest.m index a2b98d3..118d571 100644 --- a/xUnitTests/EvalTest.m +++ b/xUnitTests/EvalTest.m @@ -191,7 +191,62 @@ function testExtrapolationSpline(testCase, obj) testCase.verifyEqual(d, dSet); end%fcn - + + function testExtrapolationDubins(testCase, obj) + + if ~isa(obj, 'DubinsPath') + return + end + + % Evaluate inside and outside of the path's domain + [tau0,tau1] = obj.domain(); + tauEval = [tau0-3 tau0-1 tau0:1:tau1 tau1+1 tau1+3]; + N = numel(tauEval); + + [x,y,t,h,c,d] = obj.eval(tauEval, true); + if obj.isempty() % Nothing to extrapolate + xSet = NaN(N, 1); + ySet = NaN(N, 1); + tSet = NaN(N, 1); + hSet = NaN(N, 1); + cSet = NaN(N, 1); + dSet = NaN(N, 1); + elseif tau1 - tau0 < eps + xSet = NaN(N, 1); + ySet = NaN(N, 1); + tSet = NaN(N, 1); + hSet = NaN(N, 1); + cSet = NaN(N, 1); + dSet = NaN(N, 1); + isInDomain = (tauEval == 0); + xSet(isInDomain) = obj.InitialPos(1); + ySet(isInDomain) = obj.InitialPos(2); + tSet(isInDomain) = 0; + hSet(isInDomain) = obj.InitialAng; + cSet(isInDomain) = obj.SegmentTypes(1)*obj.TurningRadius; + dSet(isInDomain) = 0; + + else % Assume path is straight -> we can use interp1() + [xP,yP,~,hP] = obj.eval(tau0:tau1); + xyhSet = interp1(tau0:tau1, [xP yP hP], tauEval, 'linear','extrap'); + xSet = xyhSet(:,1); + ySet = xyhSet(:,2); + hSet = xyhSet(:,3); + tSet = tauEval(:); + cSet = zeros(numel(tauEval), 1); + dSet = zeros(numel(tauEval), 1); + end + + %%% Checks + testCase.verifyEqual(x, xSet); + testCase.verifyEqual(y, ySet); + testCase.verifyEqual(t, tSet); + testCase.verifyEqual(h, hSet); + testCase.verifyEqual(c, cSet); + testCase.verifyEqual(d, dSet); + + end%fcn + function testReturnValuesDubins(testCase) % Make sure eval() returns the same values irrespective of the path % segments adressed by the path parameter argument. I.e., the diff --git a/xUnitTests/S2TauTest.m b/xUnitTests/S2TauTest.m index 6259cec..fd4328f 100644 --- a/xUnitTests/S2TauTest.m +++ b/xUnitTests/S2TauTest.m @@ -3,7 +3,8 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPathEmpty',PolygonPath(), ... - 'SplinePathEmpty',SplinePath()) + 'SplinePathEmpty',SplinePath(), ... + 'DubinsPathEmpty',DubinsPath()) PathObj = struct(... 'PolygonPathEmpty',PolygonPath(), ... @@ -11,7 +12,10 @@ 'PolygonPathNonEmpty',PolygonPath.xy2Path(0:10, zeros(1,11)), ... 'SplinePathEmpty',SplinePath(), ... 'SplinePathZeroLength',SplinePath([0 0], reshape([1 0; 0 0], [2 1 2])), ... - 'SplinePathNonEmpty',SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], [2 2 2]))) + 'SplinePathNonEmpty',SplinePath([0 1 2], reshape([1 0; 0 0; 1 1; 1 0], [2 2 2])), ... + 'DubinsPathEmpty',DubinsPath(), ... + 'DubinsPathZeroLength',DubinsPath([0 0 0], 0, 0, 2), ... + 'DubinsPathNonEmpty',DubinsPath([0 0 0], [0 1 -1], [2 1 1], 2)) s = struct(... 'emptyColS',zeros(0,1), ... From 50a4453b189b6b9260fbf853f770ffda5dd5f1f3 Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Wed, 20 May 2026 16:24:07 +0200 Subject: [PATCH 09/16] Implement methods clear(), circle() and straight(). --- src/DubinsPath.m | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/src/DubinsPath.m b/src/DubinsPath.m index 068f3b4..aafc920 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -65,7 +65,6 @@ methods - function obj = DubinsPath(startPose, types, lengths, R, isCircuit) %DUBINSPATH Create Dubins path object. % OBJ = DUBINSPATH() creates an empty path. @@ -112,6 +111,11 @@ error('Not implemented!') end%fcn + function obj = clear(obj) + obj.SegmentTypes(:) = []; + obj.SegmentLengths(:) = []; + end%fcn + function c = convertSegmentType2Char(obj) %CONVERTSEGMENTTYPE2CHAR Convert segment type to character. % C = CONVERTSEGMENTTYPE2CHAR(OBJ) converts numeric property @@ -344,12 +348,10 @@ function write2file(obj, fn) assert(isscalar(val) && isnumeric(val) && val > 0); obj.TurningRadius = double(val); end%fcn - end%methods methods (Access = private) - function [xyhc,tau] = evalImpl(obj, tau, extrap) N = obj.numel(); @@ -476,7 +478,6 @@ function write2file(obj, fn) end tau(i0:i1) = taui; end%for - end%fcn function objs = simplify(obj) @@ -503,6 +504,20 @@ function write2file(obj, fn) end methods (Static) + function obj = circle(r, phi01, ~) + P0 = [r*cos(phi01(1)) r*sin(phi01(1)), phi01(1)+pi/2]; + P1 = [r*cos(phi01(2)) r*sin(phi01(2)), phi01(2)+pi/2]; + obj = DubinsPath.connect(P0, P1, r); + end%fcn + + function obj = straight(P0, P1) + dP = P1 - P0; + phi = atan2(dP(2), dP(1)); + + % Since the start/end point have the same heading, the radius + % is arbitrary.. + obj = DubinsPath.connect([P0(1) P0(2) phi], [P1(1) P1(2) phi], 1); + end%fcn function obj = fromStruct(s) end%fcn @@ -545,7 +560,6 @@ function write2file(obj, fn) obj = DubinsPath(C0, T(:, minIdx), L(:, minIdx)*R, R); end%fcn - end%methods end%class From c8a7beed65d769f02336a83583999ebf807d487c Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Tue, 26 May 2026 08:49:18 +0200 Subject: [PATCH 10/16] Refactoring. --- ...ile.xml => lineSegXCircle.m.type.File.xml} | 0 .../lineSegXline.m.type.File.xml | 6 + src/PolygonPath.m | 116 +----------------- src/private/circleLeft.m | 12 -- src/private/circleRight.m | 12 -- src/private/lineSegXCircle.m | 59 +++++++++ src/private/lineSegXline.m | 61 +++++++++ xUnitTests/DubinsPath/ConnectTestDubins.m | 1 - 8 files changed, 132 insertions(+), 135 deletions(-) rename resources/project/Root.type.Files/src.type.File/private.type.File/{circleRight.m.type.File.xml => lineSegXCircle.m.type.File.xml} (100%) create mode 100644 resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXline.m.type.File.xml delete mode 100644 src/private/circleLeft.m delete mode 100644 src/private/circleRight.m create mode 100644 src/private/lineSegXCircle.m create mode 100644 src/private/lineSegXline.m diff --git a/resources/project/Root.type.Files/src.type.File/private.type.File/circleRight.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXCircle.m.type.File.xml similarity index 100% rename from resources/project/Root.type.Files/src.type.File/private.type.File/circleRight.m.type.File.xml rename to resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXCircle.m.type.File.xml diff --git a/resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXline.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXline.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXline.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/src/PolygonPath.m b/src/PolygonPath.m index 7866893..807a383 100644 --- a/src/PolygonPath.m +++ b/src/PolygonPath.m @@ -505,61 +505,12 @@ % >> s = PolygonPath.xy2Path([0 0 -3 -2 -4 -3 1 1], [0 1 2 3 4 5 4 0]); % >> intersectCircle(s, [-1 3], 2, true) - % Brute force approach: check every path segment - idxs = (1:obj.numel())'; - x0 = obj.x - C(1); - dx = diff(x0); - x0(end) = []; - y0 = obj.y - C(2); - dy = diff(y0); - y0(end) = []; - - % Each path segment is written as a line P(t) = P0 + t*(P1-P0) - % from its initial point P0 to its end point P1, where t = - % [0,1]. This results in - % [x0 + t(x1-x0)]^2 + [y0 + t(y1-y0)]^2 = r^2 - % which requires solving a quadratic polynomial - % a*t^2 + b*t + c = 0 - a = dx.^2 + dy.^2; - b = 2*(x0.*dx + y0.*dy); - c = x0.^2 + y0.^2 - r^2; - discriminant = b.^2 - 4*a.*c; - - %%% Case 1: Discriminant > 0 - % We have two solutions from the quadratic equation (per - % segment), i.e. a secant line. - isSecant = (discriminant > 0); - xi = sqrt(discriminant(isSecant)); - tauSecant = 0.5*[... - (-b(isSecant) + xi)./a(isSecant); ... - (-b(isSecant) - xi)./a(isSecant)]; - idxSecant = repmat(idxs(isSecant), [2,1]); - isValidSec = ~((tauSecant < 0) | (tauSecant > 1)); - - %%% Case 2: Discriminant = 0 - % We have one solution from the quadratic equation (per - % segment), i.e. a tangent line. - isTangent = ~((discriminant < 0) | isSecant); % (discriminant == 0) - tauTangent = -0.5*b(isTangent)./a(isTangent); - idxTangent = idxs(isTangent); - isValidTan = ~(tauTangent < 0) & (tauTangent < 1); - - %%% Case 3: Discriminant < 0 - % Quadratic formula has complex solutions -> No intersections - - % Combined set of solutions - tauLoc = [tauSecant(isValidSec); tauTangent(isValidTan)]; - segIdx = [idxSecant(isValidSec); idxTangent(isValidTan)]; - - % Set return values - tau = sort(segIdx - 1 + tauLoc, 'ascend'); - [x,y] = obj.eval(tau); - xy = [x,y]; + [xy,tau] = lineSegXCircle([obj.x obj.y], C, r); errFlag = isempty(tau); - % At most two intersections per path segment! - assert(size(xy, 1) <= (numel(obj.x)-1)*2) - assert(size(xy, 1) == size(tau, 1)) +% % At most two intersections per path segment! +% assert(size(xy, 1) <= (numel(obj.x)-1)*2) +% assert(size(xy, 1) == size(tau, 1)) if (nargin > 3) && doPlot [~,ax] = plot(obj, 'Marker','.'); @@ -575,63 +526,8 @@ function [xy,tau,errFlag] = intersectLine(obj, O, psi, doPlot) - % Shift by line origin O and rotate so that the line is - % horizontal -> We can find intersections by checking where the - % path's y-components equals zero! - R = rotmat2D(psi); - xyPath = [obj.x - O(1), obj.y - O(2)] * R; - xPath = xyPath(:,1); - yPath = xyPath(:,2); - - % Find segment indexes where the paths y-component (A) changes - % sign or (B) equals zero using sign(), which returns 0 only - % for inputs that are exactly equal to zero. We try to catch - % values almost equal to zero via a magic threshold. - signsA = int8(sign(yPath)); - signsB = abs(yPath) <= eps(O(1)); - signsA(signsB) = int8(0); - idxs0 = find([abs(diff(signsA)) > 1; false] | signsB); - idxs0 = min(idxs0, obj.numel()); - - if isempty(idxs0) % No intersection of path/line - xy = zeros(0, 2); - tau = zeros(0,1); - errFlag = true; - else - % End index can not exceed number of path samples since - % indexes were obtained using DIFF! - idxsE = idxs0 + 1; - x0F = [xPath(idxs0), xPath(idxsE)]; - y0Fd = diff([yPath(idxs0), yPath(idxsE)], 1, 2); - x = xPath(idxs0) - yPath(idxs0) .* diff(x0F, 1, 2)./y0Fd; - - % Undo transformation. Due to the above rotation/shift, the - % intersections y-component is zero. Therefore, only the - % x-component needs to be rotated. -% xy = (R * [x';zeros(1,numel(x))] + repmat(O(:), [1,numel(x)]))'; - xy = [R(1,1)*x + O(1), R(2,1)*x + O(2)]; - - % Since we assume linear interpolation between waypoints, - % the local path segment parameter can be computed from x - % or y -% tauLocal = (x - x0F(1))/diff(x0F); - tauLocal = -yPath(idxs0)./y0Fd; - tau = idxs0 - 1 + tauLocal; - errFlag = false; - end%if - -% % Alternative approach using matrix inversion -% Q1 = O(:) + [cos(psi); sin(psi)]; -% tau = zeros(0,1); -% for i = 1:numel(obj) -% P0 = [obj.x(i) obj.y(i)]; -% P1 = [obj.x(i+1) obj.y(i+1)]; -% [~,tauPQ] = lineLineIntersection(P0, P1, O, Q1); -% taui = tauPQ(1); -% if taui >= 0 && taui <= 1 -% tau = [tau; taui + i - 1]; -% end -% end + [xy,tau] = lineSegXline([obj.x obj.y], O, psi); + errFlag = isempty(tau); if (nargin > 3) && doPlot [~,ax] = plot(obj, 'Marker','.', 'MarkerSize',8); diff --git a/src/private/circleLeft.m b/src/private/circleLeft.m deleted file mode 100644 index 48f1fd2..0000000 --- a/src/private/circleLeft.m +++ /dev/null @@ -1,12 +0,0 @@ -function [x,y,c] = circleLeft(r, head) -%UNTITLED Summary of this function goes here -% Detailed explanation goes here - -% We can avoid calculating phi = head - pi/2 by using the identities -% cos(x - pi/2) = sin(x) and -% sin(x - pi/2) = -cos(x) -x = r*sin(head); -y = r*-cos(head); -c = 1/r*ones(size(x)); - -end%fcn diff --git a/src/private/circleRight.m b/src/private/circleRight.m deleted file mode 100644 index b55263f..0000000 --- a/src/private/circleRight.m +++ /dev/null @@ -1,12 +0,0 @@ -function [x,y,c] = circleRight(r, head) -%UNTITLED Summary of this function goes here -% Detailed explanation goes here - -% We can avoid calculating phi = head + pi/2 by using the identities -% cos(x + pi/2) = -sin(x) and -% sin(x + pi/2) = cos(x) -x = r*-sin(head); -y = r*cos(head); -c = -1/r*ones(size(x)); - -end%fcn diff --git a/src/private/lineSegXCircle.m b/src/private/lineSegXCircle.m new file mode 100644 index 0000000..ecbb485 --- /dev/null +++ b/src/private/lineSegXCircle.m @@ -0,0 +1,59 @@ +function [xy,tau] = lineSegXCircle(xyPath, C, r) +%LINESEGXCIRCLE Intersection of a line segment and a circle. +% Detailed explanation goes here + +idxs = (1:size(xyPath,1))'; + +xyPath = bsxfun(@minus, xyPath, C(:)'); +dxy = diff(xyPath, 1, 1); + +% Each path segment is written as a line +% P(t) = P0 + t*(P1-P0) +% from its initial point P0 to its end point P1, where t = [0,1]. Using the +% implicit equation +% x^2 + y^2 = r^2 +% of a circle, we get +% [x0 + t(x1-x0)]^2 + [y0 + t(y1-y0)]^2 = r^2 +% which requires solving a quadratic polynomial +% a*t^2 + b*t + c = 0 +a = sum(dxy.^2, 2); +b = 2*sum(xyPath(1:end-1,:).*dxy, 2); +c = sum(xyPath(1:end-1,:).^2, 2) - r^2; +discriminant = b.^2 - 4*a.*c; + +%%% Case 1: Discriminant > 0 +% We have two solutions from the quadratic equation (per segment), i.e. a +% secant line. +isSecant = (discriminant > 0); +xi = sqrt(discriminant(isSecant)); +tauSecant = 0.5*[... + (-b(isSecant) + xi)./a(isSecant); ... + (-b(isSecant) - xi)./a(isSecant)]; +idxSecant = repmat(idxs(isSecant), [2 1]); +isValidSec = ~((tauSecant < 0) | (tauSecant > 1)); + +%%% Case 2: Discriminant = 0 +% We have one solution from the quadratic equation (per segment), i.e. a +% tangent line. +isTangent = ~((discriminant < 0) | isSecant); % (discriminant == 0) +tauTangent = -0.5*b(isTangent)./a(isTangent); +idxTangent = idxs(isTangent); +isValidTan = ~(tauTangent < 0) & (tauTangent < 1); + +%%% Case 3: Discriminant < 0 +% Quadratic formula has complex solutions -> No intersections + +% Combined set of solutions +tauLoc = [tauSecant(isValidSec); tauTangent(isValidTan)]; +segIdx = [idxSecant(isValidSec); idxTangent(isValidTan)]; + +% Set return values +tau = sort(segIdx - 1 + tauLoc, 'ascend'); +xy = interp1(xyPath, tau + 1); +xy = bsxfun(@plus, xy, C(:)'); + +% At most two intersections per path segment! +assert(size(xy, 1) <= (size(xyPath, 1)-1)*2) +assert(size(xy, 1) == size(tau, 1)) + +end%fcn diff --git a/src/private/lineSegXline.m b/src/private/lineSegXline.m new file mode 100644 index 0000000..017bbd6 --- /dev/null +++ b/src/private/lineSegXline.m @@ -0,0 +1,61 @@ +function [xy,tau] = lineSegXline(xyPath, O, psi) +%LINESEGXLINE Intersection of a line segment and an infinite line. +% Detailed explanation goes here + + +% Shift by line origin O and rotate so that the line is horizontal -> We +% can find intersections by checking where the path's y-components equals +% zero! +R = rotmat2D(psi); +xyPath = bsxfun(@minus, xyPath, O(:)')*R; +xPath = xyPath(:,1); +yPath = xyPath(:,2); + +% Find segment indexes where the paths y-component (A) changes sign or (B) +% equals zero using sign(), which returns 0 only for inputs that are +% exactly equal to zero. We try to catch values almost equal to zero via a +% magic threshold. +signsA = int8(sign(yPath)); +signsB = abs(yPath) <= eps(O(1)); +signsA(signsB) = int8(0); +idxs0 = find([abs(diff(signsA)) > 1; false] | signsB); +idxs0 = min(idxs0, size(xyPath,1) - 1); + +if isempty(idxs0) % No intersection of path/line + xy = zeros(0, 2); + tau = zeros(0,1); +else + % End index can not exceed number of path samples since indexes were + % obtained using DIFF! + idxsE = idxs0 + 1; + x0F = [xPath(idxs0), xPath(idxsE)]; + y0Fd = diff([yPath(idxs0), yPath(idxsE)], 1, 2); + x = xPath(idxs0) - yPath(idxs0) .* diff(x0F, 1, 2)./y0Fd; + + % Undo transformation. Due to the above rotation/shift, the + % intersections y-component is zero. Therefore, only the x-component + % needs to be rotated. +% xy = (R * [x';zeros(1,numel(x))] + repmat(O(:), [1,numel(x)]))'; + xy = [R(1,1)*x + O(1), R(2,1)*x + O(2)]; + + % Since we assume linear interpolation between waypoints, the local + % path segment parameter can be computed from x or y +% tauLocal = (x - x0F(1))/diff(x0F); + tauLocal = -yPath(idxs0)./y0Fd; + tau = idxs0 - 1 + tauLocal; +end%if + +% % Alternative approach using matrix inversion +% Q1 = O(:) + [cos(psi); sin(psi)]; +% tau = zeros(0,1); +% for i = 1:size(xyPath, 1) +% P0 = xyPath(i,:); +% P1 = xyPath(i+1,:); +% [~,tauPQ] = lineLineIntersection(P0, P1, O, Q1); +% taui = tauPQ(1); +% if taui >= 0 && taui <= 1 +% tau = [tau; taui + i - 1]; +% end +% end + +end%fcn diff --git a/xUnitTests/DubinsPath/ConnectTestDubins.m b/xUnitTests/DubinsPath/ConnectTestDubins.m index 6a53516..a6399e5 100644 --- a/xUnitTests/DubinsPath/ConnectTestDubins.m +++ b/xUnitTests/DubinsPath/ConnectTestDubins.m @@ -156,7 +156,6 @@ function testRLR(testCase, ConfigsRLR) testCase.verifyFalse(dub.IsCircuit) end%fcn - end end%class From d2fff6a7d89ed9ea42d0df744d73ba48dff8f94d Mon Sep 17 00:00:00 2001 From: Georg Date: Tue, 26 May 2026 18:48:00 +0200 Subject: [PATCH 11/16] Implement method intersectLine(). --- .../circleLeft.m.type.File.xml | 6 - .../circularArcXline.m.type.File.xml | 6 + .../IntersectLineTestDubins.m.type.File.xml | 6 + runAllTests.m | 4 + src/DubinsPath.m | 88 ++++++++++++-- src/PolygonPath.m | 8 +- src/private/circularArcXline.m | 108 ++++++++++++++++++ src/private/lineSegXCircle.m | 4 - .../DubinsPath/IntersectLineTestDubins.m | 105 +++++++++++++++++ 9 files changed, 313 insertions(+), 22 deletions(-) delete mode 100644 resources/project/Root.type.Files/src.type.File/private.type.File/circleLeft.m.type.File.xml create mode 100644 resources/project/Root.type.Files/src.type.File/private.type.File/circularArcXline.m.type.File.xml create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IntersectLineTestDubins.m.type.File.xml create mode 100644 src/private/circularArcXline.m create mode 100644 xUnitTests/DubinsPath/IntersectLineTestDubins.m diff --git a/resources/project/Root.type.Files/src.type.File/private.type.File/circleLeft.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/circleLeft.m.type.File.xml deleted file mode 100644 index 80b5b16..0000000 --- a/resources/project/Root.type.Files/src.type.File/private.type.File/circleLeft.m.type.File.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - \ No newline at end of file diff --git a/resources/project/Root.type.Files/src.type.File/private.type.File/circularArcXline.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/circularArcXline.m.type.File.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/Root.type.Files/src.type.File/private.type.File/circularArcXline.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IntersectLineTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IntersectLineTestDubins.m.type.File.xml new file mode 100644 index 0000000..378b613 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/IntersectLineTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/runAllTests.m b/runAllTests.m index f529d2f..ca6a00e 100644 --- a/runAllTests.m +++ b/runAllTests.m @@ -26,6 +26,10 @@ testRes = run(testSuite); +if nargout < 1 + disp(testRes) +end + end%fcn diff --git a/src/DubinsPath.m b/src/DubinsPath.m index aafc920..d81277f 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -226,7 +226,56 @@ end%fcn function [xy,tau,errFlag] = intersectLine(obj, O, psi, doPlot) - error('Not implemented!') + + xy = zeros(0,2); + tau = zeros(0,1); + r = obj.TurningRadius; + for i = 1:obj.numel() + [Ax,Ay,~,Ah] = obj.eval(i-1); + [Bx,By,~,Bh] = obj.eval(i); + + sig = double(sign(obj.SegmentTypes(i))); + if sig ~= 0 % Segment is a Circle + phiA = Ah - pi/2; + phiB = Bh - pi/2; + + C = points2CircleCenter([Ax;Ay], [Bx;By], r, sig); + [xyi,~,si] = circularArcXline(C, r, sig*phiA, phiB - phiA, O, psi); + + % Compute normalized path parameter + taui = si/obj.SegmentLengths(i); + + % Append + xy = [xy; xyi]; + tau = [tau; taui + i - 1]; + else + [xyi,taus] = lineSegXline([Ax Ay; Bx By], O, psi); + if ~isempty(taus) + xy = [xy; xyi]; + tau = [tau; taus(1) + i - 1]; + end + end + end + + % % At most two intersections per path segment! + % assert(size(xy, 1) <= (size(xyPath, 1)-1)*2) + % assert(size(xy, 1) == size(tau, 1)) + + if (nargin > 3) && doPlot + [~,ax] = plot(obj, 'Marker','.'); + npState = get(ax, 'NextPlot'); + set(ax, 'NextPlot','add'); + + [r1,r2] = scaleTangentToAxis(xlim(), ylim(), O, psi); + Pstart = [O(1) + r2*cos(psi); O(2) + r2*sin(psi)]; + Pstop = [O(1) + r1*cos(psi); O(2) + r1*sin(psi)]; + h = plot(ax, [Pstart(1) Pstop(1)], [Pstart(2) Pstop(2)], ... + 'Displayname','Line'); + + plot(ax, xy(:,1), xy(:,2), 'kx', 'DisplayName','Intersections') + set(ax, 'NextPlot',npState) + end%if + end%fcn function n = numel(obj) @@ -255,7 +304,7 @@ for i = 1:builtin('numel', obj) obj(i).InitialPos = R*obj(i).InitialPos; obj(i).InitialAng = obj(i).InitialAng + phi; - end%for + end end%fcn @@ -275,7 +324,7 @@ % BUILTIN is supported for code-generation starting with R2017b for i = 1:builtin('numel', obj) obj(i).InitialPos = obj(i).InitialPos + P; - end%for + end end%fcn @@ -395,20 +444,20 @@ function write2file(obj, fn) if typei == obj.LEFT % Linear interpolation from h0 to h1 = h0 + si/R: hi = h0 + si/R*dtau; - [xi,yi,ci] = circleLeft(R, hi); + [xi,yi,ci] = evalCircle(R, hi); % Explicitly calculate the terminal points of the % current segment.. since they may not be included in % the path parameter dtau hEnd = h0 + si/R; - [xT,yT] = circleLeft(R, [h0 hEnd]); + [xT,yT] = evalCircle(R, [h0 hEnd]); elseif typei == obj.RIGHT % Linear interpolation from h0 to h1 = h0 - si/R: hi = h0 - si/R*dtau; - [xi,yi,ci] = circleRight(R, hi); + [xi,yi,ci] = evalCircle(-R, hi); hEnd = h0 - si/R; - [xT,yT] = circleRight(R, [h0 hEnd]); + [xT,yT] = evalCircle(-R, [h0 hEnd]); else % Straight segment % Linear interpolation x0 + (x1-x0)*tau, where x0 = 0 @@ -436,7 +485,7 @@ function write2file(obj, fn) y0 = yT(2) + dy; h0 = hEnd; - xyhc(logIdxi, :) = [xi yi hi ci zeros(size(xi))]; + xyhc(logIdxi,:) = [xi yi hi ci zeros(size(xi))]; end%for % Set return values to NaN outside path domain @@ -565,6 +614,29 @@ function write2file(obj, fn) end%class +function [x,y,c] = evalCircle(r, head) +%EVALCIRCLE Evaluate Dubins circle. + +% We can avoid calculating phi = head - pi/2 by using the identities +% cos(x - pi/2) = sin(x) and +% sin(x - pi/2) = -cos(x) +x = r*sin(head); +y = r*-cos(head); +c = 1/r*ones(size(x)); + +end%fcn + +function C = points2CircleCenter(A, B, r, sign) + +v = B - A; +a = 0.5*hypot(v(1), v(2)); +h = sqrt(r^2 - a^2); +v = v/norm(v); +v = [-v(2); v(1)]; +C = 0.5*(A + B) - double(sign)*h*v; + +end%fcn + function [w,s,l] = dubinsLSL(d, a, b) w = coder.const(uint8([1;0;1])); diff --git a/src/PolygonPath.m b/src/PolygonPath.m index 807a383..0c988f6 100644 --- a/src/PolygonPath.m +++ b/src/PolygonPath.m @@ -508,10 +508,10 @@ [xy,tau] = lineSegXCircle([obj.x obj.y], C, r); errFlag = isempty(tau); -% % At most two intersections per path segment! -% assert(size(xy, 1) <= (numel(obj.x)-1)*2) -% assert(size(xy, 1) == size(tau, 1)) - + % At most two intersections per path segment! + assert(size(xy, 1) <= (numel(obj.x)-1)*2) + assert(size(xy, 1) == size(tau, 1)) + if (nargin > 3) && doPlot [~,ax] = plot(obj, 'Marker','.'); npState = get(ax, 'NextPlot'); diff --git a/src/private/circularArcXline.m b/src/private/circularArcXline.m new file mode 100644 index 0000000..0822735 --- /dev/null +++ b/src/private/circularArcXline.m @@ -0,0 +1,108 @@ +function [xy,phi,s] = circularArcXline(C, r, phi0, dPhi, O, psi) +%CIRCULARARCXLINE Intersection of a circular arc and an infinite line. +% XY = CIRCULARARCXLINE(C,R,PHI0,dPHI,O,PSI) returns the intersection +% points XY between a circular arc at center C and of radius R and an +% infinite line passing through O at an angle PSI. The arc section is +% defined by the start angle PHI0 and the sweep angle DPHI. +% +% [XY,PHI,S] = CIRCULARARCXLINE(___) also returns the angular positions +% PHI in the range (-pi,pi] of the intersections around C and the arc +% length S from the arc start. +% +% Behavior and notes +% - The function solves the circle-line quadratic and filters real +% solutions to those that lie on the specified arc [PHI0, PHI0+dPHI], +% correctly handling wrap-around and both sweep directions. +% - Returned rows are sorted by increasing distance along the arc from +% the start (abs(S) increasing). Near-duplicate intersection points +% are removed (numerical tolerance). +% - If there are no intersections on the arc, XY is 0x2, PHI is 0x1 and +% S is 0x1 empty. +% +% See also LINESEGXCIRCLE. + + +% Direction vector of the line +d = [cos(psi) sin(psi)]; + +% Shift such that origin of the circle is at (0,0) +O0 = O(:)' - C(:)'; + +% Compute coefficients of quadratic equation +% a = sum(d.^2, 2); % Equals 1 +b = 2*sum(O0.*d, 2); +discriminant = b.^2 - 4*(sum(O0.^2, 2) - r^2); + +if discriminant > 0 + xi = sqrt(discriminant); + tau = -0.5*[b - xi; b + xi]; +elseif discriminant < 0 + % No solutions + tau = zeros(0,1); +else + tau = -0.5*b; +end + + +% Intersection points and angles around center +xy = O(:)' + tau*d; +phi = atan2(xy(:,2) - C(2), xy(:,1) - C(1)); % in (-pi,pi] + + + +% Normalize to [0,2pi) +phi_2pi = mod(phi, 2*pi); +phi0_2pi = mod(phi0, 2*pi); + +% Robust membership: forward angular distance from phi0 to tau in [0,2pi) +absSweep = mod(dPhi, 2*pi); +epsAng = 1e-12; + +% if absSweep == 0 +% % Zero-length arc: accept only exact angle match within tol +% isOnArc = abs(min(mod(taus2 - phi0_2pi, 2*pi), mod(phi0_2pi - taus2, 2*pi))) <= epsAng; +% else + if dPhi > 0 + % Increasing-angle sweep: accept forward delta <= absSweep + deltaF = mod(phi_2pi - phi0_2pi, 2*pi); + isOnArc = (deltaF >= -epsAng) & (deltaF <= absSweep + epsAng); + else + % Negative sweep: backward motion; check forward distance from tau to phi0 + deltaF_rev = mod(phi0_2pi - phi_2pi, 2*pi); + isOnArc = (deltaF_rev >= -epsAng) & (deltaF_rev <= absSweep + epsAng); + end +% end + + +phi = phi(isOnArc); +xy = xy(isOnArc,:); + + +% Compute signed angle from phi0 to intersection angle following the sweep +% direction +signedAngles = signedAngleFromTo(phi0, phi, sign(dPhi)); + +% Arc path parameter (arc length along sweep direction) +s = r*signedAngles; + +% Sort by absolute path parameter (distance along arc from start) +[s,idx] = sort(abs(s)); +xy = xy(idx,:); +phi = phi(idx); + +end%fcn + + +function dPhi = signedAngleFromTo(phi0, phi1, signDir) + +dPhi = mod(phi1, 2*pi) - mod(phi0, 2*pi); +dPhi = mod(dPhi + pi, 2*pi) - pi; % (-pi, pi] +if signDir >= 0 + % Convert negative values to equivalent positive angle in [0,2pi) + dPhi(dPhi < 0) = dPhi(dPhi < 0) + 2*pi; +else + % Convert positive values to equivalent negative angle in (-2pi,0] + dPhi(dPhi > 0) = dPhi(dPhi > 0) - 2*pi; +end + +end%fcn diff --git a/src/private/lineSegXCircle.m b/src/private/lineSegXCircle.m index ecbb485..598b27a 100644 --- a/src/private/lineSegXCircle.m +++ b/src/private/lineSegXCircle.m @@ -52,8 +52,4 @@ xy = interp1(xyPath, tau + 1); xy = bsxfun(@plus, xy, C(:)'); -% At most two intersections per path segment! -assert(size(xy, 1) <= (size(xyPath, 1)-1)*2) -assert(size(xy, 1) == size(tau, 1)) - end%fcn diff --git a/xUnitTests/DubinsPath/IntersectLineTestDubins.m b/xUnitTests/DubinsPath/IntersectLineTestDubins.m new file mode 100644 index 0000000..6388637 --- /dev/null +++ b/xUnitTests/DubinsPath/IntersectLineTestDubins.m @@ -0,0 +1,105 @@ +classdef IntersectLineTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + PathObj = {... + DubinsPath([0 0 0], [1 0 -1], [pi 1 pi], 1); + DubinsPath([0 0 pi], [-1 0 1], [pi 1 pi], 1)} + end + + + + methods (Test) + function testNoIntersection(testCase, PathObj) + % Test for no intersections. + + [act,tau] = PathObj.intersectLine([5 0], pi/2, false); + exp = zeros(0,2); + verifyEqual(testCase, act, exp); + verifyEqual(testCase, tau, zeros(0,1)); + end%fcn + + function testSingleIntersectionLine(testCase, PathObj) + % Test for a single intersection with a single line segment. + + x0 = -0.5*sign(cos(PathObj.InitialAng)); + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, false); + xyExp = [x0 2]; + verifyEqual(testCase, xyAct, xyExp, 'AbsTol',3e-16); + verifyEqual(testCase, tauAct, 1.5); + end%fcn + + function testMultipleIntersectionsCircle(testCase, PathObj) + % Test for a two intersections with a arc segment. + + r = PathObj.TurningRadius; + x0 = 0.5*r*sign(cos(PathObj.InitialAng)); + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, false); + xyExp = [x0 x0; [-r r].*sin(pi/3) + r]'; + verifyEqual(testCase, xyAct, xyExp, 'AbsTol',1e-15); + verifyEqual(testCase, tauAct, [1/6; 5/6], 'AbsTol',2e-16); + + end%fcn + + function testIntersectionAllSegments(testCase, PathObj) + % Test intersections with all three segments (L-S-L / R-S-R style) + % Expect that enabling circle intersections returns three points and that + % the single-line intersection (checked in testSingleIntersectionLine) is + % included among them. + + sig = sign(cos(PathObj.InitialAng)); + x0 = 1*sig; + + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2 + sig*atan(3/4), false); + + xyExp = [sig*[0.74787 -0.5 -1.74787]; 0.33616 2 3.66383]'; + verifyEqual(testCase, xyAct, xyExp, 'AbsTol',1e-5); + verifyEqual(testCase, tauAct, [0.26893; 1.5; 2.73107], 'AbsTol',1e-5); + end%fcn + + +% function testIntersectionEndPoint(testCase, Offset, DPhi) +% +% obj0 = PolygonPath.xy2Path([-10 0 2 10] + Offset, [1 0 0 1] + Offset); +% +% % Intersection with end point +% [act,tau] = intersectLine(obj0, [10 0] + Offset, pi/2 + DPhi, false); +% verifyEqual(testCase, act, [10 1] + Offset, 'AbsTol',1e-12); +% verifyEqual(testCase, tau, 3); +% end%fcn + +% function testIntersectionWithWaypoint(testCase, Offset) +% % Test the intersection of a line with a non-terminal waypoint of +% % the path. This situation can cause redundant solutions, i.e. end +% % of segment k and start of segmen i+1. +% +% obj0 = PolygonPath.xy2Path([-1 0 1 2] + Offset, [0 0 1 2] + Offset); +% +% [act,tau] = intersectLine(obj0, [2 0] + Offset, 3*pi/4, false); +% +% verifyEqual(testCase, act, [1 1] + Offset, 'AbsTol',1e-12); +% verifyEqual(testCase, tau, 2); +% end%fcn + +% function testSignReturnsZero(testCase) +% % Test where sign() returns zero. +% +% obj0 = PolygonPath.xy2Path(0:4, 0:4); +% +% [act,tau] = intersectLine(obj0, [0 2], 0, false); +% +% verifyEqual(testCase, act, [2 2]); +% verifyEqual(testCase, tau, 2); +% end%fcn +% +% function testTouchingIntersection(testCase) +% +% obj0 = PolygonPath.xy2Path([-1 1 2], [0.1 pi -exp(1)]); +% +% [act,tau] = intersectLine(obj0, [0 pi], 0, false); +% +% verifyEqual(testCase, act, [1 pi]); +% verifyEqual(testCase, tau, 1); +% end%fcn + end + +end%class From 3e30cc1f51c492f55c5d49ee51f146198faef41a Mon Sep 17 00:00:00 2001 From: Georg <84226458+gnestlinger@users.noreply.github.com> Date: Tue, 26 May 2026 21:40:28 +0200 Subject: [PATCH 12/16] Backwards compatibility tweaks. --- src/DubinsPath.m | 2 +- src/private/circularArcXline.m | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/DubinsPath.m b/src/DubinsPath.m index d81277f..ccc595d 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -440,7 +440,7 @@ function write2file(obj, fn) si = obj.SegmentLengths(i); typei = obj.SegmentTypes(i); - dtau = taui - i + 1; + dtau = taui(:) - i + 1; if typei == obj.LEFT % Linear interpolation from h0 to h1 = h0 + si/R: hi = h0 + si/R*dtau; diff --git a/src/private/circularArcXline.m b/src/private/circularArcXline.m index 0822735..dab21db 100644 --- a/src/private/circularArcXline.m +++ b/src/private/circularArcXline.m @@ -45,7 +45,7 @@ % Intersection points and angles around center -xy = O(:)' + tau*d; +xy = bsxfun(@plus, O(:)', tau*d); phi = atan2(xy(:,2) - C(2), xy(:,1) - C(1)); % in (-pi,pi] From d9f699f40f8b9d7ebe3ee65092742f483f99adb2 Mon Sep 17 00:00:00 2001 From: Georg Date: Sun, 31 May 2026 13:32:59 +0200 Subject: [PATCH 13/16] Implement methods pointProjection(), reverse(), select(), toStruct,(), fromStruct() and getBusDef(). --- .../PointProjectionTestDubins.m.type.File.xml | 6 + src/DubinsPath.m | 143 +++++++++++------- src/private/circularArcXline.m | 19 +-- .../DubinsPath/PointProjectionTestDubins.m | 85 +++++++++++ 4 files changed, 191 insertions(+), 62 deletions(-) create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/PointProjectionTestDubins.m.type.File.xml create mode 100644 xUnitTests/DubinsPath/PointProjectionTestDubins.m diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/PointProjectionTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/PointProjectionTestDubins.m.type.File.xml new file mode 100644 index 0000000..378b613 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/PointProjectionTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/src/DubinsPath.m b/src/DubinsPath.m index ccc595d..03b4d03 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -236,16 +236,13 @@ sig = double(sign(obj.SegmentTypes(i))); if sig ~= 0 % Segment is a Circle - phiA = Ah - pi/2; - phiB = Bh - pi/2; - - C = points2CircleCenter([Ax;Ay], [Bx;By], r, sig); - [xyi,~,si] = circularArcXline(C, r, sig*phiA, phiB - phiA, O, psi); + C = head2CircleCenter([Ax;Ay], sig*r, Ah); + [xyi,~,si] = circularArcXline(C, r, Ah - sig*pi/2, Bh - Ah, O, psi); % Compute normalized path parameter taui = si/obj.SegmentLengths(i); - % Append + % Append xy = [xy; xyi]; tau = [tau; taui + i - 1]; else @@ -256,6 +253,7 @@ end end end + errFlag = isempty(tau); % % At most two intersections per path segment! % assert(size(xy, 1) <= (size(xyPath, 1)-1)*2) @@ -283,7 +281,48 @@ end%fcn function [Q,idx,tau,dphi] = pointProjection(obj, poi, ~, doPlot) - error('Not implemented!') + + r = obj.TurningRadius; + + N = obj.numel(); + Q = zeros(0,2); + idx = zeros(0,1); + tau = zeros(0,1); + for i = 1:N % Loop over path segments + [x01,y01,~,h01] = obj.eval([i-1 i]); + if obj.SegmentTypes(i) == 0 + p = PolygonPath(x01, y01, h01, [0;0]); + [Qi,~,taui] = p.pointProjection(poi); + else + sig = double(obj.SegmentTypes(i)); + C = head2CircleCenter([x01(1);y01(1)], sig*r, h01(1)); + psi = atan2(poi(2) - C(2), poi(1) - C(1)); + [Qi,~,si] = circularArcXline(C, r, h01(1) - sig*pi/2, diff(h01), C, psi); + taui = si/obj.SegmentLengths(i); + end + + Q = [Q; Qi]; + idx = [idx; repmat(i, [numel(taui) 1])]; + tau = [tau; taui + i - 1]; + end + dphi = zeros(numel(idx), 1); + + if (nargin > 3) && doPlot + [~,ax] = plot(obj, 'DisplayName','RefPath'); + npState = get(ax, 'NextPlot'); + set(ax, 'NextPlot','add'); + plot(ax, obj.InitialPos(1), obj.InitialPos(2), 'g.', ... + 'MarkerSize',18, 'DisplayName','Initial point'); + plot(ax, poi(1), poi(2), 'ro', 'DisplayName','PoI') + plot(ax, Q(:,1), Q(:,2), 'kx', 'DisplayName','Q') + legend('-DynamicLegend'); + plot(ax, ... + [Q(:,1)'; repmat([poi(1) NaN], size(Q,1),1)'],... + [Q(:,2)'; repmat([poi(2) NaN], size(Q,1),1)'], 'k:', ... + 'DisplayName','Q-PoI'); + set(ax, 'NextPlot',npState); + end%if + end%fcn function [obj,tau0,tau1] = restrict(obj, tau0, tau1) @@ -291,7 +330,12 @@ end%fcn function obj = reverse(obj) - error('Not implemented!') + [x1,y1,~,h1] = obj.eval(numel(obj)); + obj.InitialPos = [x1; y1]; + obj.InitialAng = h1 + pi; + obj.SegmentTypes = -flip(obj.SegmentTypes); + obj.SegmentLengths = flip(obj.SegmentLengths); + obj.ArcLengths = cumsum(obj.SegmentLengths)'; end%fcn function obj = rotate(obj, phi) @@ -309,7 +353,9 @@ end%fcn function obj = select(obj, idxs) - error('Not implemented!') + obj.SegmentTypes = obj.SegmentTypes(idxs); + obj.SegmentLengths = obj.SegmentLengths(idxs); + obj.ArcLengths = cumsum(obj.SegmentLengths)'; end%fcn function obj = shift(obj, P) @@ -327,33 +373,7 @@ end end%fcn - -% function [tau,idx] = s2tau(obj, s) -% -% if obj.length() < eps % Zero-length path -% tau = nan(size(s)); -% idx = zeros(size(s), 'uint32'); -% if ~obj.isempty() -% theIdx = abs(s) < eps; -% tau(theIdx) = 0; -% idx(theIdx) = 1; -% end -% return -% end -% -% if obj.IsCircuit -% s = mod(s, obj.length()); -% end -% -% S = obj.ArcLengths; -% [~,tmp] = histc(s, [0;S;inf]); %#ok -% idx = min(max(uint32(tmp), 1), numel(S)); -% -% S = [0; S]; -% ds = s - reshape(S(idx), size(s)); -% tau = double(idx-1) + ds./reshape(S(idx+1) - S(idx), size(s)); -% end%fcn - + function [P0,P1] = termPoints(obj) if isempty(obj) @@ -368,15 +388,19 @@ end%fcn function write2file(obj, fn) - %WRITE2FILE Write path to file. - % WRITE2FILE(OBJ,FN) writes waypoints OBJ to file with filename - % FN (specify extension!). - % + %WRITE2FILE Write path to file. + % WRITE2FILE(OBJ,FN) writes waypoints OBJ to file with filename + % FN (specify extension!). + % error('Not implemented!') end%fcn function s = toStruct(obj) - error('Not implemented!') + s = struct(... + 'turningRadius',obj.TurningRadius, ... + 'segmentTypes',obj.SegmentTypes, ... + 'segmentLengths',obj.SegmentLengths, ... + 'initialPose',[obj.InitialPos; obj.InitialAng]); end%fcn %%% Set methods @@ -569,9 +593,29 @@ function write2file(obj, fn) end%fcn function obj = fromStruct(s) + obj = DubinsPath(... + s.initialPose, ... + s.segmentTypes, ... + s.segmentLengths, ... + s.turningRadius); end%fcn - function c = getBusDef() + function c = getBusDef(N) + % GETBUSDEF Return bus information. + % C = GETBUSDEF(N) returns the bus information cell C for a + % DubinsPath of at most N-1 segments. + % + % See also Path2D/getBusDef. + BusName = 'SBus_DubinsPath'; + HeaderFile = ''; + Description = ''; + BusElements = {... + {'turningRadius', 1, 'double', -1, 'real', 'Sample', 'Variable', [], [], 'm', ''},... + {'segmentTypes', N, 'double', -1, 'real', 'Sample', 'Variable', [], [], '', ''},... + {'segmentLengths', N, 'double', -1, 'real', 'Sample', 'Variable', [], [], 'm', ''},... + {'initialPose', 3, 'double', -1, 'real', 'Sample', 'Variable', [], [], 'm/m/rad', ''},... + }; + c = {{BusName,HeaderFile,Description,BusElements}}; end%fcn function obj = connect(C0, C1, R) @@ -610,12 +654,11 @@ function write2file(obj, fn) end%fcn end%methods - end%class function [x,y,c] = evalCircle(r, head) -%EVALCIRCLE Evaluate Dubins circle. +%EVALCIRCLE Evaluate Dubins circle. % We can avoid calculating phi = head - pi/2 by using the identities % cos(x - pi/2) = sin(x) and @@ -626,15 +669,9 @@ function write2file(obj, fn) end%fcn -function C = points2CircleCenter(A, B, r, sign) - -v = B - A; -a = 0.5*hypot(v(1), v(2)); -h = sqrt(r^2 - a^2); -v = v/norm(v); -v = [-v(2); v(1)]; -C = 0.5*(A + B) - double(sign)*h*v; - +function C = head2CircleCenter(P, r, head) +coder.inline('always') +C = P + r*[-sin(head); cos(head)]; end%fcn function [w,s,l] = dubinsLSL(d, a, b) diff --git a/src/private/circularArcXline.m b/src/private/circularArcXline.m index dab21db..0d1f4c4 100644 --- a/src/private/circularArcXline.m +++ b/src/private/circularArcXline.m @@ -22,6 +22,9 @@ % See also LINESEGXCIRCLE. +% Normalize psi to the interval [0, 2*pi) +psi = mod(psi, 2*pi); + % Direction vector of the line d = [cos(psi) sin(psi)]; @@ -43,19 +46,16 @@ tau = -0.5*b; end - % Intersection points and angles around center xy = bsxfun(@plus, O(:)', tau*d); phi = atan2(xy(:,2) - C(2), xy(:,1) - C(1)); % in (-pi,pi] - - % Normalize to [0,2pi) phi_2pi = mod(phi, 2*pi); phi0_2pi = mod(phi0, 2*pi); % Robust membership: forward angular distance from phi0 to tau in [0,2pi) -absSweep = mod(dPhi, 2*pi); +absSweep = min(abs(dPhi), 2*pi); epsAng = 1e-12; % if absSweep == 0 @@ -67,17 +67,18 @@ deltaF = mod(phi_2pi - phi0_2pi, 2*pi); isOnArc = (deltaF >= -epsAng) & (deltaF <= absSweep + epsAng); else - % Negative sweep: backward motion; check forward distance from tau to phi0 - deltaF_rev = mod(phi0_2pi - phi_2pi, 2*pi); - isOnArc = (deltaF_rev >= -epsAng) & (deltaF_rev <= absSweep + epsAng); + % Negative sweep: backward motion; accept forward delta <= absSweep + % Compute forward angular distance from phi to phi0 (i.e. how far + % you'd go if rotating forward from phi to reach phi0). If this + % distance is <= absSweep, then phi lies on the backward sweep. + deltaF = mod(phi0_2pi - phi_2pi, 2*pi); + isOnArc = (deltaF >= -epsAng) & (deltaF <= absSweep + epsAng); end % end - phi = phi(isOnArc); xy = xy(isOnArc,:); - % Compute signed angle from phi0 to intersection angle following the sweep % direction signedAngles = signedAngleFromTo(phi0, phi, sign(dPhi)); diff --git a/xUnitTests/DubinsPath/PointProjectionTestDubins.m b/xUnitTests/DubinsPath/PointProjectionTestDubins.m new file mode 100644 index 0000000..f980e9b --- /dev/null +++ b/xUnitTests/DubinsPath/PointProjectionTestDubins.m @@ -0,0 +1,85 @@ +classdef PointProjectionTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + Dir = struct('Left',1, 'Right',-1) + end + + methods (Test) + + function testUniqueSolutions(testCase) + % Test for a unique solution. + + obj = DubinsPath([0 0 0], [1 0], [pi 3], 2); + POI = [1 4]; + [Q,idx,tau,dphi] = obj.pointProjection(POI, [], false); + verifyEqual(testCase, Q, [2 4]); + verifyEqual(testCase, idx, 2); + verifyEqual(testCase, tau, 1 + 2/3); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testInitalSolution(testCase) + % Test initial point solution. + + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); + + [Q,idx,tau,dphi] = pointProjection(obj, [0 1], [], false); + verifyEqual(testCase, Q, [0 0], 'absTol',2e-15); + verifyEqual(testCase, idx, 1); + verifyEqual(testCase, tau, 0); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testEndSolution(testCase) + % Test end point solution. + + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); + + [Q,idx,tau,dphi] = pointProjection(obj, [4 6], [], false); + verifyEqual(testCase, Q, [4 7]); + verifyEqual(testCase, idx, 3); + verifyEqual(testCase, tau, 3); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testMultipleSolutions(testCase) + obj = DubinsPath([0 0 0], [1 0 -1 0 -1 0], [1.5*pi 2 1.5*pi 1 pi 2], 2); + + [Q,idx,tau,dphi] = pointProjection(obj, [2 5], [], false); + + QSet = [0.9142 3.9142; 0.5614 8.0517; 2 8.2426; 3.0467 8.14; 4.4142 5]; + tauSet = [1.3536; 2.813; 3.5858; 4.2048; 5.6213]; + testCase.verifyEqual(Q, QSet, 'AbsTol',5e-5); + testCase.verifyEqual(idx, (2:6)'); + testCase.verifyEqual(tau, tauSet, 'AbsTol',5e-5); + testCase.verifyEqual(dphi, zeros(size(tauSet))); + end%fcn + + function testNoSolution(testCase) + + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); + [Q,idx,tau,dphi] = pointProjection(obj, [-1 1], [], false); + verifySize(testCase, Q, [0 2]); + verifySize(testCase, idx, [0 1]); + verifySize(testCase, tau, [0 1]); + verifySize(testCase, dphi, [0 1]); + end%fcn + + function testCircuitPath(testCase, Dir) + + % Create a path that is symmetric around (0,0) + obj = DubinsPath([0 Dir*-2.5 0], [Dir 0 Dir 0 Dir], [pi 1 2*pi 1 pi], 2); + assert(obj.IsCircuit) + + [Q,idx,tau,dphi] = pointProjection(obj, [0 0], [], false); + + % TODO: accept/discard repeated solution at initial/end point? + N = 5; + verifySize(testCase, Q, [N 2]); + verifyEqual(testCase, idx, (1:N)'); + verifyEqual(testCase, tau, [0; 1.5; 2.5; 3.5; 5]); + verifyEqual(testCase, dphi, zeros(N,1)); + end%fcn + end + +end%class From 88f44ebe44f07de3bc7556311d7e39b19c7fc0c7 Mon Sep 17 00:00:00 2001 From: Georg Date: Tue, 9 Jun 2026 21:59:26 +0200 Subject: [PATCH 14/16] Implement method cart2frenet(). --- .../Cart2FrenetTestDubins.m.type.File.xml | 6 + src/DubinsPath.m | 53 +++++++- xUnitTests/DubinsPath/Cart2FrenetTestDubins.m | 117 ++++++++++++++++++ .../DubinsPath/PointProjectionTestDubins.m | 3 +- 4 files changed, 175 insertions(+), 4 deletions(-) create mode 100644 resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Cart2FrenetTestDubins.m.type.File.xml create mode 100644 xUnitTests/DubinsPath/Cart2FrenetTestDubins.m diff --git a/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Cart2FrenetTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Cart2FrenetTestDubins.m.type.File.xml new file mode 100644 index 0000000..d8fadf3 --- /dev/null +++ b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/Cart2FrenetTestDubins.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/src/DubinsPath.m b/src/DubinsPath.m index 03b4d03..65ad19b 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -108,7 +108,39 @@ end%fcn function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, ~, doPlot) - error('Not implemented!') + + if nargin < 4 + doPlot = false; + end + + [Q,idx,tau,dphi] = obj.pointProjection(xy, [], doPlot); + if isempty(Q) % Find the break point closest to point of interest + [x,y] = obj.eval(0:numel(obj)); + [~,minIdx] = min(hypot(x - xy(1), y - xy(2))); + Q = [x(minIdx) y(minIdx)]; + tau = minIdx - 1; + idx = min(minIdx, numel(obj)); + end + + % Get orientation vector at Q + [~,~,~,phi] = obj.eval(tau); + u = Q*0 + [cos(phi) sin(phi)]; + + % Get sign via z-component of cross product U x (Q-XY) + qp = bsxfun(@minus, Q, xy(:)'); + signD = sign(crossz(u, qp)); + + sd = [obj.idxTau2s(idx, tau), ... + signD.*hypot(qp(:,1), qp(:,2))]; + + if isempty(dphi) + ux = u(:,1); + uy = u(:,2); + dx = qp(:,1); + dy = qp(:,2); + dphi = abs(pi/2 - abs(atan2(ux.*dy - uy.*dx, ux.*dx + uy.*dy))); + end + end%fcn function obj = clear(obj) @@ -223,6 +255,8 @@ function [xy,tau,errFlag] = intersectCircle(obj, C, r, doPlot) error('Not implemented!') + + % See https://mathworld.wolfram.com/Circle-CircleIntersection.html end%fcn function [xy,tau,errFlag] = intersectLine(obj, O, psi, doPlot) @@ -308,7 +342,11 @@ dphi = zeros(numel(idx), 1); if (nargin > 3) && doPlot - [~,ax] = plot(obj, 'DisplayName','RefPath'); + [~,breakIdx] = obj.sampleTau(100); + [~,ax] = plot(obj, 'DisplayName','RefPath', ... + 'Marker','.', ... + 'MarkerSize',10, ... + 'MarkerIndices',breakIdx); npState = get(ax, 'NextPlot'); set(ax, 'NextPlot','add'); plot(ax, obj.InitialPos(1), obj.InitialPos(2), 'g.', ... @@ -522,7 +560,14 @@ function write2file(obj, fn) end%fcn - function tau = sampleTau(obj, M) + function s = idxTau2s(obj, idx, tau) + %IDXTAU2S Lengths from path segment IDX and path parameter TAU. + + stmp = [0; obj.ArcLengths]; + s = stmp(idx) + obj.SegmentLengths(idx)'.*(tau - idx + 1); + end%fcn + + function [tau,breakIdx] = sampleTau(obj, M) % M samples per L/R segment, 1 sample per S segment and 1 % additional sample for the final segment of any type @@ -532,6 +577,7 @@ function write2file(obj, fn) Ns = sum((types == obj.STRAIGHT) & (lengths > 0)); Nlr = Nnz - Ns; tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); + breakIdx = coder.nullcopy(zeros(numel(obj), 1)); i1 = 1; for i = 1:obj.numel() @@ -550,6 +596,7 @@ function write2file(obj, fn) taui = linspace(tau0, tau0 + 1, M+1)'; end tau(i0:i1) = taui; + breakIdx(i) = i0; end%for end%fcn diff --git a/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m new file mode 100644 index 0000000..814d71c --- /dev/null +++ b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m @@ -0,0 +1,117 @@ +classdef Cart2FrenetTestDubins < matlab.unittest.TestCase + + methods (Test) + function testUniqueSolutions(testCase) + + r = 2; + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 4], [], false); + verifyEqual(testCase, sd(1), r*pi/2 + 2, 'AbsTol',1e-4); + verifyEqual(testCase, sd(2), 1); + verifyEqual(testCase, Q, [2 4]); + verifyEqual(testCase, idx, 2); + verifyEqual(testCase, tau, 1+2/3, 'AbsTol',1e-16); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testInitialPointSolution(testCase) + % Check for initial point solution + + r = 2; + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([0 -1], [], false); + verifyEqual(testCase, sd(:,1), 0); + verifyEqual(testCase, sd(:,2), 1); + verifyEqual(testCase, Q, [0 0], 'AbsTol',4e-16); + verifyEqual(testCase, idx, 1); + verifyEqual(testCase, tau, 0); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testEndPointSolution(testCase) + % Check for end point solution + + r = 2; + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([4 8], [], false); + verifyEqual(testCase, sd(:,1), r*pi + 3, 'AbsTol',2e-5); + verifyEqual(testCase, sd(:,2), -1); + verifyEqual(testCase, Q, [4 7]); + verifyEqual(testCase, idx, 3); + verifyEqual(testCase, tau, 3); + verifyEqual(testCase, dphi, 0); + end%fcn + + function testMultipleSolutions(testCase) + + r = 2; + obj = DubinsPath([0 0 0], [1 0 -1 0 -1 0], [r*0.75*pi 1 r*0.75*pi 1 pi 1], r); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([2.5 5], [], false); + + verifyEqual(testCase, sd, [... + 5.065942 1.889087; + 9.193818 2.655892; + 10.80345 2.535533; + 13.14361 2.820265; + 15.10190 2.621320], 'AbsTol',1e-5); + verifyEqual(testCase, Q, [... + 1.164213 3.664213; ... + 0.966619 7.168527; ... + 2.500000 7.535533; ... + 4.636245 6.841291; ... + 5.121320 5.000000], 'AbsTol',1e-6); + verifyEqual(testCase, idx, [2 3 4 5 6]'); + verifyEqual(testCase, tau, [1.353553 2.738782 3.378679 4.547122 5.535533]', 'AbsTol',1e-5); + verifyEqual(testCase, dphi, zeros(5,1)); + end%fcn + + function testFallbackInitialPoint(testCase) + % Fallback solution at initial point + + r = 2; + obj = DubinsPath([0 0 0], [1 0 -1], [r*0.75*pi 1 r*0.75*pi], r); + P0 = obj.termPoints(); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([-1 -1], [], false); + verifyEqual(testCase, sd, [0 sqrt(2)]); + verifyEqual(testCase, Q, P0(:)'); + verifyEqual(testCase, idx, 1); + verifyEqual(testCase, tau, 0); + verifyEqual(testCase, dphi, pi/4); + end%fcn + + function testFallbackEndPoint(testCase) + % Fallback solution at end point + + r = 2; + + % Adjust length of line such that y-component of endpoint is + % integer valued + dPhiCircle = 0.75*pi; + dyCircle = r*sin(dPhiCircle - pi/2); + dyLine = ceil(2*dyCircle) - 2*dyCircle; + lLine = dyLine/sin(dPhiCircle); + + obj = DubinsPath([0 0 0], [1 0 -1], [r*dPhiCircle lLine r*dPhiCircle], r); + [~,P1] = obj.termPoints(); + S = length(obj); + + [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 7], [], false); + verifyEqual(testCase, sd, [S 0]); + verifyEqual(testCase, Q, P1(:)'); + verifyEqual(testCase, idx, 3); + verifyEqual(testCase, tau, 3); + verifyEqual(testCase, dphi, pi/2); + end%fcn + + % function testFallbackNonTerminalPoint(testCase) + % % Fallback non-terminal point + % % TODO + % end%fcn + end + +end%class diff --git a/xUnitTests/DubinsPath/PointProjectionTestDubins.m b/xUnitTests/DubinsPath/PointProjectionTestDubins.m index f980e9b..83b7720 100644 --- a/xUnitTests/DubinsPath/PointProjectionTestDubins.m +++ b/xUnitTests/DubinsPath/PointProjectionTestDubins.m @@ -4,8 +4,9 @@ Dir = struct('Left',1, 'Right',-1) end + + methods (Test) - function testUniqueSolutions(testCase) % Test for a unique solution. From 33e58b9722ef506a7196a83884c1580178ec68f1 Mon Sep 17 00:00:00 2001 From: Georg Date: Thu, 11 Jun 2026 22:00:31 +0200 Subject: [PATCH 15/16] Draw the paths break points. --- src/DubinsPath.m | 36 ++++++++++++++++-------------------- src/Path2D.m | 31 +++++++++++++++++++++++++------ src/PolygonPath.m | 4 ++++ src/SplinePath.m | 4 ++++ 4 files changed, 49 insertions(+), 26 deletions(-) diff --git a/src/DubinsPath.m b/src/DubinsPath.m index 65ad19b..d7260fd 100644 --- a/src/DubinsPath.m +++ b/src/DubinsPath.m @@ -107,6 +107,10 @@ error('Not implemented!') end%fcn + function b = breaks(obj) + b = 0:numel(obj.SegmentLengths); + end%fcn + function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, ~, doPlot) if nargin < 4 @@ -275,17 +279,12 @@ % Compute normalized path parameter taui = si/obj.SegmentLengths(i); - - % Append - xy = [xy; xyi]; - tau = [tau; taui + i - 1]; else - [xyi,taus] = lineSegXline([Ax Ay; Bx By], O, psi); - if ~isempty(taus) - xy = [xy; xyi]; - tau = [tau; taus(1) + i - 1]; - end + [xyi,taui] = lineSegXline([Ax Ay; Bx By], O, psi); end + % Append + xy = [xy; xyi]; %#ok + tau = [tau; taui + i - 1]; %#ok end errFlag = isempty(tau); @@ -301,7 +300,7 @@ [r1,r2] = scaleTangentToAxis(xlim(), ylim(), O, psi); Pstart = [O(1) + r2*cos(psi); O(2) + r2*sin(psi)]; Pstop = [O(1) + r1*cos(psi); O(2) + r1*sin(psi)]; - h = plot(ax, [Pstart(1) Pstop(1)], [Pstart(2) Pstop(2)], ... + plot(ax, [Pstart(1) Pstop(1)], [Pstart(2) Pstop(2)], ... 'Displayname','Line'); plot(ax, xy(:,1), xy(:,2), 'kx', 'DisplayName','Intersections') @@ -335,18 +334,14 @@ taui = si/obj.SegmentLengths(i); end - Q = [Q; Qi]; - idx = [idx; repmat(i, [numel(taui) 1])]; - tau = [tau; taui + i - 1]; + Q = [Q; Qi]; %#ok + idx = [idx; repmat(i, [numel(taui) 1])]; %#ok + tau = [tau; taui + i - 1]; %#ok end dphi = zeros(numel(idx), 1); if (nargin > 3) && doPlot - [~,breakIdx] = obj.sampleTau(100); - [~,ax] = plot(obj, 'DisplayName','RefPath', ... - 'Marker','.', ... - 'MarkerSize',10, ... - 'MarkerIndices',breakIdx); + [~,ax] = plot(obj, 'DisplayName','RefPath'); npState = get(ax, 'NextPlot'); set(ax, 'NextPlot','add'); plot(ax, obj.InitialPos(1), obj.InitialPos(2), 'g.', ... @@ -577,9 +572,10 @@ function write2file(obj, fn) Ns = sum((types == obj.STRAIGHT) & (lengths > 0)); Nlr = Nnz - Ns; tau = coder.nullcopy(zeros(Nlr*M + Ns*1 + 1, 1)); - breakIdx = coder.nullcopy(zeros(numel(obj), 1)); + breakIdx = coder.nullcopy(zeros(numel(obj) + 1, 1)); i1 = 1; + breakIdx(1) = 1; for i = 1:obj.numel() si = lengths(i); if si < eps @@ -596,7 +592,7 @@ function write2file(obj, fn) taui = linspace(tau0, tau0 + 1, M+1)'; end tau(i0:i1) = taui; - breakIdx(i) = i0; + breakIdx(i+1) = i1; end%for end%fcn diff --git a/src/Path2D.m b/src/Path2D.m index 3c8efcf..6757ee6 100644 --- a/src/Path2D.m +++ b/src/Path2D.m @@ -24,6 +24,7 @@ % shift - Shift path. % % Path2D path operations: +% breaks - The paths break points. % cart2frenet - Convert cartesian point to frenet coordinates. % cumlengths - Cumulative path segment lengths. % domain - Domain of the path. @@ -371,13 +372,12 @@ hr = h; axr = ax; end - end%fcn function [hr,axr] = plotG2(varargin) %PLOTG2 Plot path, heading and curvature. % - % For syntax see also PATH2D/PLOT. + % For syntax see also PATH2D/PLOT. [ax,obj,dtau,opts] = parsePlotInputs(varargin{:}); @@ -594,8 +594,8 @@ % PLOTXY(AXH,OBJ,TAU,VARARGIN) plots path OBJ into axes AXH % evaluated at TAU applying line specifications via VARARGIN. % - % [H,AXH,TAU] = PLOTXY(___) return line handles H, axes handle H and path - % parameter TAU. + % [H,AXH,TAU] = PLOTXY(___) return line handles H, axes handle H + % and path parameter TAU. % % NOTE: This method supports non-scalar inputs OBJ! @@ -605,7 +605,14 @@ end%if npState = get(axh, 'NextPlot'); - isDisplayNameProvided = any(strcmp('DisplayName', varargin)); + % Extract name-value pairs from plot options + if mod(numel(varargin), 2) > 0 + nvOpts = varargin(2:end); + else + nvOpts = varargin; + end + + isDisplayNameProvided = any(strcmp('DisplayName', nvOpts)); % Plot paths N = builtin('numel', obj); @@ -615,6 +622,7 @@ set(axh, 'NextPlot','add'); end%if + % Get the x/y-values obji = obj(i); if isempty(tauIn) || isempty(obji) [x,y,tau] = obji.eval(); @@ -626,7 +634,8 @@ end [x,y] = obji.eval(tau); end - + + % Plot data on axis if isDisplayNameProvided hi = plot(axh, x, y, varargin{:}); else @@ -637,6 +646,12 @@ end hi = plot(axh, x, y, varargin{:}, 'DisplayName',name); end%if + + % When running R2016b or newer, show break points + if ~verLessThan('matlab','9.1') + idxs = find(ismember(tau, obj.breaks())); + set(hi, 'MarkerIndices',idxs, 'Marker','.', nvOpts{:}); + end if ~isempty(hi) h(i) = hi; end @@ -672,6 +687,10 @@ % the given order creating path OBJ. obj = append(obj0, varargin) + % BREAKS Get break points. + % B = BREAKS(OBJ) returns the breaks B of the path OBJ. + b = breaks(obj) + % CART2FRENET Cartesian point to frenet with respect to path. % SD = CART2FRENET(OBJ,XY,PHIMAX) converts point of interest XY % in cartesian coordinates to frenet coordinates SD with respect diff --git a/src/PolygonPath.m b/src/PolygonPath.m index 0c988f6..977f34c 100644 --- a/src/PolygonPath.m +++ b/src/PolygonPath.m @@ -96,6 +96,10 @@ [obj.curv; obj2.curv]); end%fcn + function b = breaks(obj) + b = 0:numel(obj.x); + end%fcn + function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, ~, doPlot) % % See also PATH2D/CART2FRENET. diff --git a/src/SplinePath.m b/src/SplinePath.m index 9b9b64b..e91bba6 100644 --- a/src/SplinePath.m +++ b/src/SplinePath.m @@ -100,6 +100,10 @@ end%fcn + function b = breaks(obj) + b = obj.Breaks; + end%fcn + function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, phiMax, doPlot) if nargin < 4 From ac75e8a9a5a607819b8718174e48699a3d1afda5 Mon Sep 17 00:00:00 2001 From: Georg Date: Thu, 11 Jun 2026 22:02:26 +0200 Subject: [PATCH 16/16] Test visualization methods. --- xUnitTests/DubinsPath/Cart2FrenetTestDubins.m | 32 +++++++++------- xUnitTests/DubinsPath/ConnectTestDubins.m | 2 + xUnitTests/DubinsPath/Frenet2CartTestDubins.m | 16 ++++---- .../DubinsPath/IntersectLineTestDubins.m | 37 ++++++++++--------- xUnitTests/DubinsPath/IsCircuitTestDubins.m | 4 +- .../DubinsPath/PointProjectionTestDubins.m | 26 +++++++------ 6 files changed, 66 insertions(+), 51 deletions(-) diff --git a/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m index 814d71c..a2937d0 100644 --- a/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m +++ b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m @@ -1,12 +1,18 @@ classdef Cart2FrenetTestDubins < matlab.unittest.TestCase + properties (TestParameter) + DoPlot = {false} + end + + + methods (Test) - function testUniqueSolutions(testCase) + function testUniqueSolutions(testCase, DoPlot) r = 2; obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 4], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 4], [], DoPlot); verifyEqual(testCase, sd(1), r*pi/2 + 2, 'AbsTol',1e-4); verifyEqual(testCase, sd(2), 1); verifyEqual(testCase, Q, [2 4]); @@ -15,13 +21,13 @@ function testUniqueSolutions(testCase) verifyEqual(testCase, dphi, 0); end%fcn - function testInitialPointSolution(testCase) + function testInitialPointSolution(testCase, DoPlot) % Check for initial point solution r = 2; obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([0 -1], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([0 -1], [], DoPlot); verifyEqual(testCase, sd(:,1), 0); verifyEqual(testCase, sd(:,2), 1); verifyEqual(testCase, Q, [0 0], 'AbsTol',4e-16); @@ -30,13 +36,13 @@ function testInitialPointSolution(testCase) verifyEqual(testCase, dphi, 0); end%fcn - function testEndPointSolution(testCase) + function testEndPointSolution(testCase, DoPlot) % Check for end point solution r = 2; obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], r); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([4 8], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([4 8], [], DoPlot); verifyEqual(testCase, sd(:,1), r*pi + 3, 'AbsTol',2e-5); verifyEqual(testCase, sd(:,2), -1); verifyEqual(testCase, Q, [4 7]); @@ -45,12 +51,12 @@ function testEndPointSolution(testCase) verifyEqual(testCase, dphi, 0); end%fcn - function testMultipleSolutions(testCase) + function testMultipleSolutions(testCase, DoPlot) r = 2; obj = DubinsPath([0 0 0], [1 0 -1 0 -1 0], [r*0.75*pi 1 r*0.75*pi 1 pi 1], r); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([2.5 5], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([2.5 5], [], DoPlot); verifyEqual(testCase, sd, [... 5.065942 1.889087; @@ -69,14 +75,14 @@ function testMultipleSolutions(testCase) verifyEqual(testCase, dphi, zeros(5,1)); end%fcn - function testFallbackInitialPoint(testCase) + function testFallbackInitialPoint(testCase, DoPlot) % Fallback solution at initial point r = 2; obj = DubinsPath([0 0 0], [1 0 -1], [r*0.75*pi 1 r*0.75*pi], r); P0 = obj.termPoints(); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([-1 -1], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([-1 -1], [], DoPlot); verifyEqual(testCase, sd, [0 sqrt(2)]); verifyEqual(testCase, Q, P0(:)'); verifyEqual(testCase, idx, 1); @@ -84,7 +90,7 @@ function testFallbackInitialPoint(testCase) verifyEqual(testCase, dphi, pi/4); end%fcn - function testFallbackEndPoint(testCase) + function testFallbackEndPoint(testCase, DoPlot) % Fallback solution at end point r = 2; @@ -100,7 +106,7 @@ function testFallbackEndPoint(testCase) [~,P1] = obj.termPoints(); S = length(obj); - [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 7], [], false); + [sd,Q,idx,tau,dphi] = obj.cart2frenet([3 7], [], DoPlot); verifyEqual(testCase, sd, [S 0]); verifyEqual(testCase, Q, P1(:)'); verifyEqual(testCase, idx, 3); @@ -108,7 +114,7 @@ function testFallbackEndPoint(testCase) verifyEqual(testCase, dphi, pi/2); end%fcn - % function testFallbackNonTerminalPoint(testCase) + % function testFallbackNonTerminalPoint(testCase, DoPlot) % % Fallback non-terminal point % % TODO % end%fcn diff --git a/xUnitTests/DubinsPath/ConnectTestDubins.m b/xUnitTests/DubinsPath/ConnectTestDubins.m index a6399e5..ee9562f 100644 --- a/xUnitTests/DubinsPath/ConnectTestDubins.m +++ b/xUnitTests/DubinsPath/ConnectTestDubins.m @@ -27,6 +27,8 @@ {[5 2 pi/2], [1 2 -pi/2], 3}} end + + methods (Test, ParameterCombination='sequential') function testRightTurn(testCase, ConfigsRight) diff --git a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m index 855e32e..a7c3634 100644 --- a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m +++ b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m @@ -3,17 +3,19 @@ properties (TestParameter) PathObj = {DubinsPath([0 0 0], [0 1 -1], [1 pi/2 pi/2], 2)} DSet = {-1 +1 3 -4} + + DoPlot = {false} end methods (Test) - function testFrenet2Cart(testCase, PathObj) + function testFrenet2Cart(testCase, PathObj, DoPlot) s = [0 0.5 linspace(1, PathObj.length(), 6)]; d = zeros(size(s)); - [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); + [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', DoPlot); % Test the distance from Q to xy dAct = hypot1Arg(xy - Q); @@ -28,11 +30,11 @@ function testFrenet2Cart(testCase, PathObj) verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); end%fcn - function testFrenet2CartVariedD(testCase, PathObj, DSet) + function testFrenet2CartVariedD(testCase, PathObj, DSet, DoPlot) s = [0 0.5 linspace(1, PathObj.length(), 6)]; d = DSet*ones(size(s)); - [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', false); + [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', DoPlot); % Test the distance from Q to xy dAct = hypot1Arg(xy - Q); @@ -43,17 +45,17 @@ function testFrenet2CartVariedD(testCase, PathObj, DSet) verifyEqual(testCase, tau, [0 0.5 1 1.4 1.8 2.2 2.6 3]'); end%fcn - function testOutOfBound(testCase, PathObj) + function testOutOfBound(testCase, PathObj, DoPlot) % Pre-path/post-path solution - [xy,Q,idx,tau] = PathObj.frenet2cart([-1 1; 5 1], false); + [xy,Q,idx,tau] = PathObj.frenet2cart([-1 1; 5 1], DoPlot); testCase.verifyEqual(xy, [-1 1; 5.076867634387 1.899465155730], 'AbsTol',1e-12); testCase.verifyEqual(Q, [-1 0; 4.660720797840 0.990167728905], 'AbsTol',1e-12); testCase.verifyEqual(idx, uint32([1; 3])); testCase.verifyEqual(tau, [-1; 3.546479089470], 'AbsTol',1e-12); end%fcn - function testCircuit(testCase) + function testCircuit(testCase, DoPlot) % TODO end%fcn diff --git a/xUnitTests/DubinsPath/IntersectLineTestDubins.m b/xUnitTests/DubinsPath/IntersectLineTestDubins.m index 6388637..63b3946 100644 --- a/xUnitTests/DubinsPath/IntersectLineTestDubins.m +++ b/xUnitTests/DubinsPath/IntersectLineTestDubins.m @@ -3,44 +3,47 @@ properties (TestParameter) PathObj = {... DubinsPath([0 0 0], [1 0 -1], [pi 1 pi], 1); - DubinsPath([0 0 pi], [-1 0 1], [pi 1 pi], 1)} + DubinsPath([0 0 pi], [-1 0 1], [pi 1 pi], 1); + } + + DoPlot = {false} end methods (Test) - function testNoIntersection(testCase, PathObj) + function testNoIntersection(testCase, PathObj, DoPlot) % Test for no intersections. - [act,tau] = PathObj.intersectLine([5 0], pi/2, false); + [act,tau] = PathObj.intersectLine([5 0], pi/2, DoPlot); exp = zeros(0,2); verifyEqual(testCase, act, exp); verifyEqual(testCase, tau, zeros(0,1)); end%fcn - function testSingleIntersectionLine(testCase, PathObj) + function testSingleIntersectionLine(testCase, PathObj, DoPlot) % Test for a single intersection with a single line segment. x0 = -0.5*sign(cos(PathObj.InitialAng)); - [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, false); + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, DoPlot); xyExp = [x0 2]; verifyEqual(testCase, xyAct, xyExp, 'AbsTol',3e-16); verifyEqual(testCase, tauAct, 1.5); end%fcn - function testMultipleIntersectionsCircle(testCase, PathObj) + function testMultipleIntersectionsCircle(testCase, PathObj, DoPlot) % Test for a two intersections with a arc segment. r = PathObj.TurningRadius; x0 = 0.5*r*sign(cos(PathObj.InitialAng)); - [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, false); + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2, DoPlot); xyExp = [x0 x0; [-r r].*sin(pi/3) + r]'; verifyEqual(testCase, xyAct, xyExp, 'AbsTol',1e-15); verifyEqual(testCase, tauAct, [1/6; 5/6], 'AbsTol',2e-16); end%fcn - function testIntersectionAllSegments(testCase, PathObj) + function testIntersectionAllSegments(testCase, PathObj, DoPlot) % Test intersections with all three segments (L-S-L / R-S-R style) % Expect that enabling circle intersections returns three points and that % the single-line intersection (checked in testSingleIntersectionLine) is @@ -49,7 +52,7 @@ function testIntersectionAllSegments(testCase, PathObj) sig = sign(cos(PathObj.InitialAng)); x0 = 1*sig; - [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2 + sig*atan(3/4), false); + [xyAct,tauAct] = PathObj.intersectLine([x0 0], pi/2 + sig*atan(3/4), DoPlot); xyExp = [sig*[0.74787 -0.5 -1.74787]; 0.33616 2 3.66383]'; verifyEqual(testCase, xyAct, xyExp, 'AbsTol',1e-5); @@ -57,45 +60,45 @@ function testIntersectionAllSegments(testCase, PathObj) end%fcn -% function testIntersectionEndPoint(testCase, Offset, DPhi) +% function testIntersectionEndPoint(testCase, Offset, DPhi, DoPlot) % % obj0 = PolygonPath.xy2Path([-10 0 2 10] + Offset, [1 0 0 1] + Offset); % % % Intersection with end point -% [act,tau] = intersectLine(obj0, [10 0] + Offset, pi/2 + DPhi, false); +% [act,tau] = intersectLine(obj0, [10 0] + Offset, pi/2 + DPhi, DoPlot); % verifyEqual(testCase, act, [10 1] + Offset, 'AbsTol',1e-12); % verifyEqual(testCase, tau, 3); % end%fcn -% function testIntersectionWithWaypoint(testCase, Offset) +% function testIntersectionWithWaypoint(testCase, Offset, DoPlot) % % Test the intersection of a line with a non-terminal waypoint of % % the path. This situation can cause redundant solutions, i.e. end % % of segment k and start of segmen i+1. % % obj0 = PolygonPath.xy2Path([-1 0 1 2] + Offset, [0 0 1 2] + Offset); % -% [act,tau] = intersectLine(obj0, [2 0] + Offset, 3*pi/4, false); +% [act,tau] = intersectLine(obj0, [2 0] + Offset, 3*pi/4, DoPlot); % % verifyEqual(testCase, act, [1 1] + Offset, 'AbsTol',1e-12); % verifyEqual(testCase, tau, 2); % end%fcn -% function testSignReturnsZero(testCase) +% function testSignReturnsZero(testCase, DoPlot) % % Test where sign() returns zero. % % obj0 = PolygonPath.xy2Path(0:4, 0:4); % -% [act,tau] = intersectLine(obj0, [0 2], 0, false); +% [act,tau] = intersectLine(obj0, [0 2], 0, DoPlot); % % verifyEqual(testCase, act, [2 2]); % verifyEqual(testCase, tau, 2); % end%fcn % -% function testTouchingIntersection(testCase) +% function testTouchingIntersection(testCase, DoPlot) % % obj0 = PolygonPath.xy2Path([-1 1 2], [0.1 pi -exp(1)]); % -% [act,tau] = intersectLine(obj0, [0 pi], 0, false); +% [act,tau] = intersectLine(obj0, [0 pi], 0, DoPlot); % % verifyEqual(testCase, act, [1 pi]); % verifyEqual(testCase, tau, 1); diff --git a/xUnitTests/DubinsPath/IsCircuitTestDubins.m b/xUnitTests/DubinsPath/IsCircuitTestDubins.m index 4075e08..e01693b 100644 --- a/xUnitTests/DubinsPath/IsCircuitTestDubins.m +++ b/xUnitTests/DubinsPath/IsCircuitTestDubins.m @@ -3,8 +3,9 @@ properties (TestParameter) end + + methods (Test) - function testCircuitTrue(testCase) R = 2; d = 3; @@ -18,7 +19,6 @@ function testCircuitFalse(testCase) dub = DubinsPath([0 0 0], [1 0 1 0], [R*pi d R*pi d*0.99], R); verifyFalse(testCase, dub.IsCircuit); end%fcn - end end%class diff --git a/xUnitTests/DubinsPath/PointProjectionTestDubins.m b/xUnitTests/DubinsPath/PointProjectionTestDubins.m index 83b7720..d57bc5f 100644 --- a/xUnitTests/DubinsPath/PointProjectionTestDubins.m +++ b/xUnitTests/DubinsPath/PointProjectionTestDubins.m @@ -2,51 +2,53 @@ properties (TestParameter) Dir = struct('Left',1, 'Right',-1) + + DoPlot = {false} end methods (Test) - function testUniqueSolutions(testCase) + function testUniqueSolutions(testCase, DoPlot) % Test for a unique solution. obj = DubinsPath([0 0 0], [1 0], [pi 3], 2); POI = [1 4]; - [Q,idx,tau,dphi] = obj.pointProjection(POI, [], false); + [Q,idx,tau,dphi] = obj.pointProjection(POI, [], DoPlot); verifyEqual(testCase, Q, [2 4]); verifyEqual(testCase, idx, 2); verifyEqual(testCase, tau, 1 + 2/3); verifyEqual(testCase, dphi, 0); end%fcn - function testInitalSolution(testCase) + function testInitalSolution(testCase, DoPlot) % Test initial point solution. obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); - [Q,idx,tau,dphi] = pointProjection(obj, [0 1], [], false); + [Q,idx,tau,dphi] = pointProjection(obj, [0 1], [], DoPlot); verifyEqual(testCase, Q, [0 0], 'absTol',2e-15); verifyEqual(testCase, idx, 1); verifyEqual(testCase, tau, 0); verifyEqual(testCase, dphi, 0); end%fcn - function testEndSolution(testCase) + function testEndSolution(testCase, DoPlot) % Test end point solution. obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); - [Q,idx,tau,dphi] = pointProjection(obj, [4 6], [], false); + [Q,idx,tau,dphi] = pointProjection(obj, [4 6], [], DoPlot); verifyEqual(testCase, Q, [4 7]); verifyEqual(testCase, idx, 3); verifyEqual(testCase, tau, 3); verifyEqual(testCase, dphi, 0); end%fcn - function testMultipleSolutions(testCase) + function testMultipleSolutions(testCase, DoPlot) obj = DubinsPath([0 0 0], [1 0 -1 0 -1 0], [1.5*pi 2 1.5*pi 1 pi 2], 2); - [Q,idx,tau,dphi] = pointProjection(obj, [2 5], [], false); + [Q,idx,tau,dphi] = pointProjection(obj, [2 5], [], DoPlot); QSet = [0.9142 3.9142; 0.5614 8.0517; 2 8.2426; 3.0467 8.14; 4.4142 5]; tauSet = [1.3536; 2.813; 3.5858; 4.2048; 5.6213]; @@ -56,23 +58,23 @@ function testMultipleSolutions(testCase) testCase.verifyEqual(dphi, zeros(size(tauSet))); end%fcn - function testNoSolution(testCase) + function testNoSolution(testCase, DoPlot) obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); - [Q,idx,tau,dphi] = pointProjection(obj, [-1 1], [], false); + [Q,idx,tau,dphi] = pointProjection(obj, [-1 1], [], DoPlot); verifySize(testCase, Q, [0 2]); verifySize(testCase, idx, [0 1]); verifySize(testCase, tau, [0 1]); verifySize(testCase, dphi, [0 1]); end%fcn - function testCircuitPath(testCase, Dir) + function testCircuitPath(testCase, Dir, DoPlot) % Create a path that is symmetric around (0,0) obj = DubinsPath([0 Dir*-2.5 0], [Dir 0 Dir 0 Dir], [pi 1 2*pi 1 pi], 2); assert(obj.IsCircuit) - [Q,idx,tau,dphi] = pointProjection(obj, [0 0], [], false); + [Q,idx,tau,dphi] = pointProjection(obj, [0 0], [], DoPlot); % TODO: accept/discard repeated solution at initial/end point? N = 5;