/* vogel.c:  graphics functions to draw scenes.  Written by Peng Zhang      
             Oct. 19, 1995                                               */

/*----------------------------- Local Includes -----------------------------*/
 
#include "polytope.h"
 
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <collision.h>
#include <matrix.h>
#include <vogl.h>
#include <vodevice.h>

#define sqr(x) ((x) * (x))
 
 
 
/*----------------------------- Local Constants -----------------------------*/
 

/*------------------------------ Local Globals ------------------------------*/
 
extern __col_Polyhedron      __col_polyhedronLibrary[1000];
extern __col_State  state;
extern double cube_bound;

 
/*---------------------------------Functions-------------------------------- */

void vogl_draw_scene(Status *status, float size, int wire);
void draw_poly(__col_Polyhedron *p, int wire);
void openwindow();
void draw_plane(col_Vect3 pt1, col_Vect3 pt2, col_Vect3 pt3, int color1, 
                 int color2, double div,  int wire);
void vectCopy(col_Vect3 src, col_Vect3 dest);
void vectSub(col_Vect3 a, col_Vect3 b, col_Vect3 c);
void vectNormalize(col_Vect3 src, col_Vect3 dest);
__col_Real vectNorm(col_Vect3 v);
void displacePoint(col_Vect3 point, col_Vect3 vect,
                         __col_Real lambda, col_Vect3 result);


/*****************************************************************************\
@ draw_plane()
-----------------------------------------------------------------------------
description : function to draw a plane given by three points, using the given 
              division and color 
input       : three points, two colors, and one division number
output      :
\*****************************************************************************/

void draw_plane(col_Vect3 pt1, col_Vect3 pt2, col_Vect3 pt3, int color1, 
                int color2, double div,  int wire)
{
     col_Vect3 dir1, dir2;
     col_Vect3 temp1, temp2;
     col_Vect3 temp3, temp4;
     col_Vect3 tt, ttt;
     int i, j, DIV;
     double inc1, inc2;


     DIV = div;
     vectSub(pt2, pt1, dir1);
     inc1 = vectNorm(dir1) / div;
     vectSub(pt3, pt1, dir2);
     inc2 = vectNorm(dir2) / div;
     vectNormalize(dir1, dir1);
     vectNormalize(dir2, dir2);
     vectCopy(pt1, tt);

     pushmatrix();   
     for(j=1; j<(DIV + 1); j++) {
         for(i=0; i<DIV; i++) {
             if (wire) bgnclosedline(); else bgnpolygon();
                color(((i+j)%2)?color1:color2);
                displacePoint(tt, dir1, i*inc1, temp1);
		v3d(temp1);
                displacePoint(tt, dir1, (i+1)*inc1, temp2);
		v3d(temp2);
                displacePoint(temp2, dir2, inc2, temp3);
		v3d(temp3);
                displacePoint(temp1, dir2, inc2, temp4);
		v3d(temp4);
                if( i == 0) vectCopy(temp4, ttt);
             if (wire) endclosedline(); else endpolygon();
	    }
                vectCopy(ttt, tt);
       }
    popmatrix();    
}
	
                


/*****************************************************************************\
draw_scene()
-----------------------------------------------------------------------------
description : Draw out the scene for the simulation
input       : status, polytope list
output      :
\*****************************************************************************/

void vogl_draw_scene(Status *status, float size,int wire)
{
    int i;

       pushmatrix();
   scale(size,size,size);
/*   for(i=0; i < status->num_polytopes; i++)  {   */
   for(i=(status->num_polytopes)-1; i >= 0 ; i--)  {
            draw_poly(state.polytopes[i].polytope, wire); 
   }
        popmatrix();
/*        swapbuffers();      */
}
 
/*****************************************************************************\
@ draw_poly()
-----------------------------------------------------------------------------
description:  Draw the picture of a polyhedron by passing its data structure.
input      :  a polyhedron
output     :
\*****************************************************************************/
void draw_poly(__col_Polyhedron *p, int wire)
{
   col_Fnode *fn, *fn2;
   int clor = 1;
 
   for(fn = p->faces; fn; fn = fn->next) {
     if(wire) bgnclosedline();  else bgnpolygon();
       clor = clor % 5 ;
       color(clor+1);
       clor++;
         for(fn2 = fn->f.f->verts; fn2; fn2 = fn2->next) {
             __col_xformPoint(p->pose, fn2->f.v->coords, fn2->f.v->xcoords);
             v3d(fn2->f.v->xcoords);
            }
     if(wire) endclosedline(); else endpolygon();
   }
 
}


/*****************************************************************************\
@ openwindow()
-----------------------------------------------------------------------------
description : Do the necessary initiation for vogl.
input       :
output      :
\*****************************************************************************/
void openwindow()
/* Opens my own window */
{
  double far, look_pt;

  far = 4.0*cube_bound+10.0;
  look_pt = 3.02*cube_bound+1  ;

  prefsize(500L, 500L);
  winopen("Cube-Master");
  doublebuffer();
  gconfig();
  perspective(500,1.0,0.6,far);
  lookat(0.0,0.0,look_pt,0.0,0.0,0.0,3600);
  backface(TRUE);
  qdevice(KEYBD);
  unqdevice(INPUTCHANGE);
}
 

void vectCopy(col_Vect3 src, col_Vect3 dest)
{
  dest[0] = src[0];
  dest[1] = src[1];
  dest[2] = src[2];
} 

void vectSub(col_Vect3 a, col_Vect3 b, col_Vect3 c)
{
  c[0] = a[0] - b[0];
  c[1] = a[1] - b[1];
  c[2] = a[2] - b[2];
} 

void vectNormalize(col_Vect3 src, col_Vect3 dest)
{
  __col_Real l;
 
  l = vectNorm(src);
  dest[0] = src[0] / l;
  dest[1] = src[1] / l;
  dest[2] = src[2] / l;
} 

double vectNorm(col_Vect3 v)
{
  return sqrt((double) ( sqr(v[0]) + sqr(v[1]) + sqr(v[2]) ) );
} 
 

void displacePoint(col_Vect3 point, col_Vect3 vect,
                         __col_Real lambda, col_Vect3 result)
{
  result[0] = point[0] + lambda * vect[0];
  result[1] = point[1] + lambda * vect[1];
  result[2] = point[2] + lambda * vect[2];
}