diff --git a/resources/project/Root.type.Files/src.type.File/DubinsPath.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/DubinsPath.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/resources/project/Root.type.Files/src.type.File/DubinsPath.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ 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/src.type.File/private.type.File/lineSegXCircle.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/lineSegXCircle.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/lineSegXCircle.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file 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/resources/project/Root.type.Files/src.type.File/private.type.File/mod2pi.m.type.File.xml b/resources/project/Root.type.Files/src.type.File/private.type.File/mod2pi.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/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/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/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTestDubins.m.type.File.xml b/resources/project/Root.type.Files/xUnitTests.type.File/DubinsPath.type.File/ConnectTestDubins.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/ConnectTestDubins.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/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/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/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/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/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/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/runAllTests.m b/runAllTests.m index 3bf9fab..ea531f9 100644 --- a/runAllTests.m +++ b/runAllTests.m @@ -41,6 +41,10 @@ close all +if nargout < 1 + disp(testRes) +end + end%fcn diff --git a/src/DubinsPath.m b/src/DubinsPath.m new file mode 100644 index 0000000..d7260fd --- /dev/null +++ b/src/DubinsPath.m @@ -0,0 +1,838 @@ +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. +% convertSegmentType2Char - Convert numeric segment type to character. +% +% DubinsPath static methods: +% connect - Create Dubins path from initial/target configuration. +% See superclasses. +% +% See also PATH2D. + + + + 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(1,0, 'int8') + + % SegmentLengths - Segment lengths + SegmentLengths = zeros(1,0) + + % InitialPos - Initial position + InitialPos = zeros(2,1) + + % InitialAng - Initial orientation angle + InitialAng = 0 + 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, isCircuit) + %DUBINSPATH Create Dubins path object. + % OBJ = DUBINSPATH() creates an empty path. + % + % 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. + % + + if nargin < 1 + % Return with default values + return + end + + P0 = startPose(1:2); + obj.InitialPos = P0(:); + obj.InitialAng = startPose(3); + + assert(isequal(numel(types), numel(lengths)), ... + 'DubinsPath:Constructor:numelTypesLengths', ... + 'Number of path property elements must be equal!'); + obj.SegmentTypes = types; + obj.SegmentLengths = lengths; + obj.TurningRadius = R; + + obj.ArcLengths = 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 b = breaks(obj) + b = 0:numel(obj.SegmentLengths); + end%fcn + + function [sd,Q,idx,tau,dphi] = cart2frenet(obj, xy, ~, doPlot) + + 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) + obj.SegmentTypes(:) = []; + obj.SegmentLengths(:) = []; + 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 obj = derivative(obj, n) + error('Not implemeted!') + end%fcn + + function [tauL,tauU] = domain(obj) + + if isempty(obj) + tauL = NaN; + tauU = NaN; + else + tauL = 0; + tauU = sum(obj.SegmentLengths > 0); + end + end%fcn + + function [x,y,tau,head,curv,curvDs] = eval(obj, tau, extrap) + %EVAL Evaluate path at path parameter. + % + + if nargin < 3 + extrap = false; + end + + objs = obj.simplify(); + + % Make sure that tau is defined + if nargin < 2 + if objs.isempty() + tau = zeros(0,1); + else + tau = objs.sampleTau(100); + end + else + tau = tau(:); + end%if + + if objs.isempty() % Empty path: return all NaN's/no extrapolation + N = numel(tau); + tau(:) = NaN; + 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] = objs.evalImpl(tau(:), extrap); + end + + % Unpack data + x = xyhc(:,1); + y = xyhc(:,2); + head = xyhc(:,3); + curv = xyhc(:,4); + curvDs = xyhc(:,5); + + 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) + + % 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 + 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'); + 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) + error('Not implemented!') + + % See https://mathworld.wolfram.com/Circle-CircleIntersection.html + end%fcn + + function [xy,tau,errFlag] = intersectLine(obj, O, psi, doPlot) + + 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 + 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); + else + [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); + + % % 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)]; + 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) + n = numel(obj.SegmentTypes); + end%fcn + + function [Q,idx,tau,dphi] = pointProjection(obj, poi, ~, doPlot) + + 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]; %#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 + [~,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) + error('Not implemented!') + end%fcn + + function obj = reverse(obj) + [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) + + 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 + + end%fcn + + function obj = select(obj, idxs) + obj.SegmentTypes = obj.SegmentTypes(idxs); + obj.SegmentLengths = obj.SegmentLengths(idxs); + obj.ArcLengths = cumsum(obj.SegmentLengths)'; + end%fcn + + function obj = shift(obj, P) + + % 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 + + 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()); + 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) + s = struct(... + 'turningRadius',obj.TurningRadius, ... + 'segmentTypes',obj.SegmentTypes, ... + 'segmentLengths',obj.SegmentLengths, ... + 'initialPose',[obj.InitialPos; obj.InitialAng]); + 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 + + 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 (Access = private) + function [xyhc,tau] = evalImpl(obj, tau, extrap) + + N = obj.numel(); + + 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 == 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); + typei = obj.SegmentTypes(i); + 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] = 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] = 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] = evalCircle(-R, hi); + hEnd = h0 - si/R; + [xT,yT] = evalCircle(-R, [h0 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; + xT = [0 si*cos(h0)]; + yT = [0 si*sin(h0)]; + end%if + + % 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 = xT(2) + dx; + y0 = yT(2) + dy; + h0 = hEnd; + + xyhc(logIdxi,:) = [xi yi hi ci zeros(size(xi))]; + end%for + + % 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; + end + + end%fcn + + 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 + 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)); + breakIdx = coder.nullcopy(zeros(numel(obj) + 1, 1)); + + i1 = 1; + breakIdx(1) = 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; + breakIdx(i+1) = i1; + end%for + 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 + + % 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.IsCircuit); + + end%fcn + 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) + obj = DubinsPath(... + s.initialPose, ... + s.segmentTypes, ... + s.segmentLengths, ... + s.turningRadius); + end%fcn + + 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) + %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; + + % 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); + [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); + + % Find the shortest path + [~,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 [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 = head2CircleCenter(P, r, head) +coder.inline('always') +C = P + r*[-sin(head); cos(head)]; +end%fcn + +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/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 7866893..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. @@ -505,62 +509,13 @@ % >> 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)) - + if (nargin > 3) && doPlot [~,ax] = plot(obj, 'Marker','.'); npState = get(ax, 'NextPlot'); @@ -575,63 +530,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/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 diff --git a/src/private/circularArcXline.m b/src/private/circularArcXline.m new file mode 100644 index 0000000..0d1f4c4 --- /dev/null +++ b/src/private/circularArcXline.m @@ -0,0 +1,109 @@ +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. + + +% Normalize psi to the interval [0, 2*pi) +psi = mod(psi, 2*pi); + +% 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 = 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 = min(abs(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; 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)); + +% 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 new file mode 100644 index 0000000..598b27a --- /dev/null +++ b/src/private/lineSegXCircle.m @@ -0,0 +1,55 @@ +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(:)'); + +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/src/private/mod2pi.m b/src/private/mod2pi.m new file mode 100644 index 0000000..c8615c3 --- /dev/null +++ b/src/private/mod2pi.m @@ -0,0 +1,3 @@ +function x = mod2pi(x) +x = mod(x, 2*pi); +end%fcn diff --git a/xUnitTests/ConstructorEmptyTest.m b/xUnitTests/ConstructorEmptyTest.m index 73075ff..52b5f78 100644 --- a/xUnitTests/ConstructorEmptyTest.m +++ b/xUnitTests/ConstructorEmptyTest.m @@ -3,10 +3,11 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()); + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()); end - + methods (Test) function testConstructorNoArgs(testCase, PathEmpty) 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/DubinsPath/Cart2FrenetTestDubins.m b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m new file mode 100644 index 0000000..a2937d0 --- /dev/null +++ b/xUnitTests/DubinsPath/Cart2FrenetTestDubins.m @@ -0,0 +1,123 @@ +classdef Cart2FrenetTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + DoPlot = {false} + end + + + + methods (Test) + 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], [], DoPlot); + 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, 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], [], DoPlot); + 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, 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], [], DoPlot); + 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, 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], [], DoPlot); + + 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, 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], [], DoPlot); + 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, DoPlot) + % 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], [], DoPlot); + 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, DoPlot) + % % Fallback non-terminal point + % % TODO + % end%fcn + end + +end%class diff --git a/xUnitTests/DubinsPath/ConnectTestDubins.m b/xUnitTests/DubinsPath/ConnectTestDubins.m new file mode 100644 index 0000000..ee9562f --- /dev/null +++ b/xUnitTests/DubinsPath/ConnectTestDubins.m @@ -0,0 +1,185 @@ +classdef ConnectTestDubins < 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], 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'}; + }; + + 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); +% [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([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); +% [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([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 + 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); +% [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([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); +% [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([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); + + % 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); +% [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([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('LineWidth',1, 'MarkerIndices',1, 'Marker','o') + hold off +end + +end%fcn + +function val = mod2Pi(val) +val = mod(val, 2*pi); +end%fcn diff --git a/xUnitTests/DubinsPath/Frenet2CartTestDubins.m b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m new file mode 100644 index 0000000..a7c3634 --- /dev/null +++ b/xUnitTests/DubinsPath/Frenet2CartTestDubins.m @@ -0,0 +1,68 @@ +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} + + DoPlot = {false} + end + + + + methods (Test) + + 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]', DoPlot); + + % Test the distance from Q to xy + dAct = hypot1Arg(xy - Q); + verifyEqual(testCase, dAct, d(:)); + + % 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); + 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, DoPlot) + + s = [0 0.5 linspace(1, PathObj.length(), 6)]; + d = DSet*ones(size(s)); + [xy,Q,idx,tau] = PathObj.frenet2cart([s; d]', DoPlot); + + % 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, DoPlot) + + % Pre-path/post-path solution + [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, DoPlot) + % 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/IntersectLineTestDubins.m b/xUnitTests/DubinsPath/IntersectLineTestDubins.m new file mode 100644 index 0000000..63b3946 --- /dev/null +++ b/xUnitTests/DubinsPath/IntersectLineTestDubins.m @@ -0,0 +1,108 @@ +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); + } + + DoPlot = {false} + end + + + + methods (Test) + function testNoIntersection(testCase, PathObj, DoPlot) + % Test for no intersections. + + [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, 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, DoPlot); + xyExp = [x0 2]; + verifyEqual(testCase, xyAct, xyExp, 'AbsTol',3e-16); + verifyEqual(testCase, tauAct, 1.5); + end%fcn + + 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, 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, 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 + % included among them. + + sig = sign(cos(PathObj.InitialAng)); + x0 = 1*sig; + + [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); + verifyEqual(testCase, tauAct, [0.26893; 1.5; 2.73107], 'AbsTol',1e-5); + end%fcn + + +% 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, DoPlot); +% verifyEqual(testCase, act, [10 1] + Offset, 'AbsTol',1e-12); +% verifyEqual(testCase, tau, 3); +% end%fcn + +% 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, DoPlot); +% +% verifyEqual(testCase, act, [1 1] + Offset, 'AbsTol',1e-12); +% verifyEqual(testCase, tau, 2); +% end%fcn + +% function testSignReturnsZero(testCase, DoPlot) +% % Test where sign() returns zero. +% +% obj0 = PolygonPath.xy2Path(0:4, 0:4); +% +% [act,tau] = intersectLine(obj0, [0 2], 0, DoPlot); +% +% verifyEqual(testCase, act, [2 2]); +% verifyEqual(testCase, tau, 2); +% end%fcn +% +% function testTouchingIntersection(testCase, DoPlot) +% +% obj0 = PolygonPath.xy2Path([-1 1 2], [0.1 pi -exp(1)]); +% +% [act,tau] = intersectLine(obj0, [0 pi], 0, DoPlot); +% +% verifyEqual(testCase, act, [1 pi]); +% verifyEqual(testCase, tau, 1); +% end%fcn + end + +end%class diff --git a/xUnitTests/DubinsPath/IsCircuitTestDubins.m b/xUnitTests/DubinsPath/IsCircuitTestDubins.m new file mode 100644 index 0000000..e01693b --- /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/DubinsPath/PointProjectionTestDubins.m b/xUnitTests/DubinsPath/PointProjectionTestDubins.m new file mode 100644 index 0000000..d57bc5f --- /dev/null +++ b/xUnitTests/DubinsPath/PointProjectionTestDubins.m @@ -0,0 +1,88 @@ +classdef PointProjectionTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + Dir = struct('Left',1, 'Right',-1) + + DoPlot = {false} + end + + + + methods (Test) + 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, [], 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, 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], [], 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, 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], [], DoPlot); + verifyEqual(testCase, Q, [4 7]); + verifyEqual(testCase, idx, 3); + verifyEqual(testCase, tau, 3); + verifyEqual(testCase, dphi, 0); + end%fcn + + 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], [], 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]; + 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, DoPlot) + + obj = DubinsPath([0 0 0], [1 0 -1], [pi 3 pi], 2); + [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, 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], [], DoPlot); + + % 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 diff --git a/xUnitTests/DubinsPath/S2TauTestDubins.m b/xUnitTests/DubinsPath/S2TauTestDubins.m new file mode 100644 index 0000000..cbc98d1 --- /dev/null +++ b/xUnitTests/DubinsPath/S2TauTestDubins.m @@ -0,0 +1,36 @@ +classdef S2TauTestDubins < matlab.unittest.TestCase + + properties (TestParameter) + end + + + + methods (Test) + 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 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 diff --git a/xUnitTests/EvalTest.m b/xUnitTests/EvalTest.m index 20b2452..118d571 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), ... @@ -17,7 +20,7 @@ 'nd', randn(10,3,4)*10); end - + methods (Test) function testReturnSizeNoTau(testCase, obj) @@ -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]'); @@ -186,6 +192,79 @@ function testExtrapolationSpline(testCase, obj) 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 + % (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 end%class diff --git a/xUnitTests/IsEmptyTest.m b/xUnitTests/IsEmptyTest.m index dda33a4..1b26898 100644 --- a/xUnitTests/IsEmptyTest.m +++ b/xUnitTests/IsEmptyTest.m @@ -3,11 +3,13 @@ properties (TestParameter) PathEmpty = struct(... 'PolygonPath', PolygonPath(), ... - 'SplinePath', SplinePath()) + 'SplinePath', SplinePath(), ... + 'DubinsPath', DubinsPath()) PathNonEmpty = 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 diff --git a/xUnitTests/LengthTest.m b/xUnitTests/LengthTest.m index 53d825c..48b5263 100644 --- a/xUnitTests/LengthTest.m +++ b/xUnitTests/LengthTest.m @@ -8,7 +8,8 @@ 'PolygonPathUnitLength', PolygonPath.xy2Path(0:1, zeros(1,2)), ... 'SplinePathEmpty', SplinePath(), ... 'SplinePathZeroLength', SplinePath([0 0], reshape([1 1; 1 2], [2 1 2])), ... - 'SplinePathUnitLength', SplinePath([0 1], reshape([1 0; 0 0], [2 1 2]))); + 'SplinePathUnitLength', SplinePath([0 1], reshape([1 0; 0 0], [2 1 2])), ... + 'DubinsPathEmpty', DubinsPath()); % Define sizes for path parameter argument(s) to test TauSz = struct(...% 'Empty',{{}}, ... @@ -24,6 +25,7 @@ {SplinePath(-2:1, ... cat(3, [0 0 0; 1 1 0], [1 1 1; -2 0 0], [-1 0 1; 1 0 1])), ... (sqrt(5) + asinh(2)/2) + 1}; % via Wolfram alpha ... + {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/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/S2TauTest.m b/xUnitTests/S2TauTest.m index 037f69a..8cf9cd8 100644 --- a/xUnitTests/S2TauTest.m +++ b/xUnitTests/S2TauTest.m @@ -5,7 +5,8 @@ 'PolygonPathEmpty',PolygonPath(), ... 'PolygonPathZeroLength',PolygonPath(1,2,3,4), ... 'SplinePathEmpty',SplinePath(), ... - 'SplinePathZeroLength',SplinePath([0 0], cat(3, [1;2], [0;0]))) + 'SplinePathZeroLength',SplinePath([0 0], cat(3, [1;2], [0;0])), ... + 'DubinsPathEmpty',DubinsPath()) PathObj = struct(... 'PolygonPathEmpty',PolygonPath(), ... @@ -13,7 +14,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(... 'emptyS',[], ... 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 diff --git a/xUnitTests/ShiftTest.m b/xUnitTests/ShiftTest.m index e9d3128..950231f 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 @@ -24,7 +27,7 @@ function testShift(testCase, obj, dP) verifyEqual(testCase, [P0 P1 Q0 Q1], NaN(2,4)) else verifyEqual(testCase, Q0, P0+dP); - verifyEqual(testCase, Q1, P1+dP); + verifyEqual(testCase, Q1, P1+dP, 'AbsTol',4e-15); % Tol added for Dubins path end end%fcn @@ -41,7 +44,7 @@ function testDefaultArg(testCase, obj) verifyEqual(testCase, Q1, [NaN; NaN]); else verifyEqual(testCase, Q0, [0;0]); - verifyEqual(testCase, Q1, P1-P0); + verifyEqual(testCase, Q1, P1-P0, 'AbsTol',5e-16); % Tol added for Dubins path end end%fcn 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