function [obstacle,t]=timeVariantGenericObstacles(p,fHandle,TInitial,TFinal,samples,draw)
% function [obstacle,t]=timeVariantGenericObstacles(p,fHandle,TInitial,TFinal,samples,draw)
%
% This function generates the trajectory of an obstacle with
% constant velocity in a given time interval.
%
% Input parameters:
% p-> initial positions for obstacle points, it is a 2xn matrix where each
% column is a point obstacle
% fHandle-> it is a function handle defining the trajectory of the obstacle points
% TInitial,TFinal-> initial and final times for calculating the trajectory. The trajectory
% will be calculated in the the interval [TInitial,TFinal]
% samples-> number of trajectory points to be calculated
% draw -> is a logic value (1=true, 0=false). When it is true, results
% are drawn in a figure. It is useful for debugging.
%
% Output parameters:
% obstacle-> points conforming the trajectory calculated for the point
% obstacle. It is a 2xnxsamples matrix, each column is a point in the calculated
% trajectory
% t-> time values at which the trajectory has been calculated, it is a 1xsamples
% matrix
%
% Author: Antonio C. DomÃnguez Brito (adominguez@iusiani.ulpgc.es)
% Institution: SIANI-ULPGC (www.iusiani.ulpgc.es)
% Date: April 26th 2008
obstacle=[]; t=[];
if TInitial<0 | TFinal<0 | abs(TFinal-TInitial)<=0
fprintf('WARNING: Doing nothing. Input arguments TInitial and TFinal should be greater than zero and different.\n');
elseif samples<=1
fprintf('WARNING: Doing nothing. Input argument samples should be greater than one.\n');
else
t=linspace(TInitial,TFinal,samples);
for sample=[1:samples]
for point=[1:size(p,2)]
obstacle(:,point,sample)=fHandle(p(:,point),t(sample));
end
end
end
if nargin==6 & draw
hold on;
lightRed=[244 146 113]/255;
cyan=[140 241 228]/255;
grey=[196 196 196]/255;
tmpOnes=ones(1,size(p,2));
for sample=[1:samples]
plot3(obstacle(1,:,sample),obstacle(2,:,sample),tmpOnes*t(sample),'.b');%,'markersize',10);
if sample