#include "homography.h"


 int c_Homography::computeHomo4(CvPoint2D32f* X, CvPoint2D32f* U) 
 {
      CvMat *x = cvCreateMat(4, 2, CV_32FC1);
      CvMat *y = cvCreateMat(4, 2, CV_32FC1);
	  for (int i=0; i<4; i++)
	  {
		cvmSet(x, i, 0, X[i].x); 
		cvmSet(x, i, 1, X[i].y); 
		cvmSet(y, i, 0, U[i].x); 
		cvmSet(y, i, 1, U[i].y); 
	  }
	  cvFindHomography(x,y,Matrix);
	  cvInvert(Matrix, invMatrix, 0);
	  cvReleaseMat(&x);
	  cvReleaseMat(&y);

    return 1;
 }

 void c_Homography::XtoU(CvPoint2D32f* X, CvPoint2D32f* U)
 {
    float divider =  cvmGet(Matrix,2,0)*X->x + cvmGet(Matrix,2,1)*X->y + cvmGet(Matrix,2,2);
    if (divider!=0)
    {
      U->x = (cvmGet(Matrix,0,0)*X->x+cvmGet(Matrix,0,1)*X->y+cvmGet(Matrix,0,2))/divider;
      U->y = (cvmGet(Matrix,1,0)*X->x+cvmGet(Matrix,1,1)*X->y+cvmGet(Matrix,1,2))/divider;
    }
 }

 void c_Homography::UtoX(CvPoint2D32f* U, CvPoint2D32f* X)
 {
    float divider =  cvmGet(invMatrix,2,0)*U->x + cvmGet(invMatrix,2,1)*U->y + cvmGet(invMatrix,2,2);
    if (divider!=0)
    {
      X->x = (cvmGet(invMatrix,0,0)*U->x+cvmGet(invMatrix,0,1)*U->y+cvmGet(invMatrix,0,2))/divider;
      X->y = (cvmGet(invMatrix,1,0)*U->x+cvmGet(invMatrix,1,1)*U->y+cvmGet(invMatrix,1,2))/divider;
    }
 }

