/* unproject2.c
/* When the left mouse button is pressed, the x,y mouse position is used */
/* to find two 3D points, one at near, one at far. The line joining these */
/* points is used to find the closest world point. */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <GL/glut.h>

static GLfloat roll = 10.0;     /* rotate about Z axis 'r' */
static GLfloat pitch = 5.0;     /* rotate about X axis 'p' */
static GLfloat heading = 15.0;  /* rotate about Y axis 'h' */
static GLfloat pos[3] = {2.5, 0.7, -10.0}; /* offset position */
static GLdouble m[16];
static GLfloat points[4][3] = {{1.0, 1.0, -3.0},
                               {-1.0, -1.0, -3.0},
                               {10.0, -10.0, -50.0},
                               {-8.0, 8.0, -50.0}};

void display(void)
{
  glClearColor(0.0, 0.0, 0.0, 1.0);
  glClear(GL_COLOR_BUFFER_BIT);
  glPointSize(10.0);
  glPushMatrix();

    glRotatef(pitch, 1.0, 0.0, 0.0);
    glRotatef(heading, 0.0, 1.0, 0.0);
    glRotatef(roll, 0.0, 0.0, 1.0);
    glTranslatef(pos[0], pos[1], pos[2]);

    glGetDoublev(GL_MODELVIEW_MATRIX, m); /* save transformation */
    glBegin(GL_POINTS);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3fv(points[0]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3fv(points[1]);
      glColor3f(1.0, 1.0, 0.0);
      glVertex3fv(points[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3fv(points[3]);
    glEnd();
  glPopMatrix();
  glFlush();
}

/* Change these values for a different transformation  */
void reshape(int w, int h)
{
   glViewport (0, 0, (GLsizei) w, (GLsizei) h);
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   gluPerspective (45.0, (GLfloat) w/(GLfloat) h, 1.0, 100.0);
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
}

static GLdouble dist(GLdouble a1x, GLdouble a1y, GLdouble a1z,
                     GLdouble a2x, GLdouble a2y, GLdouble a2z,
                     GLdouble a3x, GLdouble a3y, GLdouble a3z)
{
  GLdouble d, ax, ay, az, bx, by, bz, cx, cy, cz;
  
  ax = a2x-a1x;      ay = a2y-a1y;      az = a2z-a1z;
  bx = a1x-a3x;      by = a1y-a3y;      bz = a1z-a3z;
  cx = ay*bz-az*by;  cy = ax*bz-az*bx;  cz = ax*by-ay*bx;
  d =  sqrt(cx*cx+cy*cy+cz*cz)/sqrt(ax*ax+ay*ay+az*az);
  return d;
}

void mouse(int button, int state, int x, int y) 
{
   GLint viewport[4];
   GLdouble mvmatrix[16], projmatrix[16];
   GLint realy;  /*  OpenGL y coordinate position  */
   GLdouble wx0, wy0, wz0;  /*  returned world x, y, z coords at near */
   GLdouble wx1, wy1, wz1;  /*  returned world x, y, z coords at far */
   GLdouble d;
   int i;

   if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
   {
     glGetIntegerv (GL_VIEWPORT, viewport);
     printf("m my transformation\n");
     printf("m[ 0.. 3] %f, %f, %f, %f \n",
             m[0], m[1], m[2], m[3]);
     printf("m[ 4.. 7] %f, %f, %f, %f \n",
             m[4], m[5], m[6], m[7]);
     printf("m[ 8..11] %f, %f, %f, %f \n",
             m[8], m[9], m[10], m[11]);
     printf("m[12..15] %f, %f, %f, %f \n",
             m[12], m[13], m[14], m[15]);
                   
     glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix);
     printf("modelmatrix\n");
     printf("m[ 0.. 3] %f, %f, %f, %f \n",
             mvmatrix[0], mvmatrix[1], mvmatrix[2], mvmatrix[3]);
     printf("m[ 4.. 7] %f, %f, %f, %f \n",
             mvmatrix[4], mvmatrix[5], mvmatrix[6], mvmatrix[7]);
     printf("m[ 8..11] %f, %f, %f, %f \n",
             mvmatrix[8], mvmatrix[9], mvmatrix[10], mvmatrix[11]);
     printf("m[12..15] %f, %f, %f, %f \n",
             mvmatrix[12], mvmatrix[13], mvmatrix[14], mvmatrix[15]);
                   
     glGetDoublev(GL_PROJECTION_MATRIX, projmatrix);
     printf("projmatrix\n");
     printf("m[ 0.. 3] %f, %f, %f, %f \n",
             projmatrix[0], projmatrix[1], projmatrix[2], projmatrix[3]);
     printf("m[ 4.. 7] %f, %f, %f, %f \n",
             projmatrix[4], projmatrix[5], projmatrix[6], projmatrix[7]);
     printf("m[ 8..11] %f, %f, %f, %f \n",
             projmatrix[8], projmatrix[9], projmatrix[10], projmatrix[11]);
     printf("m[12..15] %f, %f, %f, %f \n",
             projmatrix[12], projmatrix[13], projmatrix[14], projmatrix[15]);
                   
     /*  note viewport[3] is height of window in pixels  */
     realy = viewport[3] - (GLint) y - 1;
     printf ("Coordinates at cursor are (x=%4d, yinv=%4d)\n", x, realy);
     gluUnProject((GLdouble) x, (GLdouble) realy, 0.0, 
                  mvmatrix, projmatrix, viewport, &wx0, &wy0, &wz0); 
     printf("World coords at winz=0.0 are (%f, %f, %f)\n", 
              wx0, wy0, wz0);
     gluUnProject((GLdouble) x, (GLdouble) realy, 1.0, 
                  mvmatrix, projmatrix, viewport, &wx1, &wy1, &wz1); 
     printf("World coords at winz=1.0 are (%f, %f, %f)\n", 
             wx1, wy1, wz1);
     gluUnProject((GLdouble) x, (GLdouble) realy, 0.0, 
                  m, projmatrix, viewport, &wx0, &wy0, &wz0); 
     printf("My coords at winz=0.0 are (%f, %f, %f)\n", 
              wx0, wy0, wz0);
     gluUnProject((GLdouble) x, (GLdouble) realy, 1.0, 
                  m, projmatrix, viewport, &wx1, &wy1, &wz1); 
     printf("My coords at winz=1.0 are (%f, %f, %f)\n", 
              wx1, wy1, wz1);
     for(i=0; i<4; i++)
     {
       d = dist(wx0, wy0, wz0, wx1, wy1, wz1, points[i][0], points[i][1], points[i][2]);
       printf("distance, d to point %d is %g\n", i, d); 
     }
     d = dist(1.0, 2.0, 4.0, 3.0, 6.0, 12.0, 2.0, 4.0, 8.0);
     printf("test dist d=%g\n", d);
  }
}

void keyboard(unsigned char key, int x, int y)
{
   switch (key) {
      case 27:
         exit(0);
         break;
   }
}
   
/* 
 *  Open window, register input callback functions
 */
int main(int argc, char** argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode (GLUT_SINGLE | GLUT_RGB);
   glutInitWindowSize (500, 500); 
   glutInitWindowPosition (100, 100);
   glutCreateWindow (argv[0]);
   glutDisplayFunc(display); 
   glutReshapeFunc(reshape); 
   glutKeyboardFunc (keyboard);
   glutMouseFunc(mouse);
   glutMainLoop();
   return 0;
}