-
Notifications
You must be signed in to change notification settings - Fork 0
/
MaxDistCP.m
50 lines (39 loc) · 1.41 KB
/
MaxDistCP.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
function Dist = MaxDistCP(Qto,Qfrom,BB,MaxDist)
Dist=ones(1,size(Qfrom,2))*MaxDist;
Range=floor((BB(2,:)-BB(1,:))/MaxDist);
tic
Done=0;
LookAt=zeros(1,size(Qfrom,2));
for x=0:Range(1),
for y=0:Range(2),
for z=0:Range(3),
Low=BB(1,:)+[x y z]*MaxDist;
High=Low+MaxDist;
idxF=find(Qfrom(1,:)>=Low(1) & Qfrom(2,:)>=Low(2) & Qfrom(3,:)>=Low(3) &...
Qfrom(1,:)<High(1) & Qfrom(2,:)<High(2) & Qfrom(3,:)<High(3));
SQfrom=Qfrom(:,idxF);
LookAt(idxF)=LookAt(idxF)+1; %Debug
Low=Low-MaxDist;
High=High+MaxDist;
idxT=find(Qto(1,:)>=Low(1) & Qto(2,:)>=Low(2) & Qto(3,:)>=Low(3) &...
Qto(1,:)<High(1) & Qto(2,:)<High(2) & Qto(3,:)<High(3));
SQto=Qto(:,idxT);
if(isempty(SQto))
Dist(idxF)=MaxDist;
else
KDstl=KDTreeSearcher(SQto');
[~,SDist] = knnsearch(KDstl,SQfrom');
Dist(idxF)=SDist;
end
Done=Done+length(idxF); %Debug
end
end
%Complete=Done/size(Qfrom,2);
%EstTime=(toc/Complete)/60
%toc
%LA=[sum(LookAt==0),...
% sum(LookAt==1),...
% sum(LookAt==2),...
% sum(LookAt==3),...
% sum(LookAt>3)]
end