Commit d98422bc authored by Markus Koschi's avatar Markus Koschi

added SPOT release 2017a

parent 6f8a0a53
classdef Polygon < geometry.Shape
% Polygon - class for describing polygons
%
% v1--v2---v3---v4
% | |
% vN+2 v5
% | |
% vN+1--vN-...--v6
%
% Syntax:
% object constructor: obj = Polygon(width, length, position, orientation)
%
% Inputs:
% vertices - array of points which define the polygon
% vertices = [x ; y], x = vector of x-coordinates of all
% points, y = vector of y-coordinates of all points
%
% Outputs:
% obj - generated object
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (SetAccess = protected, GetAccess = public)
vertices = [];
end
methods
% class constructor
function obj = Polygon(vertices)
if size(vertices,2) == 2 && size(vertices,1) ~= 2
obj.vertices = vertices';
else
obj.vertices = vertices;
end
%order vertices in clockwise order
if ~ispolycw(obj.vertices(1,:),obj.vertices(2,:))
obj.vertices(1,:) = fliplr(obj.vertices(1,:));
obj.vertices(2,:) = fliplr(obj.vertices(2,:));
%obj.vertices = poly2cw(obj.vertices(1,:),obj.vertices(2,:));
end
end
% visualization methods
disp(obj)
plot(varargin)
end
end
%------------- END CODE --------------
\ No newline at end of file
function [inLaneId] = findInLaneId(obj, obstacle)
% findInLaneId - find id of the lane which contains the object
function disp(obj)
% display - displays a Polygon object
%
% Syntax:
% inLaneId = findInLaneId(obj, map)
% display(obj)
%
% Inputs:
% obj - Map object
% obstacle - Obstacle object
% obj - Polygon object
%
% Outputs:
% inLaneId - id of the lane
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: XX-Oktober-2016
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
%TODO: Rajat
inLaneId = 0;
disp('-----------------------------------');
% display type
disp('type: Polygon');
% display properties
disp(['number of vertices: ' num2str(size(obj.vertices,2))]);
disp('-----------------------------------');
end
%------------- END CODE --------------
%------------- END CODE --------------
\ No newline at end of file
function plot(varargin)
% plot - plots a Polygon object
%
% Syntax:
% plot(obj, color)
%
% Inputs:
% obj - Polygon object
% color - color to plot
%
% Outputs:
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
if nargin == 1
obj = varargin{1};
color = [rand, rand, rand];
elseif nargin >= 2
obj = varargin{1};
color = varargin{2};
end
fill(obj.vertices(1,:), obj.vertices(2,:), color, 'FaceAlpha', 0.1, 'EdgeColor', 'k');
end
%------------- END CODE --------------
\ No newline at end of file
classdef Rectangle < geometry.Shape
% Rectangle - class for describing rectangles
%
% v4 ---------------------- v1
% | |
% | |
% | +---> | obj.width
% | position |
% | orientation |
% v3 ---------------------- v2
% obj.length
%
% Syntax:
% object constructor: obj = Rectangle(width, length, position, orientation)
%
% Inputs:
% length - length of the obstacle in m
% width - width of the obstacle in m
% position - position of the rectangle's center
% orientation - orientation of the rectangle in radians
%
% Outputs:
% obj - generated object
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (SetAccess = protected, GetAccess = public)
length = [];
width = [];
position = [];
orientation = [];
end
% properties (SetAccess = protected, GetAccess = public)
% radius
% vertices
% end
methods
% class constructor
function obj = Rectangle(length, width, position, orientation)
if nargin >= 2
obj.length = length;
obj.width = width;
end
if nargin == 4
if size(position,2) == 2
obj.position = position';
else
obj.position = position;
end
obj.orientation = orientation;
end
% obj.radius = sqrt(width^2/4+height^2/4);
%
% w = obj.height/2;
% h = obj.width/2;
% vert = [w w -w -w; h -h -h h];
% vert = geometry.rot2d(obj.orientation, vert);
% vert = [vert(1,:) + obj.position(1); vert(2,:) + obj.position(2)];
% obj.vertices = vert;
end
% visualization methods
disp(obj)
plot(varargin)
end
end
%------------- END CODE --------------
\ No newline at end of file
function disp(obj)
% display - displays a Rectangle object
%
% Syntax:
% display(obj)
%
% Inputs:
% obj - Rectangle object
%
% Outputs:
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2016
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
disp('-----------------------------------');
% display type
disp('type: Rectangle');
% display properties
disp(['width: ' num2str(obj.width)]);
disp(['length: ' num2str(obj.length)]);
if ~isempty(obj.position)
disp(['position: [' num2str(obj.position(1)) '; ' num2str(obj.position(2)) ']']);
end
if ~isempty(obj.orientation)
disp(['orientation: ' num2str(obj.orientation)]);
end
disp('-----------------------------------');
end
%------------- END CODE --------------
\ No newline at end of file
function plot(varargin)
% plot - plots a Rectangle object
%
% Syntax:
% plot(varargin)
%
% Inputs:
% obj - Rectangle object
% color - color to plot
% position - coordinates of rectangle's center
% orientation - orientation of the rectangle
%
% Outputs:
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
if nargin == 1 && ~isempty(varargin{1}.position) && ~isempty(varargin{1}.orientation)
obj = varargin{1};
color = [rand, rand, rand];
position = obj.position;
orientation = obj.orientation;
elseif nargin == 2 && ~isempty(varargin{1}.position) && ~isempty(varargin{1}.orientation)
obj = varargin{1};
color = varargin{2};
position = obj.position;
orientation = obj.orientation;
elseif nargin == 4
obj = varargin{1};
color = varargin{2};
position = varargin{3};
orientation = varargin{4};
else
return;
end
vertices = geometry.rotateAndTranslateVertices(geometry.addObjectDimensions([0;0], obj.length, obj.width), position, orientation);
fill(vertices(1,:), vertices(2,:), color, 'FaceAlpha', 1, 'EdgeColor', 'k');
end
%------------- END CODE --------------
\ No newline at end of file
classdef Shape < matlab.mixin.Heterogeneous & handle
% Shape - class for describing 2D shapes
% (Inherits from matlab.mixin.Heterogeneous to combine instances of the
% subclasses into heterogeneous arrays)
%
% Syntax:
% no object constructor
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update: 08-June-2017
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (SetAccess = protected, GetAccess = public)
end
methods
% visualization methods
disp(obj)
plot(varargin)
end
end
%------------- END CODE --------------
\ No newline at end of file
classdef Triangle < geometry.Shape
% TRIANGLE - class for describing triangles
% (vertices in clockwise order)
%
% % v1
% |\
% | \
% | \
% | \
% | \
% v3 -------v2
%
% Syntax:
% object constructor: obj = Triangle(x1, y1, x2, y2, x3, y3)
%
% Inputs:
% vertices - array of points which define the triangle
% vertices = [x ; y], x = vector of x-coordinates of all
% points, y = vector of y-coordinates of all points
%
% Outputs:
% obj - generated object
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Vanessa Bui, Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (SetAccess = protected, GetAccess = public)
vertices = [];
end
methods
% class constructor
function obj = Triangle(x1, y1, x2, y2, x3, y3)
if nargin == 6
obj.vertices = [x1, x2, x3 ; y1, y2, y3];
end
end
% visualization methods
disp(obj)
plot(varargin)
end
end
%------------- END CODE --------------
\ No newline at end of file
function disp(obj)
% display - displays a Triangle object
%
% Syntax:
% display(obj)
%
% Inputs:
% obj - Triangle object
%
% Outputs:
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
disp('-----------------------------------');
% display type
disp('type: Triangle');
% display properties
disp(['number of vertices: ' num2str(size(obj.vertices,2))]);
disp('-----------------------------------');
end
%------------- END CODE --------------
\ No newline at end of file
function plot(varargin)
% plot - plots a Triangle object
%
% Syntax:
% plot(obj, color)
%
% Inputs:
% obj - Triangle object
% color - color to plot
%
% Outputs:
% none
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 08-February-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
if nargin == 1
obj = varargin{1};
color = [rand, rand, rand];
elseif nargin >= 2
obj = varargin{1};
color = varargin{2};
end
fill(obj.vertices(1,:), obj.vertices(2,:), color, 'FaceAlpha', 0.1, 'EdgeColor', 'k');
end
%------------- END CODE --------------
\ No newline at end of file
function [angle] = calcAngleOfVerticesAtPosition(position, vertices)
% calcAngleOfVerticesAtPosition - calculates the angle (orientation) of
% the line segment of the vertices which has the minimal distance to the
% given position.
% The angle is measured in the closed interval [-pi, pi] relative to the
% global x-axis (counter-clockwise is positive).
%
% Syntax:
% [angle] = calcAngleOfVerticesAtPosition(position, vertices)
%
% Inputs:
% position - coordinates of the object
% vertices - polyline
%
% Outputs:
% angle - orientation of the closest line segment
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Markus Koschi
% Written: 18-May-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
% find closest vertex
[indexClosestVertex, ~] = geometry.findClosestVertexOfPosition(position, vertices);
if indexClosestVertex == length(vertices)
indexClosestVertex = indexClosestVertex - 1;
end
angle = atan2(vertices(2,indexClosestVertex+1) - vertices(2,indexClosestVertex), ...
vertices(1,indexClosestVertex+1) - vertices(1,indexClosestVertex));
end
%------------- END CODE --------------
\ No newline at end of file
......@@ -19,7 +19,7 @@ function [curvature] = calcCurvature(x, y)
% Author: Markus Koschi
% Written: 05-Oktober-2016
% Last update: 19-Oktober-2016 (signOfCurv --> curvature)
% Last update: 16-May-2017
%
% Last revision:---
......@@ -32,7 +32,18 @@ dy = gradient(y);
ddy = gradient(dy);
% calculate the signed curvature
curvature = (dx .* ddy - ddx .* dy) ./ ((dx.^2 + dy.^2).^(3/2));
if all(dx ~= 0) && all(dy ~= 0)
curvature = (dx .* ddy - ddx .* dy) ./ ((dx.^2 + dy.^2).^(3/2));
else
% set curvature to 0 instead of to NaN (when division by 0)
curvature = zeros(1,length(x));
isNum = and((dx ~= 0),(dy ~= 0));
curvature(isNum) = (dx(isNum) .* ddy(isNum) - ddx(isNum) .* dy(isNum)) ./ ((dx(isNum).^2 + dy(isNum).^2).^(3/2));
end
% % smoothing
% isSmall = abs(curvature) < 10e-04;
% curvature(isSmall) = 0;
end
......
function [x_cen,y_cen] = calcPolygonCentroid(x,y)
% calcPolygonCentroid - Summary of this function goes here
%
% Syntax:
% [x_cen,y_cen] = calcPolygonCentroid(x,y);
%
% Inputs:
% x,y : x and y are coulmn vector for polygon
%
% Outputs:
% x_cen,y_cen : centroid of polygon
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Rajat Koner
% Written: 30 Jan 2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
% check if inputs are same size
if ~isequal( size(x), size(y) ),
error( 'X and Y must be the same size');
end
% temporarily shift data to mean of vertices for improved accuracy
xm = mean(x);
ym = mean(y);
x = x - xm;
y = y - ym;
% summations for CCW boundary
xp = x( [2:end 1] );
yp = y( [2:end 1] );
a = x.*yp - xp.*y;
A = sum( a ) /2;
xc = sum( (x+xp).*a ) /6/A;
yc = sum( (y+yp).*a ) /6/A;
% replace mean of vertices
x_cen = xc + xm;
y_cen = yc + ym;
end
function [distance] = calcProjectedDistance(i, bound, position)
function [distance, nLamda] = calcProjectedDistance(i, bound, position)
% calcProjectedDistance - project the position on the bound such that it is
% perpendicular to the pseudo tanget and calculate the distance of the
% projection point to vertice i
......@@ -13,6 +13,8 @@ function [distance] = calcProjectedDistance(i, bound, position)
%
% Outputs:
% distance - L2 norm from pLamda to vi
% nLamda - vector perpendicular to the pseudo tanget and pointing to the
% given position
%
% Other m-files required: none
% Subfunctions: none
......@@ -25,7 +27,7 @@ function [distance] = calcProjectedDistance(i, bound, position)
% Author: Markus Koschi
% Written: 20-Oktober-2016
% Last update:
% Last update: 23-May-2017
%
% Last revision:---
......@@ -36,22 +38,14 @@ vi = bound.vertices(:,i);
% find the previous and next vertices on the lane bound:
% (note that the bound of these vertices might not be the inner one)
% set v(i-2) and v(i-1)
if (i-2) > 0 % far away from lane beginning in forward driving direction
% consider the segment v(i-1) -> v(i)
flag = 1;
if (i-2) > 0
viminus2 = bound.vertices(:,i-2);
viminus1 = bound.vertices(:,i-1);
elseif (i-2) == 0
% consider the segment v(i-1) -> v(i), in which v(i-1) is the first
% vertice of the lane
flag = 1;
viminus2 = bound.vertices(:,i-1);
viminus1 = viminus2;
else % iBorder == 1, i.e.
% object is right at the beginning of the lane
flag = 0; % consider the next segment v(i) -> v(i+1)
else % i == 1
viminus1 = vi;
end
......@@ -65,16 +59,28 @@ elseif (i+1) == length(bound.vertices)
elseif i == length(bound.vertices)
viplus2 = bound.vertices(:,i);
viplus1 = viplus2;
if ~flag % iBorder == 1 && iBorder == length(innerBound.vertices)
if ~exist('viminus2','var') % iBorder == 1 && iBorder == length(innerBound.vertices)
error(['Lane %i is too short to properly calculate the projection of'...
' the object on the lane border'], bound.id(1));
end
end
% choose the segment in which the object's position is projected in:
% calculate the angle between the points (vi, p, vi-1) and (vi, p, vi+1)
a = vi - position;
b = viminus1 - position;