/*
 * calibracao.c
 *
 * Calibracao de camera e demonstrada neste programa.
 * Atraves do click sobre as marcas no padrao de calibracao,
 *  estabelece-se 10 (dez) correspondencia na janela esquerda.
 *  A marca clicada e "highlighted" com a cor vermelha.
 * Com base destas 10 correspondencias e estimada a matriz
 *  de projecao e os parametros intrisecos e extrinsecos da
 *  camera. Para validar a matriz de projecao obtida, sao
 *  reprojetados os vertices da cena sintetica (um cubo azul e
 *  um cubo rosa) e as marcas na janela direita.
 * Atraves das teclas pode-se alterar os parametros da camera:
 *  'x': decrementa 5.0 o angulo de rotacao em torno de 'x' 
 *  'X': incrementa 5.0 o angulo de rotacao em torno de 'x' 
 *  'y': decrementa 5.0 o angulo de rotacao em torno de 'y' 
 *  'Y': incrementa 5.0 o angulo de rotacao em torno de 'y' 
 *  'z': decrementa 5.0 o angulo de rotacao em torno de 'z' 
 *  'Z': incrementa 5.0 o angulo de rotacao em torno de 'z' 
 *  'r': reseta os angulos de rotacao para (35,35,0)
 *  'R': reseta as marcas selecionadas manualmente
 *
 *             Ting (07/11/08)
 */
#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

int win;  // id da janela principal
int swin1, swin2; // id das subjanelas do glut

int width = 300, height = 300;

#define d 1.

static GLfloat ndata[8][3] = {
   {-d, -d, -d}, {d, -d, -d}, {d,d,-d}, {-d, d, -d}, 
   {-d, -d, d}, {d, -d, d}, {d,d,d}, {-d, d, d}
   };

static GLfloat mdata[18][3] = {
   {-d/2,-d/2,d}, {d/2,-d/2,d}, {d/2,d/2,d}, {-d/2,d/2,d},
   {d/2,0,d}, {-d/2,0,d}, {0,-d/2,d}, {0,d/2,d},{0,0,d},
   {-d/2, d, -d/2}, {d/2, d, -d/2},{-d/2, d, d/2}, {d/2, d, d/2},
   {-d/2, d, 0}, {d/2, d, 0},{0, d, -d/2}, {0, d, d/2}, {0,d,0}
   };

static GLuint vindices[6][4] = {
   {2,3,7,6}, {4,5,6,7}, {0,3,2,1}, 
   {0,4,7,3}, {1,2,6,5}, {0,1,5,4}
   };

typedef struct POINT_struct {   /* pontos */
  GLdouble x, y;         // coordenadas em imagem
  GLuint label;    // referencia a 3D
} Point2D;

int nsamples=0;
Point2D samples[18];

float **A, W[13], **V;
float M[3][4];
double anglex, angley, anglez;

double T[3], R[3][3], F[2], aspRatio;
int O[2];

/**********************************************************
 * SVD
 * 
 * Esta rotina faz parte da coletânea dos programas
 * do livro Numerical Recipes: The Art of Scientific Computing
 * 
 * Observe que o indice de um arranjo comeca com 1!!!!
 *
 * http://www.nr.com/
 *
*/
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
static float maxarg1,maxarg2;
#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
(maxarg1) : (maxarg2))
static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(iminarg1) : (iminarg2))
static float sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)

float pythag(float a, float b)
{
        float absa,absb;
        absa=fabs(a);
        absb=fabs(b);
        if (absa > absb) return absa*sqrt(1.0f+SQR(absb/absa));
        else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0f+SQR(absa/absb)));
}
 
void svdcmp(float **a, int m, int n, float w[], float **v)
{
	float pythag(float a, float b);
	int flag,i,its,j,jj,k,l,nm;
	float anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1 = (float *) malloc((n+1) * sizeof(float));

	g=scale=anorm=0.0;
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++)
					v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=IMIN(m,n);i>=1;i--) {
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			for (j=l;j<=n;j++) {
				for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) {
				nm=l-1;
				if ((float)(fabs(rv1[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((float)(fabs(w[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.0;
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if ((float)(fabs(f)+anorm) == anorm) break;
					g=w[i];
					h=pythag(f,g);
					w[i]=h;
					h=1.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30)
                        {
                                printf("svdcmp nao convergiu em 30 iteracoes!");
                                exit(1);
                        }
 			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=pythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=pythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g = g*c-x*s;
				h=y*s;
				y *= c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=pythag(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free((char *)rv1);
}


void monta_2_linhas (int k)
{
  int i,j;

  j = (k-1)/2;
  i = k;

  A[i][1] = mdata[samples[j].label][0];
  A[i][2] = mdata[samples[j].label][1];
  A[i][3] = mdata[samples[j].label][2];
  A[i][4] = 1;
  A[i][5] = 0;
  A[i][6] = 0;
  A[i][7] = 0;
  A[i][8] = 0;
  A[i][9] = -samples[j].x*mdata[samples[j].label][0];
  A[i][10] = -samples[j].x*mdata[samples[j].label][1];
  A[i][11] = -samples[j].x*mdata[samples[j].label][2];
  A[i][12] = -samples[j].x;
  A[i+1][1] = 0;
  A[i+1][2] = 0;
  A[i+1][3] = 0;
  A[i+1][4] = 0;
  A[i+1][5] = mdata[samples[j].label][0];
  A[i+1][6] = mdata[samples[j].label][1];
  A[i+1][7] = mdata[samples[j].label][2];
  A[i+1][8] = 1;
  A[i+1][9] = -samples[j].y*mdata[samples[j].label][0];
  A[i+1][10] = -samples[j].y*mdata[samples[j].label][1];
  A[i+1][11] = -samples[j].y*mdata[samples[j].label][2];
  A[i+1][12] = -samples[j].y;
}

/* Determinar m, tal que Am=0
 *
 */
void proj_mat_calib(void)
{
  int i, j;
  double k;

  //Alocacao de memoria. Uma posicao a mais é alocada porque
  //a base dos indices inicia com 1 em Numerical Recipes
  A = (float **)malloc (sizeof (float *) * (2*nsamples+1));
  for (i=1; i< (2*nsamples+1); i++) 
    A[i] = (float *)malloc (sizeof (float) * 13);
  V = (float **)malloc (sizeof (float *) * 13);
  for (i=1; i< 13; i++) 
    V[i] = (float *)malloc (sizeof (float) * 13);

  // Montar matriz A
  for (i=1; i<(2*nsamples+1); i=i+2) {
    monta_2_linhas(i);
  }

  svdcmp(A,16,12,W,V);

  // Selcionar o menor auto-valor (que deve ser teoricamente nulo)
  // correspondente ao espaço nulo de A, ou seja, Am=0
  for (k=1.E6,i=1; i<13; i++) {
    if (W[i]<k) {
      j=i;
      k = W[i];
    }
  }

  printf("Valor singular = %lf\n", k);

  // Montar matriz m
  M[0][0] = V[1][j]; M[0][1] = V[2][j]; M[0][2] = V[3][j]; M[0][3] = V[4][j];
  M[1][0] = V[5][j]; M[1][1] = V[6][j]; M[1][2] = V[7][j]; M[1][3] = V[8][j];
  M[2][0] = V[9][j]; M[2][1] = V[10][j]; M[2][2] = V[11][j]; M[2][3] = V[12][j];

  for (i=1; i< (2*nsamples+1); i++) 
    free((char *) A[i]);
  free((char *)A);
  for (i=1; i< 13; i++) 
    free((char *) V[i]);
	 free((char *)V);
}

/*
 * Determinar parametros da camera
 *  matriz de rotacao: r_ij
 *  matriz de translacao: T
 *        |-f_x r_11 + o_x r_31   -f_xr_12+o_xr_32   -f_xr_13+o_xr_33   -f_xT_x+o_xT_z|
 *
 *    m'= |-f_y r_21 + o_y r_31   -f_yr_22+o_yr_32   -f_yr_23+o_yr_33   -f_yT_y+o_yT_z|
 *
 *        |        r_31                 r_32               r_33              Tz       |
 *
 */

void camera_par(void)
{
  double g;
  int s, i, j;

  // considerando que os vetores que compoem a matriz de rotacao
  // serem unitarios
  g = sqrt(M[2][0]*M[2][0] + M[2][1]*M[2][1] + M[2][2]*M[2][2]);

  // Aplicar sobre m o fator de escala 
  for (i=0; i<3; i++)
    for (j=0; j<4; j++)
      M[i][j] /= g;

  // Obter Tz, considerando que a camera esteja na "frente" da origem do WC
  s = -1;
  T[2] = s*M[2][3];

  // Ultima linha da matriz de rotacao
  R[2][0] = s*M[2][0];
  R[2][1] = s*M[2][1];
  R[2][2] = s*M[2][2];

  // Determinar o centro da imagem (parametro intrinsico)
  O[0] = (int)(M[0][0]*M[2][0] + M[0][1]*M[2][1] + M[0][2]*M[2][2]);
  O[1] = (int)(M[1][0]*M[2][0] + M[1][1]*M[2][1] + M[1][2]*M[2][2]);

  // Determinar a distancia focal (parametro intrinsico)
  F[0] = sqrt((M[0][0]*M[0][0] + M[0][1]*M[0][1] + M[0][2]*M[0][2]) - O[0]*O[0]);
  F[1] = sqrt((M[1][0]*M[1][0] + M[1][1]*M[1][1] + M[1][2]*M[1][2]) - O[1]*O[1]);

  aspRatio = F[1]/F[0];

  // Outras linhas da matriz de rotacao
  R[0][0] = s *(O[0] * M[2][0] - M[0][0])/F[0];
  R[0][1] = s *(O[0] * M[2][1] - M[0][1])/F[0]; 
  R[0][2] = s *(O[0] * M[2][2] - M[0][2])/F[0]; 

  R[1][0] = s *(O[1] * M[2][0] - M[1][0])/F[1];
  R[1][1] = s *(O[1] * M[2][1] - M[1][1])/F[1];
  R[1][2] = s *(O[1] * M[2][2] - M[1][2])/F[1];

  // Outras linhas da matriz de translacao
  T[0] = s *(O[0]*T[2] - M[0][3])/F[0];  
  T[1] = s *(O[1]*T[2] - M[1][3])/F[1];  
}

/****************************************************/
/* Desenhar marcas de referencia para calibracao
 *
 */
void desenhaMarcas(GLenum mode)
{
  int i,j;

   /* desenhar as marcas sobre o padrao de calibracao */
   for (i = 0; i < 18; i++) {
     if (mode == GL_SELECT)
         glLoadName (i);
     else if (mode == GL_RENDER) {
       // Color selected marks with red
       glColor3f (0.0, 0.0, 0.0);
       for (j = 0; j < nsamples; j++) {
	 if (samples[j].label == i) glColor3f (1.0, 0.0, 0.0);
       }
     }
     glBegin(GL_POINTS);
     glVertex3fv(&mdata[i][0]);
     glEnd();
   }
}

/* Desenhar o padrao de calibracao o qual contem 
   as marcas cujas geometrias espaciais sao conhecidas
*/
void desenhaPCal ()
{
  int i;

   /* desenhar o cubo */
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
   glColor3f (1.0, 1.0, 1.0);
   for (i=0; i<2; i++) {
     glBegin(GL_POLYGON);
     glVertex3fv(&ndata[vindices[i][0]][0]);
     glVertex3fv(&ndata[vindices[i][1]][0]);
     glVertex3fv(&ndata[vindices[i][2]][0]);
     glVertex3fv(&ndata[vindices[i][3]][0]);
     glEnd();
   }
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
   glColor3f (0, 0, 0);
   for (i=0; i<2; i++) {
     glBegin(GL_POLYGON);
     glVertex3fv(&ndata[vindices[i][0]][0]);
     glVertex3fv(&ndata[vindices[i][1]][0]);
     glVertex3fv(&ndata[vindices[i][2]][0]);
     glVertex3fv(&ndata[vindices[i][3]][0]);
     glEnd();
   }

   glPointSize(8.0);
   desenhaMarcas(GL_RENDER);
}

/* Desenhar o cubo
*/
void desenhaCubo (double m[][4], double c[])
{
  int i;
  double x,y,z;

   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
   glColor3f (1.0-c[0], 1.0-c[1], 1.0-c[2]);
   for (i=0; i<6; i++) {
     glBegin(GL_POLYGON);
     x = (m[0][0]*ndata[vindices[i][0]][0] + m[0][1]*ndata[vindices[i][0]][1] +
	  m[0][2]*ndata[vindices[i][0]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][0]][0] + m[1][1]*ndata[vindices[i][0]][1] +
	  m[1][2]*ndata[vindices[i][0]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][0]][0] + m[2][1]*ndata[vindices[i][0]][1] +
	  m[2][2]*ndata[vindices[i][0]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][1]][0] + m[0][1]*ndata[vindices[i][1]][1] +
	  m[0][2]*ndata[vindices[i][1]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][1]][0] + m[1][1]*ndata[vindices[i][1]][1] +
	  m[1][2]*ndata[vindices[i][1]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][1]][0] + m[2][1]*ndata[vindices[i][1]][1] +
	  m[2][2]*ndata[vindices[i][1]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][2]][0] + m[0][1]*ndata[vindices[i][2]][1] +
	  m[0][2]*ndata[vindices[i][2]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][2]][0] + m[1][1]*ndata[vindices[i][2]][1] +
	  m[1][2]*ndata[vindices[i][2]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][2]][0] + m[2][1]*ndata[vindices[i][2]][1] +
	  m[2][2]*ndata[vindices[i][2]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][3]][0] + m[0][1]*ndata[vindices[i][3]][1] +
	  m[0][2]*ndata[vindices[i][3]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][3]][0] + m[1][1]*ndata[vindices[i][3]][1] +
	  m[1][2]*ndata[vindices[i][3]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][3]][0] + m[2][1]*ndata[vindices[i][3]][1] +
	  m[2][2]*ndata[vindices[i][3]][2]) + m[2][3];
     glVertex3f(x,y,z);

     glEnd();
   }
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
   if (c[0] == 0.0) glColor3f (1.0, 0, 0);
   else if (c[1] == 0.0) glColor3f(0, 1.0, 0.0);
   else glColor3f(0.0,0.0,1.0);

   for (i=0; i<6; i++) {
     glBegin(GL_POLYGON);
     x = (m[0][0]*ndata[vindices[i][0]][0] + m[0][1]*ndata[vindices[i][0]][1] +
	  m[0][2]*ndata[vindices[i][0]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][0]][0] + m[1][1]*ndata[vindices[i][0]][1] +
	  m[1][2]*ndata[vindices[i][0]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][0]][0] + m[2][1]*ndata[vindices[i][0]][1] +
	  m[2][2]*ndata[vindices[i][0]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][1]][0] + m[0][1]*ndata[vindices[i][1]][1] +
	  m[0][2]*ndata[vindices[i][1]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][1]][0] + m[1][1]*ndata[vindices[i][1]][1] +
	  m[1][2]*ndata[vindices[i][1]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][1]][0] + m[2][1]*ndata[vindices[i][1]][1] +
	  m[2][2]*ndata[vindices[i][1]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][2]][0] + m[0][1]*ndata[vindices[i][2]][1] +
	  m[0][2]*ndata[vindices[i][2]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][2]][0] + m[1][1]*ndata[vindices[i][2]][1] +
	  m[1][2]*ndata[vindices[i][2]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][2]][0] + m[2][1]*ndata[vindices[i][2]][1] +
	  m[2][2]*ndata[vindices[i][2]][2]) + m[2][3];
     glVertex3f(x,y,z);
     x = (m[0][0]*ndata[vindices[i][3]][0] + m[0][1]*ndata[vindices[i][3]][1] +
	  m[0][2]*ndata[vindices[i][3]][2]) + m[0][3];
     y = (m[1][0]*ndata[vindices[i][3]][0] + m[1][1]*ndata[vindices[i][3]][1] +
	  m[1][2]*ndata[vindices[i][3]][2]) + m[1][3];
     z = (m[2][0]*ndata[vindices[i][3]][0] + m[2][1]*ndata[vindices[i][3]][1] +
	  m[2][2]*ndata[vindices[i][3]][2]) + m[2][3];
     glVertex3f(x,y,z);
     glEnd();
   }
}

/**************************************
 * Gerar cena sintetica com o padrao de calibracao
 *
 */
void desenhaCena (void)
{
  double t, m[3][4], c[3];

  // Desenhar os pixels da imagem original
  glutSetWindow(swin1);

  /* "CLEAR" o display */
   glClear (GL_COLOR_BUFFER_BIT);

  /* setar a cor de desenho em branco (1,1,1) */
   glColor3f (1.0, 1.0, 1.0);

   /* chavear para a pilha de transformacao de projecao */ 
   glMatrixMode (GL_PROJECTION);
   glLoadIdentity ();
   glFrustum (-1.0, 1.0, -1.0, 1.0, 1.5, 20.0);
   /* chavear para a pilha de transformacao de modelo */
   glMatrixMode (GL_MODELVIEW);

  /* carregar a matriz de identidade na pilha de matriz de transformacao */
   glLoadIdentity();
  /* empilhar a matriz de transformacao de camera  */
   gluLookAt (0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

   glPushMatrix();
  /* concatenar a matriz de translacao com a do topo da pilha */
   glTranslatef(0.5,0.5,0.5);
   
  /* concatenar a matriz de rotacao com a do topo da pilha */
   glRotatef(anglex,1.0,0.0,0.0);
   glRotatef(angley,0.0,1.0,0.0);
   glRotatef(anglez,0.0,0.0,1.0);

   desenhaPCal ();

  /* gerar uma cena sintética com uma esfera e um cilindro*/
  /* desenhar os cubos */
   //cubo rosa
   t = sqrt(2)/2;
   m[0][1] = m[1][0] = m[2][1] = m[1][2]= 0.;
   m[0][0] = m[2][0] = m[2][2] = t*0.8; 
   m[0][2] = -t*0.8; 
   m[1][1] = 0.8;
   m[0][3] = -1.0;
   m[1][3] = -2.5;
   m[2][3] = -0.5;
   c[0] = 0.0; c[1]=0.2; c[2]=0.2;
   desenhaCubo(m,c);

   // cubo azul
   t = sqrt(2)/2;
   m[0][1] = m[0][2] = m[1][0] = m[2][0] = 0.;
   m[1][1] = m[2][1] = m[2][2] = t*0.7; 
   m[1][2] = -t*0.7; 
   m[0][0] = 0.7;
   m[0][3] = -1.0;
   m[1][3] = -0.7;
   m[2][3] = -2.0;
   c[0] = 0.2; c[1]=0.2; c[2]=0.0;
   desenhaCubo(m,c);
   glPopMatrix();
}

/*********************************************************/
void init(void)
{
  glutSetWindow(swin1);
   glClearColor(0.0, 0.0, 0.0, 0.0);
   glEnable(GL_DEPTH_TEST);
   glShadeModel(GL_FLAT);
   glDepthRange(0.0, 1.0);  /* The default z mapping */
   glutSetWindow(swin2);
   anglex = angley = 35.;
   anglez = 0.;
}

void display_swin1(void)
{
  glutSetWindow(swin1);
  glClearColor (0.0, 0.0, 0.0, 0.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  desenhaCena ();
  glFlush();
}

void display_swin2(void)
{
  int i;
  double x, y, z, X, Y, Z, t;
  double m[3][4];

  // Desenhar a imagem projetada a partir da matriz de calibracao estimada
  glutSetWindow(swin2);
  // resetar as cores nos buffers de cor
  glClearColor ((GLfloat)0.3, (GLfloat)0.3, (GLfloat)0.3, (GLfloat)0.0);
  glClear(GL_COLOR_BUFFER_BIT);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, (GLfloat) width, 0.0, (GLfloat) height);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glPointSize(5.0);

  //desenhar as marcas do padrao de calibracao
  glColor3f(1.0,1.0,0.);
  glBegin(GL_POINTS);
  for (i=0; i<18; i++) {
    x = M[0][0]*mdata[i][0] + M[0][1]*mdata[i][1] + M[0][2]*mdata[i][2] + M[0][3];
    y = M[1][0]*mdata[i][0] + M[1][1]*mdata[i][1] + M[1][2]*mdata[i][2] + M[1][3];
    z = M[2][0]*mdata[i][0] + M[2][1]*mdata[i][1] + M[2][2]*mdata[i][2] + M[2][3];
    
    glVertex2i((int) (x/z), (int) (y/z));
  }
  glEnd();

  /* desenhar os vertices do cube rosa */
   t = sqrt(2)/2;
   m[0][1] = m[1][0] = m[2][1] = m[1][2]= 0.;
   m[0][0] = m[2][0] = m[2][2] = t*0.8; 
   m[0][2] = -t*0.8; 
   m[1][1] = 0.8;
   m[0][3] = -1.0;
   m[1][3] = -2.5;
   m[2][3] = -0.5;
   glColor3f(1.0,0.0,0.0);
  glBegin(GL_POINTS);
  for (i=0; i<8; i++) {
    X = m[0][0]*ndata[i][0] + m[0][1]*ndata[i][1] + m[0][2]*ndata[i][2] + m[0][3];
    Y = m[1][0]*ndata[i][0] + m[1][1]*ndata[i][1] + m[1][2]*ndata[i][2] + m[1][3];
    Z = m[2][0]*ndata[i][0] + m[2][1]*ndata[i][1] + m[2][2]*ndata[i][2] + m[2][3];
    
    x = M[0][0]*X + M[0][1]*Y + M[0][2]*Z + M[0][3];
    y = M[1][0]*X + M[1][1]*Y + M[1][2]*Z + M[1][3];
    z = M[2][0]*X + M[2][1]*Y + M[2][2]*Z + M[2][3];
    glVertex2i((int) (x/z), (int) (y/z));
  }
  glEnd();

  /* desenhar os vertices do cube azul */
   glColor3f(0.0,0.0,1.0);
   t = sqrt(2)/2;
   m[0][1] = m[0][2] = m[1][0] = m[2][0] = 0.;
   m[1][1] = m[2][1] = m[2][2] = t*0.7; 
   m[1][2] = -t*0.7; 
   m[0][0] = 0.7;
   m[0][3] = -1.0;
   m[1][3] = -0.7;
   m[2][3] = -2.0;
   glBegin(GL_POINTS);
  for (i=0; i<8; i++) {
    X = m[0][0]*ndata[i][0] + m[0][1]*ndata[i][1] + m[0][2]*ndata[i][2] + m[0][3];
    Y = m[1][0]*ndata[i][0] + m[1][1]*ndata[i][1] + m[1][2]*ndata[i][2] + m[1][3];
    Z = m[2][0]*ndata[i][0] + m[2][1]*ndata[i][1] + m[2][2]*ndata[i][2] + m[2][3];
    
    x = M[0][0]*X + M[0][1]*Y + M[0][2]*Z + M[0][3];
    y = M[1][0]*X + M[1][1]*Y + M[1][2]*Z + M[1][3];
    z = M[2][0]*X + M[2][1]*Y + M[2][2]*Z + M[2][3];
    glVertex2i((int) (x/z), (int) (y/z));
  }
  glEnd();

  // forcar a execucao dos comandos enviados a OpenGL 
  glFlush();
}

void
main_display(void)
{
    glClearColor((GLfloat)0.8, (GLfloat)0.8, (GLfloat)0.8, (GLfloat)0.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(0.f, 0.f, 0.f);
    display_swin1();
    display_swin2();
}

void main_reshape(int w, int h)
{

  glViewport(0,0,(GLsizei)w,(GLsizei)h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, (GLfloat) w, 0.0, (GLfloat) h);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  /* redefinir a posição e as dimensões da sub-janela 1 */
  glutSetWindow(swin1);
  glutPositionWindow(0, 0);
  glutReshapeWindow(width, height);
  /* redefinir a posição e as dimensões da sub-janela 2 */
  glutSetWindow(swin2);
  glutPositionWindow(width,0 );
  glutReshapeWindow(width, height);
}

/*******************************************************/
/* processHits processa as marcas selecionadas
 */
void processHits (GLdouble x, GLdouble y, GLint hits, GLuint buffer[])
{
   GLint i;
   GLuint names, j, *ptr;

   ptr = (GLuint *) buffer;
   for (i = 0; i < hits; i++) {	/*  for each hit  */
      names = *ptr;
      ptr++;
      ptr++;
      ptr++;
      for (j = 0; j < names; j++) {	/*  for each name */
	 if (nsamples < 10) {
	   samples[nsamples].label = *ptr;
	   samples[nsamples].x = x;
	   samples[nsamples].y = y;
	   nsamples++;
	   if (nsamples == 10) {
	     proj_mat_calib();
	     camera_par();
	     printf ("Parametros Intrinsicos: \n");
	     printf ("   Ponto Principal = (%d,%d) *** Razao de aspecto = %lf *** Foco=(%lf,%lf) \n",
		     O[0],O[1],aspRatio,F[0],F[1]);
	     printf ("Parametros Extrinsicos: \n");
	     printf ("   %lf    %lf    %lf   %lf\n", R[0][0], R[0][1], R[0][2], T[0]);
	     printf ("   %lf    %lf    %lf   %lf\n", R[1][0], R[1][1], R[1][2], T[1]);
	     printf ("   %lf    %lf    %lf   %lf\n", R[2][0], R[2][1], R[2][2], T[2]);
	     nsamples = 0;
	     display_swin2();
	   }
	 }
	ptr++;
       }
   }
}

/* selectObjects desenha as marcas no modo de selecao. Cada
 * marca recebera uma identificacao unica. Ao clicar sobre
 * uma marca, será retornada esta identificacao.
 */
#define BUFSIZE 512

void selectObjects(int button, int state, int x, int y)
{
   GLuint selectBuf[BUFSIZE];
   GLint hits;
   GLint viewport[4];
   GLdouble sx, sy;

   glutSetWindow(swin1);
   if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
     // Selecionar marcas de geometria conhecida

     glGetIntegerv(GL_VIEWPORT, viewport);

     glSelectBuffer (BUFSIZE, selectBuf);
     (void) glRenderMode (GL_SELECT);

     sx = (GLdouble) x;
     sy = (GLdouble) viewport[3] - y;

     glInitNames();
     glPushName(0);

     glPushMatrix ();

     /* chavear para a pilha de transformacao de projecao */ 
     glMatrixMode (GL_PROJECTION);
     glLoadIdentity ();
     gluPickMatrix(sx, sy, 10.0, 10.0, viewport);
     glFrustum (-1.0, 1.0, -1.0, 1.0, 1.5, 20.0);
     /* chavear para a pilha de transformacao de modelo */
     glMatrixMode (GL_MODELVIEW);

     /* carregar a matriz de identidade na pilha de matriz de transformacao */
     glLoadIdentity();
     /* empilhar a matriz de transformacao de camera  */
     gluLookAt (0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

     /* concatenar a matriz de translacao com a do topo da pilha */
     glTranslatef(0.5,0.5,0.5);
   
     /* concatenar a matriz de rotacao com a do topo da pilha */
     glRotatef(anglex,1.0,0.0,0.0);
     glRotatef(angley,0.0,1.0,0.0);
     glRotatef(anglez,0.0,0.0,1.0);

     desenhaMarcas(GL_SELECT);
     glPopMatrix ();
     glFlush ();

     hits = glRenderMode (GL_RENDER);
     processHits (sx, sy, hits, selectBuf);
     display_swin1();
   } 
} 

void keyboard(unsigned char key, int x, int y)
{
   switch (key) {
   case 'x':
     anglex -= 5.0;
     break;
   case 'X':
     anglex += 5.0;
   case 'y':
     angley -= 5.0;
     break;
   case 'Y':
     angley += 5.0;
     break;
   case 'z':
     anglez -= 5.0;
     break;
   case 'Z':
     anglez += 5.0;
     break;
   case 'r':
     anglex = 35.0;
     angley = 35.0;
     anglez = 0.0;
	 nsamples=0;
     break;
   case 'R':
     nsamples = 0;
     break;
      case 27:
         exit(0);
         break;
   }
   display_swin1();
   /* Limpar o conteudo da janela 2 */
   glutSetWindow(swin2);
   glClearColor ((GLfloat)0.3, (GLfloat)0.3, (GLfloat)0.3, (GLfloat)0.0);
   glClear(GL_COLOR_BUFFER_BIT);
}

/*******************************************************/
/*  Main Loop
 */
int main(int argc, char **argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH);
   glutInitWindowSize (2*width, height);
   glutInitWindowPosition (100, 100);
  win = glutCreateWindow(argv[0]);
    glutReshapeFunc(main_reshape);
    glutDisplayFunc(main_display);
    glutKeyboardFunc(keyboard);
    swin1 = glutCreateSubWindow(win,(int)0,0,(int)width,(int)height);
    glutDisplayFunc(display_swin1);
    glutKeyboardFunc(keyboard);
    glutMouseFunc(selectObjects);
    swin2 = glutCreateSubWindow(win,(int)width,0,(int)width,(int)height);
    glutDisplayFunc(display_swin2);
    glutKeyboardFunc(keyboard);

  init();
  glutMainLoop();

   return 0; 
}