function [pSMerged,pSMergedCC]=mergeSegments(pS1,pS1CC,pS2,pS2CC,forwardBackward,precision)
% function [pSMerged,pSMergedCC]=mergeSegments(pS1,pS1CC,pS2,pS2CC,forwardBackward,precision)
%
% This function takes two sequence of segments in cartesian and circular trajectory coordinates
% and merge them in a way that occluded parts are eliminated, in order to do that it uses function
% splitAndClipSegment
%
% Input parameters:
% pS1,pS1CC-> sequence of segments in cartesian and circular trajectory coordinates respectively,
% they are both a cell array of segments where each segment is composed by two points (a 2D column
% vector), this is the first sequence of segments to merge
% pS2,pS2CC-> sequence of segments in cartesian and circular trajectory coordinates respectively,
% they are both a cell array of segments where each segment is composed by two points (a 2D column
% vector), this is the first sequence of segments to merge
% forwardBackward-> it is a logical variable, when it is true the calculation considers that the robot is
% moving forward, otherwise,it is considered that it moves backwards
% precision-> indicates the precision at which it is considered that points are inside a segment
%
% Output parameters:
% pSMerged,pSMergedCC-> it is a cell array of segments, it contains the merging of the segments
% especified by pS1 and ps1CC, and pS2 and pS2CC respectively, in such a manner that occluded segments
% have been taken away
%
% Author: Antonio C. DomÃnguez Brito (adominguez@iusiani.ulpgc.es)
% Institution: SIANI-ULPGC (www.iusiani.ulpgc.es)
% Date: June 5th 2008
% sizepS1=size(pS1)
% sizepS2=size(pS2)
pS1IsEmpty=isempty(pS1); pS2IsEmpty=isempty(pS2);
if pS1IsEmpty & pS2IsEmpty
pSMerged=[]; pSMergedCC=[]; return;
elseif pS1IsEmpty
pSMerged=pS2; pSMergedCC=pS2CC; return;
elseif pS2IsEmpty
pSMerged=pS1; pSMergedCC=pS1CC; return;
end
[pS1,pS1CC]= ...
calculateMinimumSegments(pS1,pS1CC,pS2,pS2CC,forwardBackward,precision);
[pS2Occluded,pS2OccludedCC]=occludeSegments( ...
pS1,pS1CC, ...
pS2,pS2CC, ...
forwardBackward, ...
precision ...
);
[pS2Occluded,pS2OccludedCC]= ...
calculateMinimumSegments(pS2Occluded,pS2OccludedCC,pS1,pS1CC,forwardBackward,precision);
[pS1Occluded,pS1OccludedCC]=occludeSegments( ...
pS2Occluded,pS2OccludedCC, ...
pS1,pS1CC, ...
forwardBackward, ...
precision ...
);
[pSMerged,pSMergedCC]=concatenateSegments(pS1Occluded,pS1OccludedCC,pS2Occluded,pS2OccludedCC);
return;
function [pSOccluded,pSOccludedCC]=occludeSegments(pS,pSCC,pSToOcclude,pSToOccludeCC,forwardBackward,precision)
pSIsEmpty=isempty(pS); pSToOccludeIsEmpty=isempty(pSToOcclude);
if pSIsEmpty & pSToOccludeIsEmpty
pSOccluded=[]; pSOccludedCC=[]; return;
elseif pSIsEmpty
pSOccluded=pSToOcclude; pSOccludedCC=pSToOccludeCC; return;
elseif pSToOccludeIsEmpty
pSOccluded=pS; pSOccludedCC=pSCC; return;
end
pSOccluded=pSToOcclude; pSOccludedCC=pSToOccludeCC;
for segment=[1:size(pS,2)]
for innerSegment=[1:2:size(pS{segment},2)]
p1=pS{segment}(:,innerSegment); p1CC=pSCC{segment}(:,innerSegment);
p2=pS{segment}(:,innerSegment+1); p2CC=pSCC{segment}(:,innerSegment+1);
for segmentToOcclude=[1:size(pSOccluded,2)]
[pSOccluded{segmentToOcclude},pSOccludedCC{segmentToOcclude}]= ...
clipSegments( ...
p1,p1CC,p2,p2CC, ...
pSOccluded{segmentToOcclude},pSOccludedCC{segmentToOcclude}, ...
forwardBackward, ...
precision ...
);
end
end
end
[pSOccluded,pSOccludedCC]=refineSegments(pSOccluded,pSOccludedCC,precision);
return;
function [pS12,pS12CC]=concatenateSegments(pS1,pS1CC,pS2,pS2CC)
pS12=pS1; pS12CC=pS1CC;
concatenatedSegment=size(pS1,2);
for segment=[1:size(pS2,2)]
if ~isempty(pS2{segment})
concatenatedSegment=concatenatedSegment+1;
pS12{concatenatedSegment}=pS2{segment};
pS12CC{concatenatedSegment}=pS2CC{segment};
end
end
return;