Program Listing for File ClothoidCurve.m¶
↰ Return to documentation for file (ClothoidCurve.m)
classdef ClothoidCurve < CurveBase
methods
%> Create a new C++ class instance for the
%> clothoid arc object
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref = ClothoidCurve()
%> ref = ClothoidCurve( x0, y0, theta0, k0, dk, L )
%>
%> \endrst
%>
%> **On input:**
%> - `x0`, `y0`: coordinate of initial point
%> - `theta0`: orientation of the clothoid at initial point
%> - `k0`: curvature of the clothoid at initial point
%> - `dk`: derivative of curvature respect to arclength
%> - `L`: length of curve from initial to final point
%>
%> **On output:**
%>
%> - `ref`: reference handle to the object instance
%>
function self = ClothoidCurve( varargin )
self@CurveBase( 'ClothoidCurveMexWrapper' );
self.objectHandle = ClothoidCurveMexWrapper( 'new', varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function str = is_type( ~ )
str = 'ClothoidCurve';
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Build the clothoid from known parameters
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.build( x0, y0, theta0, k0, dk, L )
%>
%> \endrst
%>
%> **On input:**
%>
%> - `x0`, `y0`: coordinate of initial point
%> - `theta0`: orientation of the clothoid at initial point
%> - `k0`: curvature of the clothoid at initial point
%> - `dk`: derivative of curvature respect to arclength
%> - `L`: length of curve from initial to final point
%>
function build( self, varargin )
ClothoidCurveMexWrapper( 'build', self.objectHandle, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Build the interpolating G1 clothoid arc
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.build_G1( x0, y0, theta0, x1, y1, theta1 )
%>
%> \endrst
%>
%> **On input:**
%>
%> - `x0`, `y0`: coordinate of initial point
%> - `theta0`: orientation of the clothoid at initial point
%> - `x1`, `y1`: coordinate of final point
%> - `theta1`: orientation of the clothoid at final point
%>
function varargout = build_G1( self, x0, y0, theta0, x1, y1, theta1 )
if nargout > 1
[ varargout{1:nargout} ] = ClothoidCurveMexWrapper( ...
'build_G1_D', self.objectHandle, x0, y0, theta0, x1, y1, theta1 ...
);
else
[ varargout{1:nargout} ] = ClothoidCurveMexWrapper( ...
'build_G1', self.objectHandle, x0, y0, theta0, x1, y1, theta1 ...
);
end
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Build the interpolating clothoid arc fixing initial position angle and curvature
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ok = ref.build_forward( x0, y0, theta0, k0, x1, y1 );
%>
%> \endrst
%>
%> **On input:**
%>
%> - `x0`, `y0`: coordinate of initial point
%> - `theta0`: orientation of the clothoid at initial point
%> - `k0`: curvature of the clothoid at initial point
%> - `x1`, `y1`: coordinate of final point
%>
%> **On output:**
%>
%> - `ok`: true iff the interpolation was successful
%>
function ok = build_forward( self, x0, y0, theta0, k0, x1, y1 )
ok = ClothoidCurveMexWrapper( ...
'build_forward', self.objectHandle, x0, y0, theta0, k0, x1, y1 ...
);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Find the closest point to a clothoid curve using the
%> algorithm described in
%>
%> \rst
%> | *E. Bertolazzi, M. Frego*, **Point-Clothoid Distance and Projection Computation**,
%> | SIAM Journal on Scientific Computing, 2019, 41(5).
%> | https://doi.org/10.1137/18M1200439
%> \endrst
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> [ X, Y, S, DST ] = ref.closestPoint( qx, qy );
%>
%> \endrst
%>
%> **On input:**
%>
%> - `qx`, `qx`: coordinate of the point
%>
%> **On output:**
%>
%> - `X`, `Y` : coordinate of the pojected point
%> - `S` : curvilinear coordinate along the clothoid of the projection
%> - `DST` : point clothoid distance
%>
function [ X, Y, S, DST ] = closestPoint( self, qx, qy )
[ X, Y, S, ~, ~, DST ] = ClothoidCurveMexWrapper( ...
'closestPoint', self.objectHandle, qx, qy ...
);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Find the closest point to a clothoid by sampling points
%>
%> \rst
%> .. code-block:: matlab
%>
%> [ X, Y, S, DST ] = ref.closestPointBySample( qx, qy, ds );
%>
%> \endrst
%>
%> **On input:**
%>
%> - `qx`, `qx`: coordinate of the point
%> - `ds`: sampling distance coordinate of the point
%>
%> **On output:**
%>
%> - `X`, `Y` : coordinate of the pojected point
%> - `S` : curvilinear coordinate along the clothoid of the projection
%> - `DST` : point clothoid distance
%>
function [ X, Y, S, DST ] = closestPointBySample( self, qx, qy, ds )
[ X, Y, S, DST ] = ClothoidCurveMexWrapper( ...
'closestPointBySample', self.objectHandle, qx, qy, ds ...
);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Find the distance between a point and a clothoid by sampling
%>
%> \rst
%> .. code-block:: matlab
%>
%> [ DST, S ] = ref.distanceBySample( qx, qy, ds );
%>
%> \endrst
%>
%> **On input:**
%>
%> - `qx`, `qx`: coordinate of the point
%> - `ds`: sampling distance coordinate of the point
%>
%> **On output:**
%>
%> - `S` : curvilinear coordinate along the clothoid of the projection
%> - `DST` : point clothoid distance
%>
function [ DST, S ] = distanceBySample( self, qx, qy, ds )
[ DST, S ] = ClothoidCurveMexWrapper( ...
'distanceBySample', self.objectHandle, qx, qy, ds ...
);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Return curvature derivatve of the clothoid
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> dk = ref.dkappa(s0,L);
%>
%> \endrst
%>
function res = dkappa( self )
res = ClothoidCurveMexWrapper( 'dkappa', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> change the origin of the clothoid curve to curviliear corrdinate `s0`
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.changeOrigin(s0,L);
%>
%> \endrst
%>
%> **On input:**
%>
%> - `s0`: curvilinear coordinate of the origin of the new curve
%> - `L`: nel length of the curve
%>
function changeCurvilinearOrigin( self, s0, L )
ClothoidCurveMexWrapper( ...
'changeCurvilinearOrigin', self.objectHandle, s0, L ...
);
end
%% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%function [ s1, s2 ] = intersect( self, C, varargin )
% [ s1, s2 ] = ClothoidCurveMexWrapper( ...
% 'intersect', self.objectHandle, C.obj_handle(), C.is_type(), varargin{:} ...
% );
%end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> point at infinity
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> [xp,yp,xm,ym] = ref.infinity();
%>
%> \endrst
%>
%> - `xp`, `yp`: point at infinity (positive arc)
%> - `xm`, `ym`: point at infinity (negative arc)
%>
function [xp,yp,xm,ym] = infinity( self )
[xp,yp,xm,ym] = ClothoidCurveMexWrapper( 'infinity', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Get clothoid parameters
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> [ x0, y0, theta0, k0, dk, L ] = ref.getPars();
%>
%> \endrst
%>
%> - `x0`, `y0`: initial point of the clothoid arc
%> - `theta0`: initial angle of the clothoid arc
%> - `kappa0`: initial curvature of the clothoid arc
%> - `dk`: curvature derivative
%> - `L`: length of the clothoid arc
%>
function [ x0, y0, theta0, k0, dk, L ] = getPars( self )
x0 = self.xBegin();
y0 = self.yBegin();
theta0 = self.thetaBegin();
k0 = self.kappaBegin();
dk = self.kappa_D(0);
L = self.length();
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Get an optimized sampling of curviliear coordinates on the clothoid arc
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> S = ref.optimized_sample(npts,max_angle,offs);
%> S = ref.optimized_sample(npts,max_angle,offs,'ISO');
%> S = ref.optimized_sample(npts,max_angle,offs,'SAE');
%>
%> \endrst
%>
%> **Input:**
%>
%> - `npts`: total number of sampling points
%> - `max_angle`: initial angle of the clothoid arc
%> - `offs`: offset of the curve
%> - `ISO`: use ISO orientation of the normal for offset
%> - `SAE`: use SAE orientation of the normal for offset
%>
%> **Output:**
%> - `S`: the vector with the sampled curvilinear coordinates
%>
function s = optimized_sample( self, npts, max_angle, offs )
s = ClothoidCurveMexWrapper( ...
'optimized_sample', self.objectHandle, npts, max_angle, offs ...
);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the clothoid arc
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.plot();
%> ref.plot( npts );
%> ref.plot( npts, 'Color','blue','Linewidth',2);
%>
%> \endrst
%>
%> - `npts`: number of sampling points for plotting
%>
function plot( self, npts, varargin )
if nargin < 2
npts = 1000;
end
S = self.optimized_sample( npts, pi/180, 0 );
[ X, Y ] = ClothoidCurveMexWrapper( 'eval', self.objectHandle, S );
plot( X, Y, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the clothoid arc with offset
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.plot_offs( offs );
%> ref.plot_offs( offs, npts );
%> ref.pplot_offslot( offs, npts, 'Color','blue','Linewidth',2);
%>
%> \endrst
%>
%> - `npts`: number of sampling points for plotting
%> - `offs`: offset of the curve
%>
function plot_offs( self, offs, npts, varargin )
S = self.optimized_sample( npts, pi/180, offs );
[ X, Y ] = ClothoidCurveMexWrapper( 'eval', self.objectHandle, S, offs );
plot( X, Y, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the curvature of the clothoid curve
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.plotCurvature( npts );
%> ref.plotCurvature( npts, 'Color','blue','Linewidth',2);
%>
%> \endrst
%>
%> - `npts`: number of sampling points for plotting
%>
function plotCurvature( self, npts, varargin )
if nargin < 2
npts = 1000;
end
L = ClothoidCurveMexWrapper( 'length', self.objectHandle );
S = 0:L/npts:L;
[ ~, ~, ~, kappa ] = ...
ClothoidCurveMexWrapper( 'evaluate', self.objectHandle, S );
plot( S, kappa, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the angle of the clothoid curve
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.plotAngle( npts );
%> ref.plotAngle( npts, 'Color','blue','Linewidth',2);
%>
%> \endrst
%>
%> - `npts`: number of sampling points for plotting
%>
function plotAngle( self, npts, varargin )
if nargin < 2
npts = 1000;
end
L = ClothoidCurveMexWrapper( 'length', self.objectHandle );
S = 0:L/npts:L;
[ ~, ~, theta, ~ ] = ...
ClothoidCurveMexWrapper( 'evaluate', self.objectHandle, S );
plot( S, theta, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the normal of the clothoid curve
%>
%> **Usage:**
%>
%> \rst
%> .. code-block:: matlab
%>
%> ref.plotNormal( step, len );
%>
%> \endrst
%>
%> - `step`: number of sampling normals
%> - `len`: length of the plotted normal
%>
function plotNormal( self, step, len )
for s=0:step:self.length()
[ x, y, theta, ~ ] = self.evaluate(s);
n = [sin(theta),-cos(theta)];
A = [x,x+len*n(1)];
B = [y,y+len*n(2)];
plot(A,B);
end
end
end
end
