Point projection solution at terminal points improved. (PolygonPath.m)

Author: gnestlinger

src/PolygonPath.m

@@ -717,8 +717,8 @@
 
             % To find Q, two conditions must be satisfied: 
             % https://de.wikipedia.org/wiki/Orthogonalprojektion
-            %    (1) Q = P0 + lambda * u, where u := P1-P0
-            %    (2) dot(Q-POI, u) = 0
+            %   (1) Q = P0 + lambda * u, where u := P1-P0
+            %   (2) dot(Q-POI, u) = 0
             % Inserting (1) into (2) yields 
             %   lambda = dot(POI - P0, u)/dot(u, u)
             P0 = [X(1:end-1), Y(1:end-1)];
@@ -732,8 +732,13 @@
             %   compile-time assumption was vector(vector) indexing." for
             %   2-element paths (i.e. scalar lambdas).
             % idx = find((lambdas >= 0) & [lambdas(1:end-1) < 1; lambdas(end) <= 1]);
-            lambdas(end) = lambdas(end) - eps(lambdas(end));
-            idx = find((lambdas >= 0) & (lambdas < 1));
+            if numel(lambdas) > 1
+                lambdas(end) = lambdas(end) - eps(lambdas(end));
+            end
+            idx = find((lambdas(1:end-1) >= 0) & (lambdas(1:end-1) < 1));
+            if (lambdas(end) >= 0) && (lambdas(end) <= 1)
+                idx(end+1) = numel(lambdas);
+            end
 
             % For paths with 2 elements, find can return an array of size
             % 0-by-0 which would raise an error in BSXFUN. Avoid by

xUnitTests/PolygonPath/PointProjectionTest.m

@@ -29,6 +29,36 @@ function testTerminalSolutions(testCase)
             testCase.verifySize(dphi, [1 1]);
         end%fcn
 
+        function testInitalSolution(testCase)
+        % Test initial solutions with a 2-waypoint path
+        
+            P0 = [0 0];
+            P1 = [10 0];
+            obj = PolygonPath.straight(P0, P1);
+            
+            % Initial point solution
+            [Q,idx,tau,dphi] = pointProjection(obj, P0 + [0 1], [], false);
+            verifyEqual(testCase, Q, P0);
+            verifyEqual(testCase, idx, 1);
+            verifyEqual(testCase, tau, 0);
+            verifySize(testCase, dphi, [1 1]);
+        end%fcn
+        
+        function testEndlSolution(testCase)
+        % Test end solutions with a 2-waypoint path
+        
+            P0 = [0 0];
+            P1 = [10 0];
+            obj = PolygonPath.straight(P0, P1);
+            
+            % End point solution
+            [Q,idx,tau,dphi] = pointProjection(obj, P1 + [0 1], [], false);
+            verifyEqual(testCase, Q, P1, 'RelTol',1e-14);
+            verifyEqual(testCase, idx, 1);
+            verifyEqual(testCase, tau, 1, 'RelTol',3e-16);
+            verifySize(testCase, dphi, [1 1]);
+        end%fcn
+        
         function testMultipleSolutions(testCase)
             obj = PolygonPath.circle(3, [-pi pi]/2, 9);
             [Q,idx,tau,dphi] = pointProjection(obj, [0 0], [], false);