#include "stereo.h"


Matrix stereoEssencialMatrix(Matrix Ra, Vector eye_a, Matrix Rb, Vector eye_b) 
{
   Matrix Rba = algMult(Rb,algTransp(Ra));
   Vector eye = algTransf(Ra,algSub(eye_b,eye_a));
   Matrix S   = algVectorProductMatrix(eye);
   Matrix E   = algMult(Rba,S);
   return E;
}


Matrix stereoFundamentalMatrix(Matrix Ka, Matrix Ra, Vector eye_a, Matrix Kb, Matrix Rb, Vector eye_b)
{
   Matrix E = stereoEssencialMatrix(Ra,eye_a,Rb,eye_b);
   Matrix invKa = algInv(Ka);
   Matrix invKbTransp = algTransp(algInv(Kb));
   Matrix tmp = algMult(invKbTransp,E);
   Matrix F = algMult(tmp,invKa);
   return F;
}


void stereoEpipolarLineEquation(double x, double y, Matrix F, double* a, double *b, double *c) 
{
   Vector p = algVector(x,y,0,1);
   Vector line = algTransf(F,p);
   *a = algGetX(line);
   *b = algGetY(line);
   *c = algGetW(line);
}

void stereoEpipolarLine(double xm, double ym, Matrix F, int width, double* x0, double* y0, double* x1, double* y1)
{
   double a,b,c;  /* ax+by+c = 0 */
   double x;

   Vector p = algVector(xm,ym,0.,1.);
   Vector line = algTransf(F,p);
   
   a = algGetX(line);
   b = algGetY(line);
   c = algGetW(line);
   
    x  = 0.;           /* ponto a esquerda */
   *x0 = x;               
   *y0 = -(a*x+c)/b;
   
   x= (double) width; /* ponto a direita */
   *x1= x;            
   *y1 = -(a*x+c)/b;
}


Vector stereoReconstruct(double xa, double ya, double fa, Matrix Ra, Vector eye_a, double cxa, double cya,
                         double xb, double yb, double fb, Matrix Rb, Vector eye_b, double cxb, double cyb)
{
   Vector pb_a,n,eye,c,Pa;
   Matrix Rab,A;
   Vector pa = algVector(xa-cxa,ya-cya,-fa,1.);
   Vector pb = algVector(xb-cxb,yb-cyb,-fb,1.);

   pa = algUnit(pa);  /* utilizamos vetores unitarios para podermos comparar as coordenadas */
   pb = algUnit(pb);

   Rab = algMult(Ra,algTransp(Rb));  /* Matriz que leva de b para a */
   pb_a = algTransf(Rab,pb);
   n = algCross(pa,pb_a);
   eye = algTransf(Ra,algSub(eye_b,eye_a));
   A = algMatrix4x4(pa.x, pb_a.x, n.x, 0.,
                    pa.y, pb_a.y, n.y, 0.,
                    pa.z, pb_a.z, n.z, 0.,
                      0.,     0.,  0., 1.);
   c = algSolve(A,eye);
   Pa = algAdd(algScale(c.x,pa),algScale(c.z/2,n));
   
   if (c.z*c.z>0.0001*(c.x*c.x+c.y*c.y)) 
   {
	   printf("ERRO:  RAIOS MUITO SEPARADOS\n");
	   Pa.x = 1000000; Pa.y = 1000000; Pa.z = 1000000;
   }

  return Pa;
}