%==================================================================
%
%	CalcPolSphere.m
%
%	Markus Junghoefer	[1995]
%   This software is protected by german copyright and international treaties.             
%   Copyright 2004 Markus Junghöfer & Peter Peyk. All Rights Reserved.                     
%                                                                                          
%   THIS SOFTWARE AND DOCUMENTATION IS PROVIDED "AS IS," AND COPYRIGHT HOLDERS MAKE        
%   NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO,    
%   WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR THAT THE USE    
%   OF THE SOFTWARE OR DOCUMENTATION WILL NOT INFRINGE ANY THIRD PARTY PATENTS, COPYRIGHTS,
%   TRADEMARKS OR OTHER RIGHTS. COPYRIGHT HOLDERS WILL NOT BE LIABLE FOR ANY DIRECT,       
%   INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF ANY USE OF THE SOFTWARE      
%   OR DOCUMENTATION.                                                                      
%
%
%	This function will calculate the best sphere-fit to given cartesian values
%	Method:
%	Zunaechst wird als Startpunkt das Mittelwerttripel der drei Raumrichtungen x,y,z
%	aller eingelesenen Werte Tripel berechnet. Nun werden mittels calc_box 27 Punkte 
%	eines Wuerfels berechnet, die diesen Punkt als Zentrum (14 ter Wert) haben. Gesucht 
%	wird nun der Punkt von dem aus die Summe aller Abstaende zu allen Werte Tripeln 
%	moeglichst klein ist.
%	Innerhalb der naechsten Schleife wird nun dieser Punkt als neues (oder evtl. auch
%	altes) Zentrum eines neuen Wuerfels definiert dessen Kantenlaenge nur noch 2/3 der 
%	vorhergehenden betragt und erneut wie oben der Punkt minimalen Summenabstandes errechnet.
%	 
%	Diese Suchschleife wird abgebrochen, falls die Kantenlaenge des Wuerfels kleiner
%	als das doppelte einer gewuenschten Toleranz wird. Das Verhaeltnis zwischen der 
%	hier gewaehlten Toleranz und der Rechengenauigkeit ist natuerlich von der Einheit
%	der eigegebenen Wertetripel abhaengig.
%	Beispiel: Werden die Werte in Metern angegeben so entspricht tol=0.001 einer 
%	Genauigkeit von einem Millimeter.
%	Diese Rechenmethode ist im Gegensatz zu anderen Suchalgorithmen (wie Gradientenmethode
%	u.s.w.) recht langwierig hat jedoch zwei Vorteile:
%	1)	Die Gefahr anderer Algorithmen in Nebenextrema "haengen zu bleiben" entfaellt.
%	2)	Es kann auf recht einfache Art und Weise definiert werden, welche Punkte im Raum
%		wie stark in die Kugelanpassung eingehen sollen. Diese Art der "Gewichtung" einzelner
%		Punkte kann sinnvoll sein, falls die Kugelanpassung an bestimmten Kopfregionen
%		beispielsweise dort, wo Aktivitaeten erwartet werden, moeglichst gut sein soll,
%		oder falls bestimmte Punkte ungenau gemessen wurden.
%		So besteht hier die Moeglichkeit die Standartabweichungen der einzelnen Messpunkte
%		als Gewichtswerte zu waehlen und/oder Gewichte selbst zu bestimmen. Ein Wert von 1
%		entspricht einem Gewicht von 100 %. Mit einem Gewicht von Null geht dieser Messpunkt
%		nicht in die Rechnung ein.
%	
%
%	Function definition

function [PointMatOut,PointMatSphere,Center,Radius,CenterIter,RadiusIter,PointVecDiff] =...
CalcPolSphere(PointMat,StdMat,ENames,WeightVec);

%=================================================================
if nargin<4; WeightVec=[]; end
if nargin<3; ENames=[]; end
if nargin<2; StdMat=[]; end
if nargin<1; PointMat=[]; end

%+++++SYNTHETIC DATA IF PROGRAM IS USED WITHOUT DATA

if isempty(PointMat)
	fprintf('Calculate 16 points of the unit sphere:  ');
	pause(1)
	
	[a,b,c]=sphere(3);
	for i=1:4
		for j=1:4
			PointMat((i-1)*4+j,1)=a(i,j);
			PointMat((i-1)*4+j,2)=b(i,j);
			PointMat((i-1)*4+j,3)=c(i,j);
		end
	end
	clc;
	[m,n]=size(PointMat);
	fprintf('Do you want to randomize only point 7,     [1]\n');
	fprintf('all points on unit sphere,                 [2]\n');
	fprintf('all points and specially point 7           [3]\n');
	fprintf('or dont you want to randomize data ? [4]\n');
	chRand=input('Please insert your choice:  [4]  ');
	if isempty(chRand)
		chRand=4;
	end
	if chRand==1 | chRand==3
		PointMat(7,:)=PointMat(7,:)+rand(1,3)*2;
	end
	if chRand==2 | chRand==3
		clc;
		RandAmp=input('Please insert the avg. random amplitude: ');
		RandMat=(rand(m,n).*2-1).*RandAmp;
		StdMat=RandMat;
		PointMat=PointMat+RandMat;
	end
end
[m,n]=size(PointMat);

if isempty(StdMat)
	StdMat=ones(m,n); chWeight=0;
else
	[mStd,nStd]=size(StdMat);
	if m~=mStd | n~=nStd; error('PointMat and StdMat have to have the same size !'); end
	chWeight=[];
end
if m==3 & n~=3
	TransPointMatStatus=1;
	PointMat=PointMat';
	StdMat=StdMat';
	m=n;
	n=3;
else
	TransPointMatStatus=0;
end
%+++++CALCULATE WEIGHTING FACTORS
if isempty(WeightVec)
	[WeightVec]=CalcPolWeight(PointMat,StdMat,ENames,chWeight);
end

%CALCULATE STARTING POINT OF SEARCH PStart

MaxPointMat=max(PointMat);
MinPointMat=min(PointMat);

PStart=(MaxPointMat+MinPointMat)./2;


CenterOld=PStart;
CenterNew=PStart;
BoxIter=[3 3 3];
BoxWidth=[MaxPointMat-PStart];
stop=[];
iter=0;

%DEFINITION OF MAXIMAL TOLERANCE
tol=0.001;
MaxIter=200;
NIter=0;
%CALCULATION: 
clc;
fprintf('Calculate center of best fitting sphere:   ');
while isempty(stop)
    NIter=NIter+1;
	fprintf('.');
	[M]=CalcBox(CenterNew,BoxWidth,BoxIter);
	size_M=size(M);
	SumDistVec=zeros(1,size_M(1));
	for i=1:size_M(1);
		dist=[];
		for j=1:m	
			dist(j)=sqrt(sum((PointMat(j,:)-M(i,:)).^2));
		end
		MeanDist=sum(abs(dist.*WeightVec))./sum(abs(WeightVec));
		SumDistVec(i)=sum(abs(dist-MeanDist).*WeightVec);
	end
	[MinSumDist,MinSumDistInd]=min(SumDistVec);
	CenterNew=M(MinSumDistInd,:);
	if MinSumDistInd==14
		iter=iter+1;
		BoxWidth=BoxWidth*2./BoxIter;
		CenterIter(iter,:)=CenterNew;	
		for j=1:n
			dist(j)=sqrt(sum((PointMat(j,:)-M(MinSumDistInd,:)).^2));
		end
		RadiusIter(iter)=mean(dist);
		if max(BoxWidth)<tol | NIter==MaxIter
			stop='stop';
		end
	end
	CenterOld=CenterNew;
end
Center=CenterNew;
Radius=RadiusIter(iter);

%CALCULATE NEW AND OLD POINTS ON UNIT SPHERE 
for i=1:m
	PointMatIn(i,:)=PointMat(i,:)-Center;
	PointMatOut(i,:)=PointMatIn(i,:)./sqrt(sum(PointMatIn(i,:).^2)).*Radius;
end

%CALCULATE DISTANCE BETWEEN ORIGINAL AND CALCULATED POINTS IN PER HUNDRED OF Radius
PointMatDiff=PointMatIn-PointMatOut;
for i=1:m
	PointVecDiff(i)=sqrt(sum(PointMatDiff(i,:).^2)).*100./Radius;
end
clc;
fprintf('\n\n\n');
fprintf('Calculated center: x = %g ; y = %g ; z = %g \n\n\n',Center);
fprintf('Difference between original polhemus data\n');
fprintf('and calculated points on the unit sphere in per hundred:\n\n');
disp(PointVecDiff);
pause(3)

[PointMatSphere] = change_sphere_cart(PointMatOut, [], -1);

if TransPointMatStatus
	PointMatOut=PointMatOut';
	PointMatSphere=PointMatSphere';
end
return;
