/*
 *   GRacer
 *
 *   Copyright (C) 1999 Takashi Matsuda <matsu@users.sourceforge.net>
 *
 * This program is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public License as 
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "gr_vertex.h"

static GrVertex _origin = {{0, 0, 0}};
static GrVertex _ux = {{1, 0, 0}};
static GrVertex _uy = {{0, 1, 0}};
static GrVertex _uz = {{0, 0, 1}};

GrVertex  *gr_vertex_origin = &_origin;
GrVertex  *gr_vertex_unit_x = &_ux;
GrVertex  *gr_vertex_unit_y = &_uy;
GrVertex  *gr_vertex_unit_z = &_uz;

static GrVertexd _origind = {{0, 0, 0}};
static GrVertexd _uxd = {{1, 0, 0}};
static GrVertexd _uyd = {{0, 1, 0}};
static GrVertexd _uzd = {{0, 0, 1}};

GrVertexd  *gr_vertexd_origin = &_origind;
GrVertexd  *gr_vertexd_unit_x = &_uxd;
GrVertexd  *gr_vertexd_unit_y = &_uyd;
GrVertexd  *gr_vertexd_unit_z = &_uzd;

#ifndef GR_VERTEX_USE_MACRO

int
gr_vertex_equal (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.x == v2->c.x && v1->c.y == v2->c.y && v1->c.z == v2->c.z;
}

void
gr_vertex_scale (GrVertex *d, const GrVertex *s, const float t)
{
  d->c.x = s->c.x * t;
  d->c.y = s->c.y * t;
  d->c.z = s->c.z * t;
}

void
gr_vertex_add (GrVertex *d, const GrVertex *s1, const GrVertex *s2)
{
  d->c.x = s1->c.x + s2->c.x;
  d->c.y = s1->c.y + s2->c.y;
  d->c.z = s1->c.z + s2->c.z;
}

void
gr_vertex_sub (GrVertex *d, const GrVertex *s1, const GrVertex *s2)
{
  d->c.x = s1->c.x - s2->c.x;
  d->c.y = s1->c.y - s2->c.y;
  d->c.z = s1->c.z - s2->c.z;
}

double
gr_vertex_length (const GrVertex *v)
{
  return sqrt ((double)(v->c.x)*v->c.x +
               (double)(v->c.y)*v->c.y +
	       (double)(v->c.z)*v->c.z);
}

void
gr_vertex_cross (GrVertex *d, const GrVertex *s1, const GrVertex *s2)
{
  GrVertex tmp, *v;

  if (d == s1 || d == s2)
    v = &tmp;
  else
    v = d;

  v->c.x = s1->c.y * s2->c.z - s2->c.y * s1->c.z;
  v->c.y = s2->c.x * s1->c.z - s1->c.x * s2->c.z;
  v->c.z = s1->c.x * s2->c.y - s2->c.x * s1->c.y;

  if (v == &tmp)
    *d = tmp;
}

double
gr_vertex_product (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.x * v2->c.x + v1->c.y * v2->c.y + v1->c.z * v2->c.z;
}


int
gr_vertex_equal_xy (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.x == v2->c.x  && v1->c.y == v2->c.y;
}

void
gr_vertex_scale_xy (GrVertex *d, const GrVertex *s, const float t)
{
  d->c.x = s->c.x * t;
  d->c.y = s->c.y * t;
}

void
gr_vertex_scale_xya (GrVertex *d, const float t)
{
  d->c.x *= t;
  d->c.y *= t;
}

void
gr_vertex_add_xy (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.x = v->c.x + w->c.x;
  d->c.y = v->c.y + w->c.y;
}

void
gr_vertex_add_xya (GrVertex *d, const GrVertex *s)
{
  d->c.x += s->c.x;
  d->c.y += s->c.y;
}

void
gr_vertex_sub_xy (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.x = v->c.x - w->c.x;
  d->c.y = v->c.y - w->c.y;
}

void
gr_vertex_sub_xya (GrVertex *d, const GrVertex *s)
{
  d->c.x -= s->c.x;
  d->c.y -= s->c.y;
}

double
gr_vertex_length_xy (const GrVertex *v)
{
  return sqrt (v->c.x * v->c.x + v->c.y * v->c.y);
}

void
gr_vertex_rot_xy (GrVertex *d, const GrVertex *s,
    		  const double sn, const double cn)
{
  d->c.x = s->c.x * cn - s->c.y * sn;
  d->c.y = s->c.y * cn + s->c.x * sn;
}

double
gr_vertex_product_xy (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.x * v2->c.x + v1->c.y * v2->c.y;
}


int
gr_vertex_equal_yz (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.y == v2->c.y  && v1->c.z == v2->c.z;
}

void
gr_vertex_scale_yz (GrVertex *d, const GrVertex *s, const float t)
{
  d->c.y = s->c.y * t;
  d->c.z = s->c.z * t;
}

void
gr_vertex_scale_yza (GrVertex *d, const float t)
{
  d->c.y *= t;
  d->c.z *= t;
}

void
gr_vertex_add_yz (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.y = v->c.y + w->c.y;
  d->c.z = v->c.z + w->c.z;
}

void
gr_vertex_add_yza (GrVertex *d, const GrVertex *s)
{
  d->c.y += s->c.y;
  d->c.z += s->c.z;
}

void
gr_vertex_sub_yz (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.y = v->c.y - w->c.y;
  d->c.z = v->c.z - w->c.z;
}

void
gr_vertex_sub_yza (GrVertex *d, const GrVertex *s)
{
  d->c.y -= s->c.y;
  d->c.z -= s->c.z;
}

double
gr_vertex_length_yz (const GrVertex *v)
{
  return sqrt (v->c.y * v->c.y + v->c.z * v->c.z);
}

void
gr_vertex_rot_yz (GrVertex *d, const GrVertex *s,
    		  const double sn, const double cn)
{
  d->c.y = s->c.y * cn - s->c.z * sn;
  d->c.z = s->c.z * cn + s->c.y * sn;
}

double
gr_vertex_product_yz (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.y * v2->c.y + v1->c.z * v2->c.z;
}


int
gr_vertex_equal_zx (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.z == v2->c.z  && v1->c.x == v2->c.x;
}

void
gr_vertex_scale_zx (GrVertex *d, const GrVertex *s, const float t)
{
  d->c.z = s->c.z * t;
  d->c.x = s->c.x * t;
}

void
gr_vertex_scale_xxa (GrVertex *d, const float t)
{
  d->c.z *= t;
  d->c.x *= t;
}

void
gr_vertex_add_zx (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.z = v->c.z + w->c.z;
  d->c.x = v->c.x + w->c.x;
}

void
gr_vertex_add_zxa (GrVertex *d, const GrVertex *s)
{
  d->c.z += s->c.z;
  d->c.x += s->c.x;
}

void
gr_vertex_sub_zx (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  d->c.z = v->c.z - w->c.z;
  d->c.x = v->c.x - w->c.x;
}

void
gr_vertex_sub_zxa (GrVertex *d, const GrVertex *s)
{
  d->c.z -= s->c.z;
  d->c.x -= s->c.x;
}

double
gr_vertex_length_zx (const GrVertex *v)
{
  return sqrt (v->c.z * v->c.z + v->c.x * v->c.x);
}

void
gr_vertex_rot_zx (GrVertex *d, const GrVertex *s,
    		  const double sn, const double cn)
{
  d->c.z = s->c.z * cn - s->c.x * sn;
  d->c.x = s->c.x * cn + s->c.z * sn;
}

double
gr_vertex_product_zx (const GrVertex *v1, const GrVertex *v2)
{
  return v1->c.z * v2->c.z + v1->c.x * v2->c.x;
}


#endif /* ! GR_VERTEX_USE_MACRO */

static int
check_side (GrVertex *v, GrVertex *w)
{
  return v->c.x * w->c.y - w->c.x * v->c.y > -0.00001;
}

int
gr_vertex_check_inner_xy (GrVertex *p, int num, GrVertex *v)
{
  int i;
  GrVertex t, u;

  for (i=0; i<num; i++) {
    t = v[(i + 1) % num];
    u = *p;

    gr_vertex_sub_xya (&t, &v[i]);
    gr_vertex_sub_xya (&u, &v[i]);

    if (!check_side (&t, &u))
      return 0;
  }

  return 1;
}

void
gr_vertex_normalize (GrVertex *d, const GrVertex *s)
{
  double len = gr_vertex_length (s);

  d->c.x = s->c.x / len;
  d->c.y = s->c.y / len;
  d->c.z = s->c.z / len;
}

void
gr_vertex_normal (GrVertex *d, const GrVertex *v, const GrVertex *w)
{
  gr_vertex_cross (d, v, w);
  gr_vertex_normalize (d, d);
}

void
gr_vertex_normalize_xy (GrVertex *d, const GrVertex *s)
{
  double len = gr_vertex_length_xy (s);

  d->c.x = s->c.x / len;
  d->c.y = s->c.y / len;
}

void
gr_vertex_normalize_yz (GrVertex *d, const GrVertex *s)
{
  double len = gr_vertex_length_yz (s);

  d->c.y = s->c.y * len;
  d->c.z = s->c.z * len;
}

void
gr_vertex_normalize_zx (GrVertex *d, const GrVertex *s)
{
  double len = gr_vertex_length_zx (s);

  d->c.z = s->c.z * len;
  d->c.x = s->c.y * len;
}

int
gr_vertex_near (const GrVertex *v1,
		const GrVertex *v2,
		const double threshold)
{
  GrVertex v;
  double distance;
  
  gr_vertex_sub (&v, v1, v2);
  distance = gr_vertex_length (&v);
  
  if (distance <= threshold)
    return 1;
  else
    return 0;
}

int
gr_vertex_near_xy (const GrVertex *v1,
    		   const GrVertex *v2,
		   const double threshold)
{
  GrVertex v;
  double distance;
  
  gr_vertex_sub_xy (&v, v1, v2);
  distance = gr_vertex_length_xy (&v);
  
  if (distance <= threshold)
    return 1;
  else
    return 0;
}

int
gr_vertex_near_yz (const GrVertex *v1,
    		   const GrVertex *v2,
		   const double threshold)
{
  GrVertex v;
  double distance;
  
  gr_vertex_sub_yz (&v, v1, v2);
  distance = gr_vertex_length_yz (&v);
  
  if (distance <= threshold)
    return 1;
  else
    return 0;
}

int
gr_vertex_near_zx (const GrVertex *v1,
    		   const GrVertex *v2,
		   const double threshold)
{
  GrVertex v;
  double distance;
  
  gr_vertex_sub_zx (&v, v1, v2);
  distance = gr_vertex_length_zx (&v);
  
  if (distance <= threshold)
    return 1;
  else
    return 0;
}

void gr_vertex_rotate (GrVertex *dst, GrVertex *src,
		       float s, float c, float x, float y, float z)
{
  float m[3][3];
  GrVertex tmp;
  float sx, sy, sz;
  float c2 = 1.0 - c;
  int i;

  if (src == dst) {
    tmp = *src;
    src = &tmp;
  }

  sx = x * s;
  sy = y * s;
  sz = z * s;

  m[0][0] = c2 * x * x + c;
  m[0][1] = c2 * x * y - sz;
  m[0][2] = c2 * x * z + sy;

  m[1][0] = c2 * y * x + sz;
  m[1][1] = c2 * y * y + c;
  m[1][2] = c2 * y * z - sx;

  m[2][0] = c2 * z * x - sy;
  m[2][1] = c2 * z * y + sx;
  m[2][2] = c2 * z * z + c;

  for (i=0; i<3; i++) {
    dst->f[i] = m[i][0] * src->f[0];
    dst->f[i] += m[i][1] * src->f[1];
    dst->f[i] += m[i][2] * src->f[2];
  }
}