| Code: |
function xy = solver(kd,bx)
p0=bx([1 3]);
npts = length(kd);
nones=ones(npts,1);
kd(1:npts+1:end)=-1;
if any(kd(:)>=0)
xybase = solverOpt(kd,bx);
dx=bx(2)-bx(1);
dy=bx(4)-bx(3);
if npts==2 % broke the convhull case!!
if (kd(1,2)<=dx) | (dx>dy)
xy=p0([1 1],:)+[0 0;min(dx,kd(1,2)) 0];
return;
else
xy=p0([1 1],:)+[0 0;0 min(dy,kd(1,2))];
return;
end
end
dxy=max(dx,dy);
kdnorm=kd/dxy;
xy = zeros(npts,2);
locatedpts = logical(zeros(1,npts));
c=0;
noexactmatch=0;
while ~all(locatedpts) & any(kdnorm(:)>=0)
c=c+1;
if c>1 % difficult cases, multiple clusters ...
noexactmatch=1;
break;
end
oldlocpts=locatedpts;
[xy,locatedpts,noexactmatch] = solverExact2D(kdnorm,npts,xy,locatedpts);
if noexactmatch, break; end
clust{c}=find(locatedpts & ~oldlocpts);
kdnorm(locatedpts,:)=-1;
kdnorm(:,locatedpts)=-1;
end
if ~noexactmatch,
% perfect match, now fit it in the box ...
xy=xy*dxy;
if any(xy(:,1)>dx) | any(xy(:,1)<0) | any(xy(:,2)>dy) | any(xy(:,2)<0)
k=convhull(xy(:,1),xy(:,2));
oversize=inf;
osxy=[];
for j=1:length(k)-1
% align k with edge and check whether it fits ...
dxy=xy-repmat(xy(k(j),:),npts,1);
e1=xy(k(j+1),:)-xy(k(j),:);
e1=e1./sqrt(sum(e1.^2));
e2=[-e1(2) e1(1)];
nxy=dxy*[e1;e2]';
%plot(xy(:,1),xy(:,2),'.-',xy(k,1),xy(k,2),'.-',[0 dx dx 0 0],[0 0 dy dy 0],nxy(:,1),nxy(:,2),'.-'); set(gca,'da',[1 1 1])
maxnxy=max(nxy);
minnxy=min(nxy);
if all(maxnxy-minnxy<[dx dy])
xy=nxy-minnxy(nones,:);
osxy=[];
break
else
os=sum(max(0,maxnxy-minnxy-[dx dy]));
if os<oversize
osxy=nxy-minnxy(nones,:);
oversize=os;
end
end
end
if ~isempty(osxy)
xy=osxy;
% clip when necessary ... can be optimized further!
% but is unlikely that this will be necessary
if max(xy(:,1))>dx
xy(:,1)=min(xy(:,1),dx);
end
if max(xy(:,2))>dy
xy(:,2)=min(xy(:,2),dy);
end
end
end
end
xy = xy+p0(nones,:);
x=xy(:,1);
y=xy(:,2);
[XY1,XY2] = meshgrid(x,y);
dist = sqrt((XY1 - XY1').^2 + (XY2 - XY2').^2);
% Calculate strain matrix
strainMatrix = dist - kd;
strainMatrix(kd < 0) = 0;
results1 = sum(abs(strainMatrix(:)));
x=xybase(:,1);
y=xybase(:,2);
[XY1,XY2] = meshgrid(x,y);
dist = sqrt((XY1 - XY1').^2 + (XY2 - XY2').^2);
% Calculate strain matrix
strainMatrix = dist - kd;
strainMatrix(kd < 0) = 0;
results2 = sum(abs(strainMatrix(:)));
if results2<results1
xy=xybase;
end
else
xy = p0(nones,:);
end
function [xy,locatedpts,noexactmatch] = solverExact2D(kd,npts,xy,locatedpts)
noexactmatch=0;
first=1;
% pick the most connected point (cases without any connection caught, so there always exists one)
[kdmax,i] = max(sum(kd>=0));
locatedpts(i) = 1;
%xy(i,:) = [0 0];
% pick the most connected point, connected to i (there always exists one)
[kdmax,ii] = max(sum(kd>=0).*(kd(i,:)>=0));
locatedpts(ii) = 1;
xy(ii,:) = [kd(i,ii) 0];
while ~all(locatedpts)
connected = sum(kd(locatedpts,:)>=0);
if any((connected>2) & (~locatedpts))
[kdmax,i] = max(sum(kd>=0).*(connected>2).*(~locatedpts));
connectedto = find((kd(i,:)>=0) & locatedpts);
i1 = connectedto(1);
i2 = connectedto(2);
% % optionally use the base as wide as possible for optimal
% % numerical stability.
% x=xy(connectedto,1);
% y=xy(connectedto,2);
% [XY1,XY2] = meshgrid(x,y);
% dist = sqrt((XY1 - XY1').^2 + (XY2 - XY2').^2);
% [ii,jj,vv]=find(dist==max(dist(:)));
% i1=connectedto(ii(1));
% i2=connectedto(jj(1));
% tmp=connectedto([1 2]);
% connectedto([1 2])=[i1 i2];
% connectedto([ii(1) jj(1)])=tmp;
e1=xy(i2,:)-xy(i1,:);
dist2=sum(e1.^2); dist=sqrt(dist2);
if (kd(i1,i)>dist+kd(i2,i)) | (kd(i2,i)>dist+kd(i1,i)) | (dist>kd(i2,i)+kd(i1,i))
% no crossing to be found
noexactmatch=1;
return
else
dx=(kd(i1,i).^2-kd(i2,i).^2-dist2)./(2*dist);
dy=sqrt(kd(i2,i).^2-dx.^2);
e1=e1./dist;
e2=[-e1(2) e1(1)];
% solutions xy(i2,:)+e1*dx+e2*dy and xy(i2,:)+e1*dx-e2*dy
if abs(dy)<1E-10;
xy(i,:)=xy(i2,:)+e1*dx;
locatedpts(i)=1;
else
xy0=xy(i2,:)+e1*dx+e2*dy;
xy1=xy(i2,:)+e1*dx-e2*dy;
for i3=connectedto(3:end)
dist0=sum((xy0-xy(i3,:)).^2);
dist1=sum((xy1-xy(i3,:)).^2);
distT=kd(i3,i).^2;
if abs(dist0-distT)<abs(dist1-distT)
xy(i,:)=xy0;
locatedpts(i)=1;
break;
elseif abs(dist0-distT)>abs(dist1-distT)
xy(i,:)=xy1;
locatedpts(i)=1;
break;
end
end
if ~locatedpts(i)
% cannot distinguish between left and right
% extremely unlikely
% warning('symmetric triple')
% keyboard
return
end
end
end
if sum(sum((xy(connectedto,:)-repmat(xy(i,:),length(connectedto),1)).^2,2)-kd(connectedto,i).^2)>1E-10
% probably due to no exact match
noexactmatch=1;
return
end
elseif any((connected==2) & (~locatedpts))
[kdmax,i] = max(sum(kd>=0).*(connected==2).*(~locatedpts));
connectedto = find((kd(i,:)>=0) & locatedpts);
i1 = connectedto(1);
i2 = connectedto(2);
e1=xy(i2,:)-xy(i1,:);
dist2=sum(e1.^2); dist=sqrt(dist2);
if (kd(i1,i)>dist+kd(i2,i)) | (kd(i2,i)>dist+kd(i1,i)) | (dist>kd(i2,i)+kd(i1,i))
% no crossing to be found
noexactmatch=1;
return
else
locatedpts(i)=1;
dx=(kd(i1,i).^2-kd(i2,i).^2-dist2)./(2*dist);
dy=sqrt(kd(i2,i).^2-dx.^2);
e1=e1./dist;
e2=[-e1(2) e1(1)];
% solutions xy(i2,:)+e1*dx+e2*dy and xy(i2,:)+e1*dx-e2*dy
% first time only one option based on symmetry assumptions
if first
xy(i,:)=xy(i2,:)+e1*dx+e2*dy;
first=0;
else
% warning('two symmetric options')
% keyboard
return
end
end
else
% all connected < 2
% warning('all connected <2')
% keyboard
return
end
end
function xy = solverOpt(kd,bx)
npts = length(kd);
if ~any(kd(:)>0),
xy = zeros(npts,2);
return
end
dx=bx(2);
dy=bx(4);
D=dx+1i*dy;
pm=D/2;
xy=zeros(1,npts)+pm;
d=0.1*max(dx,dy);
x=0:d:dx;
y=0:d:dy;
[xx,yy] = ndgrid(x,1i*y);
[nx,ny]=size(xx);
onesx=ones(1,nx);
onesy=ones(1,ny);
XX=xx(:)+yy(:);
dr=abs(D);
kd(find(kd>dr))=dr;
S=(kd-eye(npts))>=0;
nX=length(XX);
xOnes=ones(nX,1);
xZeros=zeros(nX,1);
start=1;
iones=ones(1,npts);
sCut = 1;
olds = 1.0e30;
iterbigmax = 1;
for iterbig = 1:iterbigmax
[dum,p]=sort(-max(kd));
S=S(p,p);
kd=kd(p,p);
[v,w]=sort(p);
for iter=1:11
for index = 1:npts
I = find(S(:,index));
if ~isempty(I)
st = sum(abs(abs(XX(:,ones(size(I)))-xy(xOnes,I))-kd(xZeros+index,I)),2);
[null,minloc] = min(st);
xy(index) = XX(minloc);
end
end
[s,M]=findstrain(S,xy,kd);
if((abs(s-olds)/(s+1)) < 0.002) | s < sCut
break;
end
olds = s;
end
xy=xy.';
xy=xy(w,:);
kd=kd(w,w);
S=S(w,w);
olds = 1.0e50;
oldxy = xy;
alpha = 1;
for iter=1:40
[s,M]=findstrain(S,xy,kd);
if( iter > 5 & abs(s-olds) / (s+1) < 0.0001 ) | s < sCut
xy = oldxy;
break;
end
if( s > olds )
xy = oldxy;
break;
end
oldxy = xy;
olds = s;
for indx = 1:npts
I = find(S(:,index)>0);
dX=xy(I)-xy(indx);
Dx=mean(dX.*(1-kd(I,index)./(abs(dX)+eps)));
% Dx=mean(dX./(abs(dX)+eps) .* M(I,indx));
xy(indx) = xy(indx) + alpha*Dx;
xy(indx) = min(dx,max(0,real(xy(indx))))+1i*min(dy,max(0,imag(xy(indx))));
end
end
if( iterbig < iterbigmax )
xy=xy.';
end
end
alpha = 0.1;
ngrad = 40;
obj = 1.0e20*ones(ngrad,1);
xybig = zeros(npts,ngrad);
[objnew,strainMatrix]=findstrain(S,xy,kd);
obj(1) = objnew;
xybig(:,1) = xy;
gradient = zeros(npts,1);
for ij=2:ngrad
[s,sM,dM]=findstrain(S,xy,kd);
gradient(:) = sum(dM./(abs(dM)+eps).*sign(sM));
xy = xy - alpha * gradient;
xy = min(max(real(xy),bx(1)),bx(2)) + j * min(max(imag(xy),bx(3)),bx(4));
[objnew,strainMatrix]=findstrain(S,xy,kd);
obj(ij) = objnew;
xybig(:,ij) = xy;
if( obj(ij) > obj(ij-1) )
xy = xybig(:,ij-1);
alpha = alpha / 10;
else
alpha = alpha * 2;
end
[bestobj,index] = min(obj);
if( ij > 3 & index < 2 ) | bestobj < sCut
break
end
end
[bestobj,index] = min(obj);
xy = xybig(:,index);
xy=[real(xy) imag(xy)];
function [s,strainMatrix,dX]=findstrain(S,xy,kd)
sI=find(S);
strainMatrix=S;
dX=S;
[X1,X2]=meshgrid(xy);
dX(sI)=X1(sI)-X2(sI);
strainMatrix(sI) = abs(dX(sI))-kd(sI);
s = sum(abs(strainMatrix(sI)));
|
