function E = proyError(p,q,w)
	qpredicho=proy2d(p,w);
	E=sum((q-qpredicho).^2)/size(q,'c');

function E = proyDif(p,q,w)
	qpredicho=proy2d(p,w);
	E=sum(abs(q-qpredicho))/size(q,'c')/2;

// primero se rota sobre eje x, luego sobre eje y, finalmente sobre eje z
// en sentido contrario a punteros del reloj
function R = Rot(w1,w2,w3)
        R=[cos(w3) sin(w3) 0;-sin(w3) cos(w3) 0;0 0 1] * ...
          [cos(w2) 0 -sin(w2);0 1 0; sin(w2) 0 cos(w2)] * ...
	  [1 0 0;0 cos(w1) sin(w1);0 -sin(w1) cos(w1)]

function R = Rotf(v)
        s1 = sin(v(1)); c1 = cos(v(1));
        s2 = sin(v(2)); c2 = cos(v(2));
        s3 = sin(v(3)); c3 = cos(v(3));
	R=[c3*c2,s3*c1+c3*s2*s1,s3*s1-c3*s2*c1;
	   -s3*c2,c3*c1-s3*s2*s1,c3*s1+s3*s2*c1;
	   s2,-s1*c2,c1*c2]

function R = Rotv(v)
	R=Rot(v(1),v(2),v(3))

// rotacion para apuntar a p2 desde p1 (eje z apunta, eje x se mantiene en el
// plano horizontal)
function R = apunta(p1,p2)
  Rz = p2-p1; Rz = Rz/sqrt(sum(Rz.*Rz));
  if Rz(1)==0 & Rz(2)==0 then
    Rx = [1;0;0]
  elseif abs(Rz(1))>abs(Rz(2)) then
    Rx = [-Rz(2)/Rz(1);1;0]
  else
    Rx = [1;-Rz(1)/Rz(2);0]
  end
  Rx = Rx/sqrt(sum(Rx.*Rx));
  Ry = [ Rx(3)*Rz(2)-Rx(2)*Rz(3);
         Rx(1)*Rz(3)-Rx(3)*Rz(1);
         Rx(2)*Rz(1)-Rx(1)*Rz(2) ];
  R = [Rx Ry Rz]';

// funcion para obtener angulos a partir de matriz de rotacion
function a = toEuler(R)
  alfa = atan(-R(3,2)/R(3,3))
  beta = asin(R(3,1))
  gama = atan(-R(2,1)/R(1,1))
  a = [alfa beta gama];

function qp = proy2d(p,w)
	R=Rot(w(1),w(2),w(3));
	c=w(4:6)';
	D=w(7);
	pp=R*(p-c*ones(p(1,:)));
	qp=[pp(1,:)./abs(pp(3,:));pp(2,:)./abs(pp(3,:))]*D;

function qp = proy2d_D(p,w)
	R=Rot(w(1),w(2),w(3));
	p=p/w(7);
	c=w(4:6)';
	D=1;
	pp=R*(p-c*ones(p(1,:)));
	qp=[pp(1,:)./pp(3,:);pp(2,:)./pp(3,:)]*D;
		

function g = pEGrad(p,q,w)
	g=zeros(w);
	delta=1e-7;
	E0=proyError(p,q,w);
	for i=1:length(w)-gD
		valor=w(i);
		w(i)=valor-delta; e0=proyError(p,q,w);
		w(i)=valor+delta; e1=proyError(p,q,w);
		w(i)=valor;
		g(i)=(e1-e0)/2/delta;
	end
			
function w = ciclo(p,q,w0,num,b)
	[nargout,nargin]=argn(0);
	if nargin<5 then b = 1; end;
	global('multpaso','e0');
	wtemp=w0;
	for i=1:num
		wtemp = buscagc(p,q,wtemp);
	        if b>0 & modulo(i,b)==0
			printf("%5i %e %e\n",i,e0,multpaso);
		end
	end
	w=wtemp;
			
function initciclo()
	global('d','multpaso');
	d = []
	multpaso = [];

function w = busca(p,q,w0)
	global('multpaso','d','e0');
	if multpaso==[] | multpaso<=0 then multpaso = .001; end
	f = multpaso;
	e0 = proyError(p,q,w0);
	w = w0 + multpaso*d;
	e1 = proyError(p,q,w);
	if e1 > e0
		while e1 > e0
			e2 = e1;
			f = f/2;
			w(:) = w0 + f*d;
			e1 = proyError(p,q,w);
		end
	else
		w = w0 + 2*f*d;
		e2 = proyError(p,q,w);
		while e2 < e1
			e1 = e2;
			f = f*2;
			w(:) = w0 + 2*f*d;
			e2 = proyError(p,q,w);
		end
	end
	if 2*e0-4*e1+2*e2 <> 0
		w(:) = w0 + (e0-e1)/(2*e0-4*e1+2*e2)*f*d;
		e = proyError(p,q,w);
		if e < e1
			f = (e0-e1)/(2*e0-4*e1+2*e2)*f;
		end
	end
	d = f*d;
	w(:) = w0 + d
	multpaso = .8*multpaso + .2*f;

function w = buscalin(p,q,w)
	global('d','g0')
	if d==[] then 
		g0 = pEGrad(p,q,w); d = -g0;
	else
	g0(:) = pEGrad(p,q,w); d(:) = -g0;
	end
	w(:) = busca(p,q,w)

function w = buscagc(p,q,w)
	global('d','g1','g0')
	if d==[] then 
		w(:) = buscalin(p,q,w);
	else
		if g1==[] then g1 = zeros(g0); end
		g1(:) = pEGrad(p,q,w);
                d(:) = -g1 + (g1*(g1-g0)')/(g0*g0')*d;
                if d*g1' >= 0 then
			d(:) = -g1; //printf("*");
		end
                g0(:) = g1;
                w(:) = busca(p,q,w);
	end

function wf = normaliza(w)
	wf=w;
	for i=1:3
		wf(i)=pmodulo(wf(i),2*%pi);
		if wf(i)>=%pi then
			wf(i)=wf(i)-2*%pi
		end
	end

function [w,wi,p,wo,qo,qm] = proyej(iter,D)
	p = rand(3,10)-.5;
	wo = [0 0 .1 0 0 -1 1];
	qo = proy2d(p,wo);
	qm = qo + (rand (qo) - 0.5)/100;
	E=1;
	while E>1e-2
		wi = [(rand(1,3)-.5)*2*%pi (rand(1,3)-.5)*10 D];
		R = Rot(wi(1),wi(2),wi(3));
		if R(3,:)*wi(4:6)' > 0 then wi(1) = wi(1)+%pi; end
		w = wi;
		initciclo();
		global('gD');
		gD=1;
		w = ciclo(p,qm,w,iter/2,20);
		gD=0;	
		E=proyError(p,qo,w);
		disp(E);
	end
	w=ciclo(p,qm,w,iter/2,20);
	w=normaliza(w);
	printf("\ndif original(+)/reconstruccion(x) %f\n",proyDif(p,qo,w));
	q = proy2d(p,w);
	xbasc();
	plot2d(qo(1,:),-qo(2,:),style=-1,frameflag=4);
	plot2d(q(1,:),-q(2,:),style=-2,frameflag=4);

	
function [w,wi,p,wo,qo,qm] = proyej2(iter)
	p = rand(3,10)-.5;	
	wo = [0 0 .1 0 0 -1 1];
	qo = proy2d_D(p,wo);
	qm = qo + (rand (qo) - 0.5)/100;
	wi = [(rand(1,3)-.5)*2*%pi (rand(1,3)-.5)*10 1];
	R = Rot(wi(1),wi(2),wi(3));
	if R(3,:)*wi(4:6)' > 0 then wi(1) = wi(1)+%pi; end
	w = wi;
	global('gD');
	gD=0;
	initciclo();
	w = ciclo(p,qm,w,iter,20);
	q = proy2d_D(p,w);
	xbasc();
	plot2d(qo(1,:),-qo(2,:),style=-1,frameflag=4);
	plot2d(q(1,:),-q(2,:),style=-2,frameflag=4);