Skip to content

Commit

Permalink
updated ClothoidCurve
Browse files Browse the repository at this point in the history
  • Loading branch information
@ebertolazzi
ebertolazzi committed Oct 31, 2018
1 parent 33432d3 commit 0938a8e
Show file tree
Hide file tree
Showing 41 changed files with 3,080 additions and 2,725 deletions.
14 changes: 8 additions & 6 deletions G2solver.xcodeproj/project.pbxproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -313,6 +313,7 @@
D92EFA922186932B00BF8E18 /* testIntersect2.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = testIntersect2.cc; sourceTree = "<group>"; };
D92EFAAA2187B03600BF8E18 /* testIntersect3 */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = testIntersect3; sourceTree = BUILT_PRODUCTS_DIR; };
D92EFAAB2187B05800BF8E18 /* testIntersect3.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = testIntersect3.cc; sourceTree = "<group>"; };
D92EFAAD2188742200BF8E18 /* ClothoidList.hh */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = ClothoidList.hh; sourceTree = "<group>"; };
D938F4DB1DD3B83000E08A70 /* G2solver */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = G2solver; sourceTree = BUILT_PRODUCTS_DIR; };
D938F4E71DD3B84A00E08A70 /* Clothoid.hh */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = Clothoid.hh; sourceTree = "<group>"; };
D938F4E81DD3B84A00E08A70 /* CubicRootsFlocke.cc */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = CubicRootsFlocke.cc; sourceTree = "<group>"; };
Expand Down Expand Up @@ -483,13 +484,14 @@
D9099D281F600B9400740ECF /* Circle.cc */,
D971F8981F613BC2000E663E /* Biarc.hh */,
D971F8971F613BC2000E663E /* Biarc.cc */,
D9E22EE1216CB48F00C033D2 /* PolyLine.hh */,
D9E22EE0216CB48F00C033D2 /* PolyLine.cc */,
D938F4E71DD3B84A00E08A70 /* Clothoid.hh */,
D9FCD3352075552600553A10 /* Clothoid.cc */,
D9FCD33C207556DD00553A10 /* ClothoidDistance.cc */,
D971F89B1F6169D2000E663E /* ClothoidG2.cc */,
D92EFAAD2188742200BF8E18 /* ClothoidList.hh */,
D9614FD41FB8609F007E5770 /* ClothoidList.cc */,
D971F89B1F6169D2000E663E /* ClothoidG2.cc */,
D9E22EE1216CB48F00C033D2 /* PolyLine.hh */,
D9E22EE0216CB48F00C033D2 /* PolyLine.cc */,
D9FCD33C207556DD00553A10 /* ClothoidDistance.cc */,
D938F4E81DD3B84A00E08A70 /* CubicRootsFlocke.cc */,
D938F4E91DD3B84A00E08A70 /* CubicRootsFlocke.hh */,
D9BD861A1EFC021300C82B74 /* ClothoidAsyPlot.cc */,
Expand Down Expand Up @@ -1119,7 +1121,7 @@
CLANG_ANALYZER_NONNULL = YES;
CLANG_ANALYZER_SECURITY_FLOATLOOPCOUNTER = YES;
CLANG_ANALYZER_SECURITY_INSECUREAPI_STRCPY = YES;
CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
CLANG_CXX_LANGUAGE_STANDARD = "c++0x";
CLANG_CXX_LIBRARY = "libc++";
CLANG_ENABLE_MODULES = YES;
CLANG_ENABLE_OBJC_ARC = YES;
Expand Down Expand Up @@ -1188,7 +1190,7 @@
CLANG_ANALYZER_NONNULL = YES;
CLANG_ANALYZER_SECURITY_FLOATLOOPCOUNTER = YES;
CLANG_ANALYZER_SECURITY_INSECUREAPI_STRCPY = YES;
CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
CLANG_CXX_LANGUAGE_STANDARD = "c++0x";
CLANG_CXX_LIBRARY = "libc++";
CLANG_ENABLE_MODULES = YES;
CLANG_ENABLE_OBJC_ARC = YES;
Expand Down
Binary file modified ...codeproj/project.xcworkspace/xcuserdata/enrico.xcuserdatad/UserInterfaceState.xcuserstate
View file
Binary file not shown.
2 changes: 1 addition & 1 deletion Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ endif

# check if the OS string contains 'Darwin'
ifneq (,$(findstring Darwin, $(OS)))
WARN = -Wall -Wno-sign-compare -Weverything -Wno-global-constructors -Wno-padded -Wno-documentation-unknown-command
WARN = -Wall -Weverything -Wno-sign-compare -Wno-global-constructors -Wno-padded -Wno-documentation-unknown-command
LIBS = -L./lib -lClothoids
CXXFLAGS = $(WARN) -O3 -fPIC
AR = libtool -static -o
Expand Down
14 changes: 3 additions & 11 deletions matlab/CircleArc.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,20 +123,12 @@ function plotPolygon( self, varargin )
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function plotTriangles( self, varargin )
arc = self.to_nurbs();
if nargin > 1
[p1,p2,p3] = self.bbTriangles(varargin{1});
else
[p1,p2,p3] = self.bbTriangles();
end
[p1,p2,p3] = self.bbTriangles();
for k=1:size(p1,2)
x = [ p1(1,k), p2(1,k), p3(1,k), p1(1,k) ];
y = [ p1(2,k), p2(2,k), p3(2,k), p1(2,k) ];
if nargin > 2
plot( x, y, varargin{2:end});
else
plot( x, y );
end
fill( x, y, 'red','FaceAlpha', 0.5 );
%plot( x, y, varargin{:});
end
end
end
Expand Down
208 changes: 10 additions & 198 deletions matlab/ClothoidCurve.m
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,4 @@
classdef ClothoidCurve < handle
%% MATLAB class wrapper for the underlying C++ class
properties (SetAccess = private, Hidden = true)
objectHandle; % Handle to the underlying C++ class instance
end

classdef ClothoidCurve < CurveBase
methods
function self = ClothoidCurve( varargin )
%% Create a new C++ class instance for the clothoid arc object
Expand All @@ -20,26 +15,14 @@
%
% On output:
% ref: reference handle to the object instance
self@CurveBase( 'ClothoidCurveMexWrapper' );
self.objectHandle = ClothoidCurveMexWrapper( 'new', varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function delete( self )
%% Destroy the C++ class instance
ClothoidCurveMexWrapper( 'delete', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function obj = obj_handle( self )
obj = self.objectHandle ;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function str = is_type( ~ )
str = 'ClothoidCurve' ;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function copy( self, C )
ClothoidCurveMexWrapper('copy', self.objectHandle, C.obj_handle() );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function build( self, varargin )
%
% Build the clothoid from known parameters
Expand Down Expand Up @@ -102,63 +85,12 @@ function build( self, varargin )
x0, y0, theta0, k0, x1, y1 );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function varargout = evaluate( self, s )
% evaluate the curve at curvilinear abscissa `s`
%
% Usage:
% [x,y] = ref.eval( s )
% [x,y,theta,kappa] = ref.eval( s )
%
% On input:
% s: curvilinear coordinates where to evaluate the curve
% (scalar or vector)
%
% On output:
% x, y: coordinates of the curve
% theta: orientation of the curve
% kappa: curvature of the curve
%
[ varargout{1:nargout} ] = ...
ClothoidCurveMexWrapper( 'evaluate', self.objectHandle, s );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function varargout = eval( self, s, varargin )
[ varargout{1:nargout} ] = ...
ClothoidCurveMexWrapper( 'eval', ...
self.objectHandle, s, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function varargout = eval_D( self, s, varargin )
[ varargout{1:nargout} ] = ...
ClothoidCurveMexWrapper( 'eval_D', ...
self.objectHandle, s, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function varargout = eval_DD( self, s, varargin )
[ varargout{1:nargout} ] = ...
ClothoidCurveMexWrapper( 'eval_DD', ...
self.objectHandle, s, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function varargout = eval_DDD( self, s, varargin )
[ varargout{1:nargout} ] = ...
ClothoidCurveMexWrapper( 'eval_DDD', ...
self.objectHandle, s, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ X, Y, S, DST ] = closestPoint( self, qx, qy )
[ X, Y, S, DST ] = ...
ClothoidCurveMexWrapper( 'closestPoint', ...
self.objectHandle, qx, qy );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ DST, S ] = distance( self, varargin )
% eval the angle of the circle curve at curvilinear abscissa `s`
[ DST, S ] = ...
ClothoidCurveMexWrapper( 'distance', ...
self.objectHandle, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ X, Y, S, DST ] = closestPointBySample( self, qx, qy, ds )
[ X, Y, S, DST ] = ...
ClothoidCurveMexWrapper( 'closestPointBySample', ...
Expand All @@ -171,38 +103,6 @@ function build( self, varargin )
self.objectHandle, qx, qy, ds );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ x, y ] = xyBegin( self )
[ x, y ] = ClothoidCurveMexWrapper( 'xyBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ x, y ] = xyEnd( self )
[ x, y ] = ClothoidCurveMexWrapper( 'xyEnd', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = xBegin( self )
res = ClothoidCurveMexWrapper( 'xBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = xEnd( self )
res = ClothoidCurveMexWrapper( 'xEnd', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = yBegin( self )
res = ClothoidCurveMexWrapper( 'yBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = yEnd( self )
res = ClothoidCurveMexWrapper( 'yEnd', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = thetaBegin( self )
res = ClothoidCurveMexWrapper( 'thetaBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = thetaEnd( self )
res = ClothoidCurveMexWrapper( 'thetaEnd', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = kappaBegin( self )
res = ClothoidCurveMexWrapper( 'kappaBegin', self.objectHandle );
end
Expand All @@ -215,31 +115,6 @@ function build( self, varargin )
res = ClothoidCurveMexWrapper( 'kappa_D', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function res = length( self )
res = ClothoidCurveMexWrapper( 'length', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%function info( self )
% fprintf('x0 = %g\n',self.xBegin());
% fprintf('y0 = %g\n',self.yBegin());
% fprintf('theta0 = %g\n',self.thetaBegin());
% fprintf('kappa0 = %g\n',self.kappaBegin());
% fprintf('dk = %g\n',self.kappa_D());
% fprintf('length = %g\n',self.length());
%end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function trim( self, smin, smax )
% trim the clothoid curve at the corresponging curvilinear coordinates
%
% Usage:
% ref.trim(smin, smax)
%
% On input:
% smin: initial curvilinear coordinate of the curve
% smax: final curvilinear coordinate of the curve
ClothoidCurveMexWrapper( 'trim', self.objectHandle, smin, smax );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function changeCurvilinearOrigin( self, s0, L )
% change the origin of the clothoid curve to curviliear corrdinate `s0`
% Usage:
Expand All @@ -253,64 +128,6 @@ function changeCurvilinearOrigin( self, s0, L )
self.objectHandle, s0, L );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function rotate( self, angle, cx, cy )
% rotate the clothoid curve by angle respect to the centre `(cx,cy)`
%
% Usage:
% ref.rotate(angle, cx, cy)
%
% On input:
% angle: the angle of rotation
% cx, cy: coordinates of the centre of rotation
%
ClothoidCurveMexWrapper( 'rotate', self.objectHandle, angle, cx, cy );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function translate( self, tx, ty )
% translate the clothoid curve by `(tx,ty)`
%
% Usage:
% ref.translate(tx, ty)
%
% On input:
% tx, ty: horizontal and vertical translation
%
ClothoidCurveMexWrapper( 'translate', self.objectHandle, tx, ty );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function changeOrigin( self, newX0, newY0 )
% move the origin of the clothoid to `(newX0, newY0)`
%
% Usage:
% ref.changeOrigin(newX0, newY0)
%
% On input:
% newX0, newY0: new coordinates of initial point
%
ClothoidCurveMexWrapper( 'changeOrigin', ...
self.objectHandle, newX0, newY0 );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function scale( self, s )
% scale clothoid by `sc` factor
%
% Usage:
% ref.scale(newX0, newY0)
%
% On input:
% newX0, newY0: new coordinates of initial point

ClothoidCurveMexWrapper( 'scale', self.objectHandle, s );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function reverse( self )
% reverse the orientation of the clothoid curve
% Usage:
% ref.reverse()
%
ClothoidCurveMexWrapper( 'reverse', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ s1, s2 ] = intersect( self, C )
stype = C.is_type();
if strcmp(stype,'LineSegment')
Expand Down Expand Up @@ -343,23 +160,18 @@ function reverse( self )
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ x0, y0, theta0, k0, dk, L ] = getPars( self )
x0 = self.xBegin();
y0 = self.yBegin();
theta0 = self.thetaBegin();
k = self.kappaBegin();
dk = self.kappa_D();
x0 = self.xBegin();
[ x0, y0, theta0, k0, dk, L ] = ...
ClothoidCurveMexWrapper( 'getPars', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function BB = bbox( self, max_angle, max_size, varargin )
BB = ClothoidCurveMexWrapper( 'bbox', ...
self.objectHandle, ...
max_angle, max_size, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function info( self )
ClothoidCurveMexWrapper( 'info', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ s, t ] = find_coord( self, x, y )
[ s, t ] = ...
ClothoidCurveMexWrapper( 'findST', self.objectHandle, x, y );
function [P1,P2,P3] = bbTriangles( self, varargin )
[P1,P2,P3] = ClothoidCurveMexWrapper( 'bbTriangles', self.objectHandle, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function plot( self, npts, varargin )
Expand Down
18 changes: 13 additions & 5 deletions matlab/CurveBase.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,10 @@ function changeOrigin( self, newX0, newY0 )
th = eval( self.mexName, 'theta_DDD', self.objectHandle, s );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [x,y] = xyBegin( self )
[x,y] = feval( self.mexName, 'xyBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function X0 = xBegin( self )
X0 = feval( self.mexName, 'xBegin', self.objectHandle );
end
Expand All @@ -105,6 +109,10 @@ function changeOrigin( self, newX0, newY0 )
th0 = feval( self.mexName, 'thetaBegin', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [x,y] = xyEnd( self )
[x,y] = feval( self.mexName, 'xyEnd', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function X1 = xEnd( self )
X1 = feval( self.mexName, 'xEnd', self.objectHandle );
end
Expand All @@ -130,14 +138,14 @@ function changeOrigin( self, newX0, newY0 )
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [s,idx] = projection( self, x, y, varargin )
[d,s] = feval( self.mexName, 'projection', self.objectHandle, x, y, varargin{:} );
[s,idx] = feval( self.mexName, 'projection', self.objectHandle, x, y, varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function ok = collision( self, OBJ, varargin )
[d,s] = feval( self.mexName, 'collision', ...
self.objectHandle, ...
OBJ.obj_handle(), OBJ.is_type(), ...
varargin{:} );
ok = feval( self.mexName, 'collision', ...
self.objectHandle, ...
OBJ.obj_handle(), OBJ.is_type(), ...
varargin{:} );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [s1,s2] = intersect( self, OBJ, varargin )
Expand Down
Loading

0 comments on commit 0938a8e

Please sign in to comment.