Bugs fixed; cosmetics.

- Fix heading/curvature/path length in method circle(). - Test fix. (AppendTest.m) - Method clothoid() returns exact path length. - Cosmetic updates. (clothoidQuad.m)
gnestlinger · Oct 30, 2023 · a7eb3b3 · a7eb3b3
1 parent 0bd56dc
commit a7eb3b3
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/PolygonPath.m b/PolygonPath.m
@@ -855,10 +855,11 @@ function write2file(obj, fn)
                 phi01 = [0; 2*pi];
             end
             t = linspace(phi01(1), phi01(2), N)';
-            obj = PolygonPath(r*cos(t), r*sin(t), t+pi/2, repmat(1/r,N,1));
+            signPhi = sign(phi01(2) - phi01(1));
+            obj = PolygonPath(r*cos(t), r*sin(t), t+signPhi*pi/2, signPhi*repmat(1/r,N,1));
 
             % Set exact path length
-            obj.s = (t - t(1))*r; % r*phi where phi=0,...,2*pi
+            obj.s = abs(t - t(1))*r; % r*phi where phi=0,...,2*pi
         end%fcn
 
         function obj = clothoid(A, curv01, N, MODE)
@@ -886,7 +887,7 @@ function write2file(obj, fn)
             signk = sign(curv1 - curv0);
 
             % Pre-calculation of path length
-            s = signk * A^2 * linspace(curv0, curv1, N);
+            s = signk*A^2*linspace(curv0, curv1, N)';
 
             switch lower(MODE)
                 case {0, 'quad'}
@@ -900,6 +901,7 @@ function write2file(obj, fn)
             head = s.^2/(2*A^2);
             curv = s/A^2;
             obj = PolygonPath(x, y, head, curv);
+            obj.s = s;
 
         end%fcn
 

diff --git a/private/clothoidQuad.m b/private/clothoidQuad.m
@@ -5,19 +5,18 @@
 % 
 
 % Check input arguments
-s = s(:);
 if any(diff(s) == 0)
     error('Input argument S must not contain repeated elements!');
-elseif abs(sum(sign(diff(s)))) ~= length(s)-1
+elseif abs(sum(sign(diff(s(:))))) ~= length(s)-1
     error('Input argument S must be strictly monotonically decreasing/increasing!');
 end%if
 assert(A > 0, 'Clothoid parameter A has to be positive!');
 
 
 % Pre-allocation
-N = length(s);
-x(1,N) = 0;
-y(1,N) = 0;
+N = numel(s);
+x = coder.nullcopy(zeros(size(s)));
+y = coder.nullcopy(zeros(size(s)));
 % xy(2,N) = 0;
 
 % Clothoid integrands

diff --git a/xUnitTests/PolygonPath/AppendTest.m b/xUnitTests/PolygonPath/AppendTest.m
@@ -6,7 +6,7 @@ function testNonDecreasingPathLength(testCase)
             obj = PolygonPath.xy2Path(x, x);
             objA = obj.append(obj);
 
-            testCase.verifyTrue(~any(diff(objA.cumlengths() < 0)))
+            testCase.verifyTrue(~any(diff(objA.cumlengths()) < 0))
         end%fcn
 
     end