#include <stdio.h>
#include <math.h>
#include "gr_vertex.h"
#include "gr_plane.h"
#include "gr_debug.h"

void
gr_plane_init (GrPlane *plane,
	       GrVertex *p0,
	       GrVertex *p1,
	       GrVertex *p2)
{
  GrVertex s;
  GrVertex u[3];
  int i, j;

  s = *p0;
  gr_vertex_sub (&u[0], p1, p0);
  gr_vertex_sub (&u[1], p2, p0);

  gr_vertex_normalize (&u[0], &u[0]);
  gr_vertex_normalize (&u[1], &u[1]);

  gr_vertex_normal (&u[2], &u[0], &u[1]);
  gr_vertex_normal (&u[1], &u[2], &u[0]);

  for (i=0; i<3; i++) {
    plane->m.f[i][3] = 0;
    plane->m.f[3][i] = 0;
    for (j=0; j<3; j++) {
      plane->m.f[i][j] = u[i].f[j];
      plane->m.f[i][3] -= u[i].f[j] * s.f[j];
    }
  }
  plane->m.f[3][3] = 1;

  gr_matrix_invert (&plane->m, &plane->im);

  gr_vertex_normalize (&plane->n, &u[2]);
  plane->s_n = gr_vertex_product (&s, &plane->n);

  gr_matrix_mult_vertex (&plane->m, gr_vertex_origin, &plane->orig);
}

int
gr_plane_get_crossing (GrPlane *plane,
		       GrVertex *c,
		       GrVertex *p,
		       GrVertex *v)
{
  double p_n;
  double v_n;
  double f;

  p_n = gr_vertex_product (p, &plane->n);
  v_n = gr_vertex_product (v, &plane->n);
  
  if (v_n != 0) {
    f = (plane->s_n - p_n) / v_n;

    c->c.x = v->c.x * f + p->c.x;
    c->c.y = v->c.y * f + p->c.y;
    c->c.z = v->c.z * f + p->c.z;

    return 0;
  } else {
    return -1;
  }
}