#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include "tcgVisionGeom.h"

#ifndef SWAP
#define SWAP(a,b) {(temp)=(a);(a)=(b);(b)=(temp);}
#endif

#ifndef SQR
#define SQR(a) ((a)*(a))
#endif

#ifndef ABS
#define ABS(a) (((a)<0)?-(a):(a))
#endif

#ifndef MAX
#define MAX(a,b) (((a)>(b))? (a): (b))
#endif

#ifndef MIN
#define MIN(a,b) (((a)<(b))? (a): (b))
#endif

#ifndef _PI_
#define _PI_ 3.1415926535897932384626433832795
#endif

#define TOL 1e-10

#define MAXSPTS	256
#define MAXS 269

static double X[MAXS], M[MAXS*MAXS];

static double normalize3d(tcgVisionPoint3D *p)
{
	double n = p->x*p->x + p->y*p->y + p->z*p->z;
	if (n) {
		n = sqrt(n);
		p->x /= n;
		p->y /= n;
		p->z /= n;
	}
	return n;
}

static int Gauss (double a[MAXS*MAXS], int n, double b[MAXS])
{
	int    i, j, k;
	int    imax;
	double amax, rmax, temp;

	for (j=0; j<n-1; j++) {
		rmax = 0.0;
		imax = j;
		for (i=j; i<n; i++) {
			amax = 0.0;
			for (k=j; k<n; k++)
				if (ABS(a[n*i+k]) > amax)
					amax = ABS(a[n*i+k]);
			if (amax < TOL) return 0;
			else if ((ABS(a[n*i+j]) > rmax*amax) && (ABS(a[n*i+j]) >= TOL)) {
				rmax = ABS(a[n*i+j]) / amax;
				imax = i;
			}
		}
		if (ABS(a[n*imax+j])<TOL) {
			for (k=imax+1; k<n; k++)
				if (ABS(a[n*k+j]) < TOL)
					imax = k;
			if (ABS(a[n*imax+j]) < TOL)
				return 0;
		}
		if (imax != j) {
			for (k=j; k<n; k++)
				SWAP(a[n*imax+k], a[n*j+k]);
			SWAP(b[imax], b[j]);
		}
		for (i=j+1; i<n; i++) {
			double aux = a[n*i+j] / a[n*j+j];
			for (k=j+1; k<n; k++)
				a[n*i+k] -= aux * a[n*j+k];
			b[i] -= aux * b[j];
		}
	}
	if (ABS(a[n*(n-1)+n-1]) <= TOL)
		return 0;
	else {
		b[n-1] /= a[n*(n-1)+n-1];
		for (i=n-2; i>=0; i--) {
			for (j=i+1; j<n; j++)
				b[i] -= a[n*i+j] * b[j];
			b[i] /= a[n*i+i];
		}
	}
	return 1;
}

int tcgVisionCameraFindTransf2DPoints(double *H, int np, tcgVisionPoint2D *p1, tcgVisionPoint2D *p2)
{
	int i, j, k, r;
	int ne, ni;
	double *A;
	double *C;

	if (np < 4) return 0;

	ne = 2*np;
	ni = 9;

	A = (double *)malloc(ne*ni*sizeof(double));
	C = (double *)malloc(ne*sizeof(double));

	for (i=0; i<ne*ni; i++)
		A[i] = 0.0;
	for (i=0; i<np; i++) {
		A[ni*(i+np)+3] = A[ni*i  ] = p1[i].x;
		A[ni*(i+np)+4] = A[ni*i+1] = p1[i].y;
		A[ni*(i+np)+5] = A[ni*i+2] = 1.0;
		A[ni*i+6] = -p2[i].x*p1[i].x;
		A[ni*i+7] = -p2[i].x*p1[i].y;
		A[ni*i+8] = -p2[i].x;
		A[ni*(i+np)+6] = -p2[i].y*p1[i].x;
		A[ni*(i+np)+7] = -p2[i].y*p1[i].y;
		A[ni*(i+np)+8] = -p2[i].y;
	}

	for (i=0; i<9; i++)
		C[i] = 0.0;
	for (i=0; i<np; i++) {
		C[6] += p1[i].x;
		C[7] += p1[i].y;
	}
	C[6] /= np;
	C[7] /= np;
	C[8] = 1.0;

	for (i=0; i<100; i++)
		M[i] = 0.0;
	for (i=0; i<9; i++)
		for (j=0; j<9; j++)
			for (k=0; k<ne; k++)
				M[10*i+j] += A[ni*k+j]*A[ni*k+i];
	for (i=0; i<9; i++) {
		M[10*i+9]= C[i];
		M[10*9+i]= C[i];
	}
	for (i=0; i<9; i++) X[i]= 0.0;
	X[9] = 1.0;

	r = Gauss (M, 10, X);
	for (i=0; i<9; i++) H[i] = X[i];

	free(C);
	free(A);
	return r;
}

int tcgVisionCameraFindTransf2DLines(double *H, int nl, tcgVisionLine2D *l1, tcgVisionLine2D *l2)
{
	int i, j, k, r;
	double aa, bb, cc;
	double AA, BB, CC;
	int ne, ni;
	double *A;
	double *C;

	if (nl < 3) return 0;

	ne = 3*nl;
	ni = 9+nl-1;

	A = (double *)malloc(ne*ni*sizeof(double));
	C = (double *)malloc(ne*sizeof(double));

	for (i=0; i<ne*ni; i++)
		A[i] = 0.0;
	for (i=0; i<nl-1; i++) {
		tcgVisionGeomGetCoefs(l1[i], &AA, &BB, &CC);
		tcgVisionGeomGetCoefs(l2[i], &aa, &bb, &cc);
		A[ni*(3*i+2)+6] = A[ni*(3*i+1)+3] = A[ni*(3*i)  ] = AA;
		A[ni*(3*i+2)+7] = A[ni*(3*i+1)+4] = A[ni*(3*i)+1] = BB;
		A[ni*(3*i+2)+8] = A[ni*(3*i+1)+5] = A[ni*(3*i)+2] = CC;
		A[ni*(3*i)+9+i] = aa;
		A[ni*(3*i+1)+9+i] = bb;
		A[ni*(3*i+2)+9+i] = cc;
	}
	tcgVisionGeomGetCoefs(l1[i], &AA, &BB, &CC);
	tcgVisionGeomGetCoefs(l2[i], &aa, &bb, &cc);
	A[ni*(3*i+2)+6] = A[ni*(3*i+1)+3] = A[ni*(3*i)  ] = AA;
	A[ni*(3*i+2)+7] = A[ni*(3*i+1)+4] = A[ni*(3*i)+1] = BB;
	A[ni*(3*i+2)+8] = A[ni*(3*i+1)+5] = A[ni*(3*i)+2] = CC;

	for (i=0; i<3*(nl-1); i++)
		C[i] = 0.0;
	C[i  ] = aa;
	C[i+1] = bb;
	C[i+2] = cc;

	for (i=0; i<ni; i++) {
		for (j=0; j<ni; j++)
			for (M[ni*i+j]=A[j]*A[i], k=1; k<ne; k++)
				M[ni*i+j] += A[ni*k+j]*A[ni*k+i];
		for (X[i]=A[i]*C[0], k=1; k<ne; k++)
			X[i] += A[ni*k+i]*C[k];
	}

	r = Gauss (M, ni, X);
	for (i=0; i<9; i++) H[i] = X[i];

	free(C);
	free(A);
	return r;
}

int tcgVisionCameraFindTransf2D(double *H, int nl, tcgVisionLine2D *l1, tcgVisionLine2D *l2, int np, tcgVisionPoint2D *p1, tcgVisionPoint2D *p2)
{
	int i, j, k, r;
	double aa, bb, cc;
	double AA, BB, CC;
	int ne, ni;
	double *A;
	double *C;

	if (nl<1 && np<1 && nl+np<4) return 0;

	ne = 3*nl+2*np+1;
	ni = 9+nl;

	A = (double *)malloc(ne*ni*sizeof(double));
	C = (double *)malloc(ne*sizeof(double));

	for (i=0; i<ne*ni; i++)
		A[i] = 0.0;
	for (i=0; i<nl; i++) {
		tcgVisionGeomGetCoefs(l1[i], &AA, &BB, &CC);
		tcgVisionGeomGetCoefs(l2[i], &aa, &bb, &cc);
		A[ni*(3*i+2)+2] = A[ni*(3*i+1)+1] = A[ni*(3*i)  ] = aa;
		A[ni*(3*i+2)+5] = A[ni*(3*i+1)+4] = A[ni*(3*i)+3] = bb;
		A[ni*(3*i+2)+8] = A[ni*(3*i+1)+7] = A[ni*(3*i)+6] = cc;
		A[ni*(3*i)+9+i] = AA;
		A[ni*(3*i+1)+9+i] = BB;
		A[ni*(3*i+2)+9+i] = CC;
	}

	for (i=0; i<np; i++) {
		A[ni*(i+np+3*nl)+3] = A[ni*(i+3*nl)  ] = p1[i].x;
		A[ni*(i+np+3*nl)+4] = A[ni*(i+3*nl)+1] = p1[i].y;
		A[ni*(i+np+3*nl)+5] = A[ni*(i+3*nl)+2] = 1.0;
		A[ni*(i+3*nl)+6] = -p2[i].x*p1[i].x;
		A[ni*(i+3*nl)+7] = -p2[i].x*p1[i].y;
		A[ni*(i+3*nl)+8] = -p2[i].x;
		A[ni*(i+np+3*nl)+6] = -p2[i].y*p1[i].x;
		A[ni*(i+np+3*nl)+7] = -p2[i].y*p1[i].y;
		A[ni*(i+np+3*nl)+8] = -p2[i].y;
	}
	for (i=0; i<np; i++) {
		A[ni*(ne-1)+6] += p1[i].x;
		A[ni*(ne-1)+7] += p1[i].y;
	}
	A[ni*(ne-1)+6] /= np;
	A[ni*(ne-1)+7] /= np;
	A[ni*(ne-1)+8] = 1.0;

	for (i=0; i<ne-1; i++)
		C[i] = 0.0;
	C[i] = 1.0;

	for (i=0; i<ni; i++) {
		for (j=0; j<ni; j++)
			for (M[ni*i+j]=A[j]*A[i], k=1; k<ne; k++)
				M[ni*i+j] += A[ni*k+j]*A[ni*k+i];
		for (X[i]=A[i]*C[0], k=1; k<ne; k++)
			X[i] += A[ni*k+i]*C[k];
	}

	r = Gauss (M, ni, X);
	for (i=0; i<9; i++) H[i] = X[i];

	free(C);
	free(A);
	return r;
}

int tcgVisionCameraTransfConvert_LinesPoints(double h1[9], double h2[9])
{
	double d = h1[0]*h1[4]*h1[8] + h1[1]*h1[5]*h1[6] + h1[2]*h1[3]*h1[7] - h1[2]*h1[4]*h1[6] - h1[0]*h1[5]*h1[7] - h1[1]*h1[3]*h1[8];

	if (d) {
		d = 1.0/d;
		h2[0] =  (h1[4]*h1[8] - h1[5]*h1[7])*d; h2[1] = -(h1[3]*h1[8] - h1[5]*h1[6])*d; h2[2] =  (h1[3]*h1[7] - h1[4]*h1[6])*d;
		h2[3] = -(h1[1]*h1[8] - h1[2]*h1[7])*d; h2[4] =  (h1[0]*h1[8] - h1[2]*h1[6])*d; h2[5] = -(h1[0]*h1[7] - h1[1]*h1[6])*d;
		h2[6] =  (h1[1]*h1[5] - h1[2]*h1[4])*d; h2[7] = -(h1[0]*h1[5] - h1[2]*h1[3])*d; h2[8] =  (h1[0]*h1[4] - h1[1]*h1[3])*d;
		return 1;
	}
	return 0;
}

int tcgVisionCameraTransfPoint(double *H, tcgVisionPoint2D p1, tcgVisionPoint2D *p2)
{
	double z;

	z = H[6]*p1.x + H[7]*p1.y + H[8];
	if (z) {
		p2->x = (H[0]*p1.x + H[1]*p1.y + H[2])/z;
		p2->y = (H[3]*p1.x + H[4]*p1.y + H[5])/z;
		return 1;
	}
	p2->x = H[0]*p1.x + H[1]*p1.y + H[2];
	p2->y = H[3]*p1.x + H[4]*p1.y + H[5];
	return 0;
}

void tcgVisionCameraTransfLine(double *H, double a1, double b1, double c1, double *a2, double *b2, double *c2)
{
	*a2 = H[0]*a1 + H[1]*b1 + H[2]*c1;
	*b2 = H[3]*a1 + H[4]*b1 + H[5]*c1;
	*c2 = H[6]*a1 + H[7]*b1 + H[8]*c1;
}

int tcgVisionCameraFindTransf3Dto2DPoints(double *H, int np, tcgVisionPoint3D *p1, tcgVisionPoint2D *p2)
{
	int i, j, k, r;
	int ne, ni;
	double norma_H;
	double *A;
	double *C;

	ne = 2*np;
	ni = 12;

	A = (double *)malloc(ne*ni*sizeof(double));
	C = (double *)malloc(ne*sizeof(double));

	for (i=0; i<2*np*12; i++)
		A[i] = 0.0;
	for (i=0; i<np; i++) {
		A[12*(i+np)+4]  = A[12*i  ]  = p1[i].x;
		A[12*(i+np)+5]  = A[12*i+1]  = p1[i].y;
		A[12*(i+np)+6]  = A[12*i+2]  = p1[i].z;
		A[12*(i+np)+7]  = A[12*i+3]  = 1.0;
		A[12*i+8]  = -p2[i].x*p1[i].x;
		A[12*i+9]  = -p2[i].x*p1[i].y;
		A[12*i+10] = -p2[i].x*p1[i].z;
		A[12*i+11] = -p2[i].x;
		A[12*(i+np)+8]  = -p2[i].y*p1[i].x;
		A[12*(i+np)+9]  = -p2[i].y*p1[i].y;
		A[12*(i+np)+10] = -p2[i].y*p1[i].z;
		A[12*(i+np)+11] = -p2[i].y;
	}

	for (i=0; i<12; i++)
		C[i] = 0.0;
	for (i=0; i<np; i++) {
		C[8]  += p1[i].x;
		C[9]  += p1[i].y;
		C[10] += p1[i].z;
	}
	C[8]  /= np;
	C[9]  /= np;
	C[10] /= np;
	C[11]  = 1.0;

	for (i=0; i<13; i++)
		for (j=0; j<13; j++)
			M[13*i+j] = 0;
	for (i=0; i<12; i++)
		for (j=0; j<12; j++)
			for (k=0; k<2*np; k++)
				M[13*i+j] += A[12*k+j]*A[12*k+i];
	for (i=0; i<12; i++) {
		M[13*i+12] = C[i];
		M[13*12+i] = C[i];
	}
	for (i=0; i<12; i++)
		X[i] = 0.0;
	X[12] = 1.0;

	r = Gauss (M, 13, X);
	for (i=0; i<12; i++) H[i] = X[i];

	norma_H = sqrt(SQR(H[8]) + SQR(H[9]) + SQR(H[10]));
	for (i=0; i<12; i++)
		H[i] /= -norma_H;

	free(C);
	free(A);

	return r;
}

int tcgVisionCameraBuildNewData(double minx, double miny, double maxx, double maxy, int width, int height, int clip, double *H,
								int npnx, int npny, tcgVisionPoint2D *p1n, tcgVisionPoint2D *p2n)
{
	int i, j, k;
	double x, y;
	double dx, dy;
	double xf0, yf0, zf0;

	dx = (maxx - minx)/(npnx - 1.0);
	dy = (maxy - miny)/(npny - 1.0);

	if (clip) {
		for (i=0, k=0, x=minx; i<npnx; i++, x+=dx)
			for (j=0, y=miny; j<npny; j++, y+=dy) {
				zf0 = 1.0/(H[6]*x + H[7]*y + H[8]);
				xf0 = (H[0]*x + H[1]*y + H[2])*zf0;
				yf0 = (H[3]*x + H[4]*y + H[5])*zf0;
				if (xf0>-(width/2) && xf0<(width/2) && yf0>-(height/2) && yf0<(height/2)) {
					p1n[k].x = x;
					p1n[k].y = y;
					p2n[k].x = xf0;
					p2n[k].y = yf0;
					k++;
				}
			}
	} else {
		for (i=0, k=0, x=minx; i<npnx; i++, x+=dx)
			for (j=0, y=miny; j<npny; j++, k++, y+=dy) {
				p1n[k].x = x;
				p1n[k].y = y;
				zf0 = 1.0/(H[6]*x + H[7]*y + H[8]);
				p2n[k].x = (H[0]*x + H[1]*y + H[2])*zf0;
				p2n[k].y = (H[3]*x + H[4]*y + H[5])*zf0;
			}
	}

	return k;
}

static int min_quad_comH(double H[9], int np, tcgVisionPoint3D *p3d, tcgVisionPoint2D *p2d, double Q[3][4])
{
	int i, j, k, r;
	double norma_Q;
	double A[3*MAXSPTS*20];
	double C[20];
	int ne, ni;

	if (np < 2) return 0;

	ne = 3*np;
	ni = 3+np;

	for (i=0; i<3*MAXSPTS*20; i++)
		A[i] = 0.0;
	for (i=0; i<np; i++) {
		A[ni*(3*i+2)+2] = A[ni*(3*i+1)+1] = A[ni*(3*i)+0] = p3d[i].z;
		A[ni*(3*i  )+i+3] = -p2d[i].x;
		A[ni*(3*i+1)+i+3] = -p2d[i].y;
		A[ni*(3*i+2)+i+3] = -1.0;
	}
	for (i=0; i<np; i++) {
		C[3*i  ] = -(H[0]*p3d[i].x + H[1]*p3d[i].y + H[2]);
		C[3*i+1] = -(H[3]*p3d[i].x + H[4]*p3d[i].y + H[5]);
		C[3*i+2] = -(H[6]*p3d[i].x + H[7]*p3d[i].y + H[8]);
	}

	for (i=0; i<ni; i++) {
		for (j=0; j<ni; j++)
			for (M[ni*i+j]=A[j]*A[i], k=1; k<ne; k++)
				M[ni*i+j] += A[ni*k+j]*A[ni*k+i];
		for (X[i]=A[i]*C[0], k=1; k<ne; k++)
			X[i] += A[ni*k+i]*C[k];
	}

	r = Gauss (M, ni, X);
	Q[0][0] = H[0]; Q[0][1] = H[1]; Q[0][2] = X[0]; Q[0][3] = H[2];
	Q[1][0] = H[3]; Q[1][1] = H[4]; Q[1][2] = X[1]; Q[1][3] = H[5];
	Q[2][0] = H[6]; Q[2][1] = H[7]; Q[2][2] = X[2]; Q[2][3] = H[8];
	norma_Q = sqrt(SQR(Q[2][0]) + SQR(Q[2][1]) + SQR(Q[2][2]));
	for (i=0; i<3; i++)
		for (j=0; j<4; j++)
			Q[i][j] /= -norma_Q;
	return r;
}

static void invert (double Q[3][4], tcgVisionPoint2D p2d, double *X)
{
	int i, j;

	for (i=0; i<3; i++)
		for (j=0; j<2; j++) 
			M[3*i+j] = Q[i][j];
	M[2] = -p2d.x; M[3*1+2] = -p2d.y; M[3*2+2] = -1.0;
	for (i=0; i<3; i++)
		X[i] = -Q[i][3];
	Gauss(M, 3, X);
}

static void FindCamera2 (tcgVisionPoint3D p0,      /* posicao da camera            */
						 tcgVisionPoint3D pf,      /* pt. que se projeta no centro */
						 int n,             /* numero de pontos             */
						 tcgVisionPoint3D *cmp3d,  /* coordenadas 3D               */
						 tcgVisionPoint2D *cmp2d,  /* numero de pontos             */
						 double Q[3][4],    /* matriz de projecao           */
						 tcgVisionPoint3D *vup)
{
	int i, j, k;
	double aux0, aux1, aux2, aux3, aux4, aux5;
	double A[2*MAXSPTS*2];	 	
	double B[2*MAXSPTS];
	double r;
	tcgVisionPoint3D dr, t;

	dr.x = pf.x - p0.x; dr.y = pf.y - p0.y; dr.z = pf.z - p0.z;
	normalize3d(&dr);
	aux0 = 1.0/dr.z;
	aux1 = dr.y*dr.y*aux0 + dr.z;
	aux2 = dr.x*dr.y*aux0;
	aux3 = dr.x*dr.x*aux0 + dr.z;
	aux4 = dr.x*aux0;
	aux5 = dr.y*aux0;
	for (i=0; i<n; i++) {
		t.x = cmp3d[i].x-p0.x; t.y = cmp3d[i].y-p0.y; t.z = cmp3d[i].z-p0.z;
		r = 1.0/(dr.x*t.x+ dr.y*t.y+ dr.z*t.z);

		A[2*i  ] = (t.x - t.z*aux4)*r;
		A[2*i+1] = (t.y - t.z*aux5)*r;
		B[i] = cmp2d[i].x;

		A[2*(i+n)  ] = (t.x*aux2 - t.y*aux3 + dr.y*t.z)*r;
		A[2*(i+n)+1] = (t.x*aux1 - t.y*aux2 - dr.x*t.z)*r;
		B[i+n] = cmp2d[i].y;
	}

	for (i=0; i<2; i++) {
		for (j=0; j<2; j++)
			for (M[2*i+j]=0, k=0; k<2*n; k++)
				M[2*i+j] += A[2*k+j]*A[2*k+i];
		for (X[i]=0, k=0; k<2*n; k++)
			X[i] += A[2*k+i]*B[k];
	}
	Gauss (M, 2, X);

	Q[0][0] = X[0]; Q[0][1] = X[1]; Q[0][2] = -(dr.x*X[0] + dr.y*X[1])/dr.z;
	Q[1][0] = dr.z*Q[0][1] - dr.y*Q[0][2];
	Q[1][1] = dr.x*Q[0][2] - dr.z*Q[0][0];
	Q[1][2] = dr.y*Q[0][0] - dr.x*Q[0][1];
	Q[2][0] = dr.x; Q[2][1] = dr.y; Q[2][2] = dr.z;
	vup->x = Q[1][0]; vup->y = Q[1][1]; vup->z = Q[1][2];
	normalize3d(vup);
	Q[0][3] = -(Q[0][0]*p0.x + Q[0][1]*p0.y + Q[0][2]*p0.z);
	Q[1][3] = -(Q[1][0]*p0.x + Q[1][1]*p0.y + Q[1][2]*p0.z);
	Q[2][3] = -(Q[2][0]*p0.x + Q[2][1]*p0.y + Q[2][2]*p0.z);
}

static double FindCameraError (tcgVisionPoint3D p0,      /* posicao da camera            */
							   tcgVisionPoint3D pf,      /* pt. que se projeta no centro */
							   int nl,            /* numero de linhas             */
							   tcgVisionLine2D *l1,      /* linhas do modelo             */
							   tcgVisionLine2D *l2,      /* linhas da tela          */
							   int np,             /* numero de pontos             */
							   tcgVisionPoint3D *cmp3d,  /* coordenadas 3D               */
							   tcgVisionPoint2D *cmp2d)  /* coordenadas de tela          */
{
	int i, j, k;
	double aux0, aux1, aux2, aux3, aux4, aux5;
	double r, erro;
	double A[2*MAXSPTS][2];	 	
	double B[2*MAXSPTS];
	tcgVisionPoint3D dr, t;
	double Q[3][4];
	double H[9], H2[9];

	dr.x = pf.x - p0.x; dr.y = pf.y - p0.y; dr.z = pf.z - p0.z;
	normalize3d(&dr);
	aux0 = 1.0/dr.z;
	aux1 = dr.y*dr.y*aux0 + dr.z;
	aux2 = dr.x*dr.y*aux0;
	aux3 = dr.x*dr.x*aux0 + dr.z;
	aux4 = dr.x*aux0;
	aux5 = dr.y*aux0;
	for (i=0; i<np; i++) {
		t.x = cmp3d[i].x-p0.x; t.y = cmp3d[i].y-p0.y; t.z = cmp3d[i].z-p0.z;
		r = 1.0/(dr.x*t.x+ dr.y*t.y+ dr.z*t.z);

		A[i][0] = (t.x - t.z*aux4)*r;
		A[i][1] = (t.y - t.z*aux5)*r;
		B[i] = cmp2d[i].x;

		A[i+np][0] = (t.x*aux2 - t.y*aux3 + dr.y*t.z)*r;
		A[i+np][1] = (t.x*aux1 - t.y*aux2 - dr.x*t.z)*r;
		B[i+np] = cmp2d[i].y;
	}

	for (i=0; i<2; i++) {
		for (j=0; j<2; j++)
			for (M[2*i+j]=0, k=0; k<2*np; k++)
				M[2*i+j] += A[k][j]*A[k][i];
		for (X[i]=0, k=0; k<2*np; k++)
			X[i] += A[k][i]*B[k];
	}
	Gauss (M, 2, X);

	for (erro=0, k=0; k<2*np; k++) {
		aux0 = A[k][0]*X[0] + A[k][1]*X[1] - B[k];
		erro += SQR(aux0);
	}

	Q[0][0] = X[0]; Q[0][1] = X[1]; Q[0][2] = -(dr.x*X[0] + dr.y*X[1])/dr.z;
	Q[1][0] = dr.z*Q[0][1] - dr.y*Q[0][2];
	Q[1][1] = dr.x*Q[0][2] - dr.z*Q[0][0];
	Q[1][2] = dr.y*Q[0][0] - dr.x*Q[0][1];
	Q[2][0] = dr.x; Q[2][1] = dr.y; Q[2][2] = dr.z;
	Q[0][3] = -(Q[0][0]*p0.x + Q[0][1]*p0.y + Q[0][2]*p0.z);
	Q[1][3] = -(Q[1][0]*p0.x + Q[1][1]*p0.y + Q[1][2]*p0.z);
	Q[2][3] = -(Q[2][0]*p0.x + Q[2][1]*p0.y + Q[2][2]*p0.z);

	H[0] = Q[0][0]; H[1] = Q[0][1]; H[2] = Q[0][3];
	H[3] = Q[1][0]; H[4] = Q[1][1]; H[5] = Q[1][3];
	H[6] = Q[2][0]; H[7] = Q[2][1]; H[8] = Q[2][3];
	tcgVisionCameraTransfConvert_LinesPoints(H, H2);

	for (i=0; i<nl; i++) {
		double a, b, c;
		double A, B, C;
		double d1, d2;
		tcgVisionGeomGetCoefs(l1[i], &a, &b, &c);
		A = H2[0]*a + H2[1]*b + H2[2]*c;
		B = H2[3]*a + H2[4]*b + H2[5]*c;
		C = H2[6]*a + H2[7]*b + H2[8]*c;
		d1 = A*l2[i].v1.x + B*l2[i].v1.y + C;
		d2 = A*l2[i].v2.x + B*l2[i].v2.y + C;
		erro += (SQR(d1) + SQR(d2))/(A*A + B*B);
	}

	return erro;
}

#define h (1e-6)

static void mrqcof_CC(tcgVisionPoint3D p0, tcgVisionPoint3D pf, double alpha[5][5], double beta[5],
					  int nl, tcgVisionLine2D *l1, tcgVisionLine2D *l2, int np, tcgVisionPoint3D *cmp3d, tcgVisionPoint2D *cmp2d, double d)
{
	tcgVisionPoint3D v0, v1, v2, v3;
	double den, den2;
	double dx, dy, dz, Dx, Dy,
		dxdx, dxdy, dxdz, dxDx, dxDy,
		dydy, dydz, dyDx, dyDy,
		dzdz, dzDx, dzDy,
		DxDx, DxDy,
		DyDy;

	den = 1.0/(2.0*h);
	den2 = SQR(den);

	v0.x = p0.x+h; v0.y = p0.y; v0.z = p0.z;
	v1.x = p0.x-h; v1.y = p0.y; v1.z = p0.z;
	dx = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den;

	v0.x = p0.x; v0.y = p0.y+h; v0.z = p0.z;
	v1.x = p0.x; v1.y = p0.y-h; v1.z = p0.z;
	dy   = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den;

	v0.x = p0.x; v0.y = p0.y; v0.z = p0.z+h;
	v1.x = p0.x; v1.y = p0.y; v1.z = p0.z-h;
	dz   = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den;

	v0.x = pf.x+h; v0.y = pf.y; v0.z = pf.z;
	v1.x = pf.x-h; v1.y = pf.y; v1.z = pf.z;
	Dx   = (FindCameraError(p0, v0, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(p0, v1, nl, l1, l2, np, cmp3d, cmp2d))*den;

	v0.x = pf.x; v0.y = pf.y+h; v0.z = pf.z;
	v1.x = pf.x; v1.y = pf.y-h; v1.z = pf.z;
	Dy   = (FindCameraError(p0, v0, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(p0, v1, nl, l1, l2, np, cmp3d, cmp2d))*den;

	v0.x = p0.x+2*h; v0.y = p0.y; v0.z = p0.z;
	v1.x = p0.x-2*h; v1.y = p0.y; v1.z = p0.z;
	dxdx = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - 2*d + FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den2;

	v0.x = p0.x; v0.y = p0.y+2*h; v0.z = p0.z;
	v1.x = p0.x; v1.y = p0.y-2*h; v1.z = p0.z;
	dydy = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - 2*d + FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den2;

	v0.x = p0.x; v0.y = p0.y; v0.z = p0.z+2*h;
	v1.x = p0.x; v1.y = p0.y; v1.z = p0.z-2*h;
	dzdz = (FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - 2*d + FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d))*den2;

	v0.x = pf.x+2*h; v0.y = pf.y; v0.z = pf.z;
	v1.x = pf.x-2*h; v1.y = pf.y; v1.z = pf.z;
	DxDx = (FindCameraError(p0, v0, nl, l1, l2, np, cmp3d, cmp2d) - 2*d + FindCameraError(p0, v1, nl, l1, l2, np, cmp3d, cmp2d))*den2;

	v0.x = pf.x; v0.y = pf.y+2*h; v0.z = pf.z;
	v1.x = pf.x; v1.y = pf.y-2*h; v1.z = pf.z;
	DyDy = (FindCameraError(p0, v0, nl, l1, l2, np, cmp3d, cmp2d) - 2*d + FindCameraError(p0, v1, nl, l1, l2, np, cmp3d, cmp2d))*den2;

	v0.x = p0.x+h; v0.y = p0.y+h; v0.z = p0.z;
	v1.x = p0.x+h; v1.y = p0.y-h; v1.z = p0.z;
	v2.x = p0.x-h; v2.y = p0.y+h; v2.z = p0.z;
	v3.x = p0.x-h; v3.y = p0.y-h; v3.z = p0.z;
	dxdy = ((FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v2, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v3, pf, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x+h; v0.y = p0.y; v0.z = p0.z+h;
	v1.x = p0.x+h; v1.y = p0.y; v1.z = p0.z-h;
	v2.x = p0.x-h; v2.y = p0.y; v2.z = p0.z+h;
	v3.x = p0.x-h; v3.y = p0.y; v3.z = p0.z-h;
	dxdz = ((FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v2, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v3, pf, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x+h; v0.y = p0.y; v0.z = p0.z;
	v1.x = p0.x-h; v1.y = p0.y; v1.z = p0.z;
	v2.x = pf.x+h; v2.y = pf.y; v2.z = pf.z;
	v3.x = pf.x-h; v3.y = pf.y; v3.z = pf.z;
	dxDx = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x+h; v0.y = p0.y; v0.z = p0.z;
	v1.x = p0.x-h; v1.y = p0.y; v1.z = p0.z;
	v2.x = pf.x; v2.y = pf.y+h; v2.z = pf.z;
	v3.x = pf.x; v3.y = pf.y-h; v3.z = pf.z;
	dxDy = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x; v0.y = p0.y+h; v0.z = p0.z+h;
	v1.x = p0.x; v1.y = p0.y+h; v1.z = p0.z-h;
	v2.x = p0.x; v2.y = p0.y-h; v2.z = p0.z+h;
	v3.x = p0.x; v3.y = p0.y-h; v3.z = p0.z-h;
	dydz = ((FindCameraError(v0, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, pf, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v2, pf, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v3, pf, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x; v0.y = p0.y+h; v0.z = p0.z;
	v1.x = p0.x; v1.y = p0.y-h; v1.z = p0.z;
	v2.x = pf.x+h; v2.y = pf.y; v2.z = pf.z;
	v3.x = pf.x-h; v3.y = pf.y; v3.z = pf.z;
	dyDx = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x; v0.y = p0.y+h; v0.z = p0.z;
	v1.x = p0.x; v1.y = p0.y-h; v1.z = p0.z;
	v2.x = pf.x; v2.y = pf.y+h; v2.z = pf.z;
	v3.x = pf.x; v3.y = pf.y-h; v3.z = pf.z;
	dyDy = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x; v0.y = p0.y; v0.z = p0.z+h;
	v1.x = p0.x; v1.y = p0.y; v1.z = p0.z-h;
	v2.x = pf.x+h; v2.y = pf.y; v2.z = pf.z;
	v3.x = pf.x-h; v3.y = pf.y; v3.z = pf.z;
	dzDx = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = p0.x; v0.y = p0.y; v0.z = p0.z+h;
	v1.x = p0.x; v1.y = p0.y; v1.z = p0.z-h;
	v2.x = pf.x; v2.y = pf.y+h; v2.z = pf.z;
	v3.x = pf.x; v3.y = pf.y-h; v3.z = pf.z;
	dzDy = ((FindCameraError(v0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v0, v3, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(v1, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(v1, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	v0.x = pf.x+h; v0.y = pf.y+h; v0.z = pf.z;
	v1.x = pf.x+h; v1.y = pf.y-h; v1.z = pf.z;
	v2.x = pf.x-h; v2.y = pf.y+h; v2.z = pf.z;
	v3.x = pf.x-h; v3.y = pf.y-h; v3.z = pf.z;
	DxDy = ((FindCameraError(p0, v0, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(p0, v1, nl, l1, l2, np, cmp3d, cmp2d)) -
		(FindCameraError(p0, v2, nl, l1, l2, np, cmp3d, cmp2d) - FindCameraError(p0, v3, nl, l1, l2, np, cmp3d, cmp2d)))*den2;

	alpha[0][0] = dxdx; alpha[0][1] = dxdy; alpha[0][2] = dxdz; alpha[0][3] = dxDx; alpha[0][4] = dxDy;
	alpha[1][0] = dxdy; alpha[1][1] = dydy; alpha[1][2] = dydz; alpha[1][3] = dyDx; alpha[1][4] = dyDy;
	alpha[2][0] = dxdz; alpha[2][1] = dydz; alpha[2][2] = dzdz; alpha[2][3] = dzDx; alpha[2][4] = dzDy;
	alpha[3][0] = dxDx; alpha[3][1] = dyDx; alpha[3][2] = dzDx; alpha[3][3] = DxDx; alpha[3][4] = DxDy;
	alpha[4][0] = dxDy; alpha[4][1] = dyDy; alpha[4][2] = dzDy; alpha[4][3] = DxDy; alpha[4][4] = DyDy;

	beta[0] = dx;
	beta[1] = dy;
	beta[2] = dz;
	beta[3] = Dx;
	beta[4] = Dy;
}

static double mrqmin_CC(tcgVisionPoint3D *p0, tcgVisionPoint3D *pf, double *lambda,
						int nl, tcgVisionLine2D *l1, tcgVisionLine2D *l2, int np, tcgVisionPoint3D *cmp3d, tcgVisionPoint2D *cmp2d)
{
	int j, k;
	static double oresult;
	static double alpha[5][5], beta[5];
	static double result;
	static double laux;
	tcgVisionPoint3D p0try, pftry;

	if (*lambda == -2.0) {
		*lambda = 0.0001;
		oresult = result = FindCameraError(*p0, *pf, nl, l1, l2, np, cmp3d, cmp2d);
		mrqcof_CC(*p0, *pf, alpha, beta, nl, l1, l2, np, cmp3d, cmp2d, result);
	} else if (*lambda == -1.0) {
		*lambda = laux;
		oresult = result = FindCameraError(*p0, *pf, nl, l1, l2, np, cmp3d, cmp2d);
		mrqcof_CC(*p0, *pf, alpha, beta, nl, l1, l2, np, cmp3d, cmp2d, result);
	} else if (*lambda == 0.0) return result;

	for (j=0; j<5; j++) {
		for (k=0; k<5; k++)
			M[5*j+k] = alpha[j][k];
		M[5*j+j] = alpha[j][j]*(1.0+(*lambda));
		X[j] = beta[j];
	}
	Gauss(M, 5, X);

	p0try.x = p0->x - X[0];
	p0try.y = p0->y - X[1];
	p0try.z = p0->z - X[2];
	pftry.x = pf->x - X[3];
	pftry.y = pf->y - X[4];
	pftry.z = pf->z;

	result = FindCameraError(p0try, pftry, nl, l1, l2, np, cmp3d, cmp2d);
	if (result < oresult) {
		*lambda *= 0.1;
		oresult = result;
		*p0 = p0try;
		*pf = pftry;
		mrqcof_CC(p0try, pftry, alpha, beta, nl, l1, l2, np, cmp3d, cmp2d, result);
	} else {
		*lambda *= 10;
		result = oresult;
	}
	laux = *lambda;
	return result;
}

int tcgVisionCameraCalibration (int nl, tcgVisionLine2D *lm, tcgVisionLine2D *lt, int np, tcgVisionPoint3D *p3d, tcgVisionPoint2D *p2d,
								double Q1[3][4], double Q2[3][4],
								tcgVisionPoint3D *pobs, tcgVisionPoint3D *pref, tcgVisionPoint3D *vup)
{ 
	int i, j;
	int Q1calc;
	double lambda;
	double l, erro0, erro1;
	tcgVisionPoint2D mt;

	Q1calc = 0;
	if (nl>1 && np>1 && nl+np>4) {
		int np_H, np_Q;
		double H[9];
		tcgVisionPoint2D *p3d_H = (tcgVisionPoint2D *)malloc(np*sizeof(tcgVisionPoint2D));
		tcgVisionPoint2D *p2d_H = (tcgVisionPoint2D *)malloc(np*sizeof(tcgVisionPoint2D));
		tcgVisionPoint3D *p3d_Q = (tcgVisionPoint3D *)malloc(np*sizeof(tcgVisionPoint3D));
		tcgVisionPoint2D *p2d_Q = (tcgVisionPoint2D *)malloc(np*sizeof(tcgVisionPoint2D));

		np_H = 0;
		np_Q = 0;
		for (i=0; i<np; i++)
			if (p3d[i].z < 1e-5) {
				p3d_H[np_H].x = p3d[i].x; p3d_H[np_H].y = p3d[i].y;
				p2d_H[np_H] = p2d[i];
				np_H++;
			} else {
				p3d_Q[np_Q] = p3d[i];
				p2d_Q[np_Q] = p2d[i];
				np_Q++;
			}
			if (tcgVisionCameraFindTransf2D(H, nl, lm, lt, np_H, p3d_H, p2d_H))
				if (min_quad_comH(H, np_Q, p3d_Q, p2d_Q, Q1)) 
					Q1calc = 1;

			free(p3d_H);
			free(p2d_H);
			free(p3d_Q);
			free(p2d_Q);
	}
	if (!Q1calc) {
		double HH[12];
		if (!tcgVisionCameraFindTransf3Dto2DPoints(HH, np, p3d, p2d)) return 0;
		Q1[0][0] = HH[0]; Q1[0][1] = HH[1]; Q1[0][2] = HH[ 2]; Q1[0][3] = HH[ 3];
		Q1[1][0] = HH[4]; Q1[1][1] = HH[5]; Q1[1][2] = HH[ 6]; Q1[1][3] = HH[ 7];
		Q1[2][0] = HH[8]; Q1[2][1] = HH[9]; Q1[2][2] = HH[10]; Q1[2][3] = HH[11];
	}

	for (i=0; i<3; i++) 
		for (j=0; j<3; j++) 
			M[3*i+j] = Q1[i][j];
	for (i=0; i<3; i++) 
		X[i] = -Q1[i][3];

	if (!Gauss (M, 3, X)) return 0;
	pobs->x = X[0]; pobs->y = X[1]; pobs->z = X[2];

	mt.x = 0.0; mt.y = 0.0;
	invert(Q1, mt, X);
	pref->x = X[0]; pref->y = X[1]; pref->z = 0.0;

	lambda = -2;
	erro0 = mrqmin_CC(pobs, pref, &lambda, nl, lm, lt, np, p3d, p2d);
	do {
		l = lambda;
		erro1 = erro0;
		erro0 = mrqmin_CC(pobs, pref, &lambda, nl, lm, lt, np, p3d, p2d);
	} while (!((ABS(erro1-erro0)<1e-15 && lambda<l) || (lambda<1e-20) || (lambda>1e20)));

	FindCamera2(*pobs, *pref, np, p3d, p2d, Q2, vup);

	return 1;
}

#define inner(p1,p2) ((p1).x*(p2).x + (p1).y*(p2).y + (p1).z*(p2).z)

double tcgVisionCameraFindVisionAngle(int img_height, double Q[3][4], tcgVisionPoint3D pobs, tcgVisionPoint3D vref)
{
	tcgVisionPoint2D base, topo;
	tcgVisionPoint3D b1, b2;

	base.x = 0.0; base.y = -img_height*0.5;
	invert(Q, base, X);
	b1.x = X[0] - pobs.x; b1.y = X[1] - pobs.y; b1.z = -pobs.z;
	if (inner(b1,vref) < 0) {
		b1.x = -b1.x; b1.y = -b1.y; b1.z = -b1.z;
	}

	topo.x = 0.0; topo.y = img_height*0.5;
	invert(Q, topo, X);
	b2.x = X[0] - pobs.x; b2.y = X[1] - pobs.y; b2.z = -pobs.z;
	if (inner(b2,vref) < 0) {
		b2.x = -b2.x; b2.y = -b2.y; b2.z = -b2.z;
	}

	normalize3d(&b1);
	normalize3d(&b2);
	return acos(inner(b1, b2))*180.0/_PI_;
}

void tcgVisionCameraProject (double Q[3][4], tcgVisionPoint3D pm, tcgVisionPoint2D *pc)
{
	double x, y, z;

	x = Q[0][0]*pm.x + Q[0][1]*pm.y + Q[0][2]*pm.z + Q[0][3];
	y = Q[1][0]*pm.x + Q[1][1]*pm.y + Q[1][2]*pm.z + Q[1][3];
	z = Q[2][0]*pm.x + Q[2][1]*pm.y + Q[2][2]*pm.z + Q[2][3];

	pc->x = x/z; pc->y = y/z;
}

void tcgVisionCameraUnproject (double Q[3][4], tcgVisionPoint2D pt, tcgVisionPoint3D *pc)
{
	invert(Q, pt, X);
	pc->x = X[0]; pc->y = X[1]; pc->z = 0.0;
}

#define MAX_POINTS 500

static int MinQuad(int nl, int nc, double A[MAX_POINTS][5], double B[MAX_POINTS])
{
	int i, j, k;

	for (i=0; i<nc; i++)
		for (j=0; j<nc; j++) {
			M[nc*i+j] = A[0][j]*A[0][i];
			for (k=1; k<nl; k++)
				M[nc*i+j] += A[k][j]*A[k][i];
		}

		for (i=0; i<nc; i++) {
			X[i] = A[0][i]*B[0];
			for (k=1; k<nl; k++)
				X[i] += A[k][i]*B[k];
		}

		if (!Gauss(M, nc, X) != 0) return 0;

		for (i=0; i<nc; i++) 
			B[i] = X[i];
		return 1;
}

void tcgVisionCameraUnprojectAir (double Q[3][4], tcgVisionPoint2D pt1, tcgVisionPoint2D pt2, tcgVisionPoint3D *pc)
{
	int i, j;
	double A[MAX_POINTS][5];
	double B[MAX_POINTS];
	tcgVisionPoint3D pobs, pt1uprj, pt2uprj, uprj;
	tcgVisionPoint3D dir_pt1uprj;
	double pi_dirs;

	for (i=0; i<3; i++) 
		for (j=0; j<3; j++) 
			M[3*i+j] = Q[i][j];
	for (i=0; i<3; i++) 
		X[i] = -Q[i][3];
	Gauss (M, 3, X);
	pobs.x = X[0]; pobs.y = X[1]; pobs.z = X[2];

	tcgVisionCameraUnproject(Q, pt1, &pt1uprj);
	tcgVisionCameraUnproject(Q, pt2, &pt2uprj);

	A[0][0] = Q[0][2]; A[0][1] = -pt1.x;
	A[1][0] = Q[1][2]; A[1][1] = -pt1.y;
	A[2][0] = Q[2][2]; A[2][1] = -1;
	B[0] = -(Q[0][0]*pt2uprj.x + Q[0][1]*pt2uprj.y + Q[0][3]);
	B[1] = -(Q[1][0]*pt2uprj.x + Q[1][1]*pt2uprj.y + Q[1][3]);
	B[2] = -(Q[2][0]*pt2uprj.x + Q[2][1]*pt2uprj.y + Q[2][3]);
	MinQuad(3, 2, A, B);
	uprj.x = pt2uprj.x;
	uprj.y = pt2uprj.y;
	uprj.z = B[0];

	dir_pt1uprj.x = pt1uprj.x - pobs.x;
	dir_pt1uprj.y = pt1uprj.y - pobs.y;
	dir_pt1uprj.z = pt1uprj.z - pobs.z;
	normalize3d(&dir_pt1uprj);

	pi_dirs = dir_pt1uprj.x*(uprj.x - pobs.x) + dir_pt1uprj.y*(uprj.y - pobs.y) + dir_pt1uprj.z*(uprj.z - pobs.z);

	pc->x = pobs.x + pi_dirs*dir_pt1uprj.x;
	pc->y = pobs.y + pi_dirs*dir_pt1uprj.y;
	pc->z = pobs.z + pi_dirs*dir_pt1uprj.z;
}

void tcgVisionCameraUnprojectAirZ (double Q[3][4], tcgVisionPoint2D pt, double z, tcgVisionPoint3D *pc)
{
	int i, j;
	tcgVisionPoint3D pobs, pt_uprj;

	for (i=0; i<3; i++) 
		for (j=0; j<3; j++) 
			M[3*i+j] = Q[i][j];
	for (i=0; i<3; i++) 
		X[i] = -Q[i][3];
	Gauss (M, 3, X);
	pobs.x = X[0]; pobs.y = X[1]; pobs.z = X[2];

	tcgVisionCameraUnproject(Q, pt, &pt_uprj);

	pc->x = z*(pobs.x - pt_uprj.x)/pobs.z + pt_uprj.x;
	pc->y = z*(pobs.y - pt_uprj.y)/pobs.z + pt_uprj.y;
	pc->z = z;
}
