/* geometry.c
   Copyright (C) 2005,2006,2007,2008 Eugene K. Ressler, Jr.

This file is part of Sketch, a small, simple system for making 
3d drawings with LaTeX and the PSTricks or TikZ package.

Sketch 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 3, or (at your option)
any later version.

Sketch 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 Sketch; see the file COPYING.txt.  If not, see
http://www.gnu.org/copyleft */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "geometry.h"
#include "error.h"
#include "memutil.h"

// global constants 
POINT_2D origin_2d = { 0, 0 };
POINT_3D origin_3d = { 0, 0, 0 };
VECTOR_2D I_2d = { 1, 0 };
VECTOR_2D J_2d = { 0, 1 };
VECTOR_3D I_3d = { 1, 0, 0 };
VECTOR_3D J_3d = { 0, 1, 0 };
VECTOR_3D K_3d = { 0, 0, 1 };

TRANSFORM identity = {
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1
};

// numerics 

FLOAT max_float(FLOAT x, FLOAT y)
{
    return x > y ? x : y;
}

FLOAT min_float(FLOAT x, FLOAT y)
{
    return x < y ? x : y;
}

// points

void copy_pt_2d(POINT_2D r, POINT_2D s)
{
    r[X] = s[X];
    r[Y] = s[Y];
}

void copy_pt_3d(POINT_3D r, POINT_3D s)
{
    r[X] = s[X];
    r[Y] = s[Y];
    r[Z] = s[Z];
}

void find_pt_3d_from_2d(POINT_3D r, POINT_2D pt)
{
    r[X] = pt[X];
    r[Y] = pt[Y];
    r[Z] = 0;
}

// polyline initialization and cleanup

#define SET_NEXT_NULL a->next = NULL;

DECLARE_DYNAMIC_2D_ARRAY_FUNCS(POLYLINE_2D, POINT_2D, FLOAT, polyline_2d,
			       v, n_vertices, SET_NEXT_NULL);
DECLARE_DYNAMIC_2D_ARRAY_FUNCS(POLYLINE_3D, POINT_3D, FLOAT, polyline_3d,
			       v, n_vertices, SET_NEXT_NULL);
// polygon initialization and cleanup
DECLARE_DYNAMIC_2D_ARRAY_FUNCS(POLYGON_2D, POINT_2D, FLOAT, polygon_2d, v,
			       n_sides, SET_NEXT_NULL);
DECLARE_DYNAMIC_2D_ARRAY_FUNCS(POLYGON_3D, POINT_3D, FLOAT, polygon_3d, v,
			       n_sides, SET_NEXT_NULL);

// rudimentary vectors of variable size
void init_vec(VECTOR * v)
{
    *v = 0;
}

void clear_vec(VECTOR * v)
{
    safe_free(*v);
    init_vec(v);
}

void setup_vec(VECTOR * v, SIZE n)
{
    clear_vec(v);
    *v = safe_malloc(n * sizeof(FLOAT));
}

void init_and_setup_vec(VECTOR * v, SIZE n)
{
    *v = safe_malloc(n * sizeof(FLOAT));
}

void zero_vec(VECTOR r, SIZE n)
{
    INDEX i;

    for (i = 0; i < n; i++)
	r[i] = 0;
}

void copy_vec(VECTOR r, VECTOR v, SIZE n)
{
    INDEX i;

    for (i = 0; i < n; i++)
	r[i] = v[i];
}

FLOAT length_vec_2d(VECTOR_2D v)
{
    return sqrt(dot_2d(v, v));
}

FLOAT length_vec_3d(VECTOR_3D v)
{
    return sqrt(dot_3d(v, v));
}

FLOAT dist_2d(POINT_2D p1, POINT_2D p2)
{
    VECTOR_2D dif;
    sub_pts_2d(dif, p1, p2);
    return length_vec_2d(dif);
}

FLOAT dist_3d(POINT_3D p1, POINT_3D p2)
{
    VECTOR_3D dif;
    sub_pts_3d(dif, p1, p2);
    return length_vec_3d(dif);
}

FLOAT length_vec_2d_sqr(VECTOR_2D v)
{
    return dot_2d(v, v);
}

FLOAT length_vec_3d_sqr(VECTOR_3D v)
{
    return dot_3d(v, v);
}

FLOAT dist_2d_sqr(POINT_2D p1, POINT_2D p2)
{
    VECTOR_2D dif;
    sub_pts_2d(dif, p1, p2);
    return length_vec_2d_sqr(dif);
}

FLOAT dist_3d_sqr(POINT_3D p1, POINT_3D p2)
{
    VECTOR_3D dif;
    sub_pts_3d(dif, p1, p2);
    return length_vec_3d_sqr(dif);
}

void zero_vec_2d(VECTOR_2D v)
{
    v[X] = v[Y] = 0;
}

void zero_vec_3d(VECTOR_3D v)
{
    v[X] = v[Y] = v[Z] = 0;
}

void negate_vec_2d(VECTOR_2D r, VECTOR_2D v)
{
    r[X] = -v[X];
    r[Y] = -v[Y];
}

void negate_vec_3d(VECTOR_3D r, VECTOR_3D v)
{
    r[X] = -v[X];
    r[Y] = -v[Y];
    r[Z] = -v[Z];
}

void copy_vec_2d(VECTOR_2D r, VECTOR_2D s)
{
    r[X] = s[X];
    r[Y] = s[Y];
}

void copy_vec_3d(VECTOR_3D r, VECTOR_3D s)
{
    r[X] = s[X];
    r[Y] = s[Y];
    r[Z] = s[Z];
}

void scale_vec_2d(VECTOR_2D r, VECTOR_2D v, FLOAT s)
{
    r[X] = v[X] * s;
    r[Y] = v[Y] * s;
}

void scale_vec_3d(VECTOR_3D r, VECTOR_3D v, FLOAT s)
{
    r[X] = v[X] * s;
    r[Y] = v[Y] * s;
    r[Z] = v[Z] * s;
}

int find_unit_vec_2d(VECTOR_2D r, VECTOR_2D v)
{
    FLOAT len = length_vec_2d(v);
    if (len <= FLT_EPSILON) {
	r[X] = 1;
	r[Y] = 0;
	return 0;
    } else {
	scale_vec_2d(r, v, 1 / len);
	return 1;
    }
}

int find_unit_vec_3d(VECTOR_3D r, VECTOR_3D v)
{
    FLOAT len = length_vec_3d(v);
    if (len == FLT_EPSILON) {
	r[X] = 1;
	r[Y] = r[Z] = 0;
	return 0;
    } else {
	scale_vec_3d(r, v, 1 / len);
	return 1;
    }
}

void add_vecs_2d(VECTOR_2D r, VECTOR_2D a, VECTOR_2D b)
{
    r[X] = a[X] + b[X];
    r[Y] = a[Y] + b[Y];
}

void add_vecs_3d(VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
{
    r[X] = a[X] + b[X];
    r[Y] = a[Y] + b[Y];
    r[Z] = a[Z] + b[Z];
}

void sub_vecs_2d(VECTOR_2D r, VECTOR_2D a, VECTOR_2D b)
{
    r[X] = a[X] - b[X];
    r[Y] = a[Y] - b[Y];
}

void sub_vecs_3d(VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
{
    r[X] = a[X] - b[X];
    r[Y] = a[Y] - b[Y];
    r[Z] = a[Z] - b[Z];
}

void add_vec_to_pt_2d(POINT_2D r, POINT_2D pt, VECTOR_2D v)
{
    r[X] = pt[X] + v[X];
    r[Y] = pt[Y] + v[Y];
}

void add_vec_to_pt_3d(POINT_3D r, POINT_3D pt, VECTOR_3D v)
{
    r[X] = pt[X] + v[X];
    r[Y] = pt[Y] + v[Y];
    r[Z] = pt[Z] + v[Z];
}

void add_scaled_vec_to_pt_2d(POINT_2D r, POINT_2D pt, VECTOR_2D v, FLOAT s)
{
    r[X] = pt[X] + v[X] * s;
    r[Y] = pt[Y] + v[Y] * s;
}

void add_scaled_vec_to_pt_3d(POINT_3D r, POINT_3D pt, VECTOR_3D v, FLOAT s)
{
    r[X] = pt[X] + v[X] * s;
    r[Y] = pt[Y] + v[Y] * s;
    r[Z] = pt[Z] + v[Z] * s;
}

void sub_pts_2d(VECTOR_2D r, POINT_2D a, POINT_2D b)
{
    r[X] = a[X] - b[X];
    r[Y] = a[Y] - b[Y];
}

void sub_pts_3d(VECTOR_3D r, POINT_3D a, POINT_3D b)
{
    r[X] = a[X] - b[X];
    r[Y] = a[Y] - b[Y];
    r[Z] = a[Z] - b[Z];
}

void fold_min_pt_2d(POINT_2D min, POINT_2D new_pt)
{
    int i;

    for (i = 0; i < 2; i++)
	if (new_pt[i] < min[i])
	    min[i] = new_pt[i];
}

void fold_min_pt_3d(POINT_3D min, POINT_3D new_pt)
{
    int i;

    for (i = 0; i < 3; i++)
	if (new_pt[i] < min[i])
	    min[i] = new_pt[i];
}

void fold_max_pt_2d(POINT_2D max, POINT_3D new_pt)
{
    int i;

    for (i = 0; i < 2; i++)
	if (new_pt[i] > max[i])
	    max[i] = new_pt[i];
}

void fold_max_pt_3d(POINT_3D max, POINT_3D new_pt)
{
    int i;

    for (i = 0; i < 3; i++)
	if (new_pt[i] > max[i])
	    max[i] = new_pt[i];
}

FLOAT dot_2d(VECTOR_2D a, VECTOR_2D b)
{
    return a[X] * b[X] + a[Y] * b[Y];
}

FLOAT dot_3d(VECTOR_3D a, VECTOR_3D b)
{
    return a[X] * b[X] + a[Y] * b[Y] + a[Z] * b[Z];
}

void cross(VECTOR_3D r, VECTOR_3D a, VECTOR_3D b)
{
    r[X] = a[Y] * b[Z] - a[Z] * b[Y];
    r[Y] = a[Z] * b[X] - a[X] * b[Z];
    r[Z] = a[X] * b[Y] - a[Y] * b[X];
}

void lerp_2d(POINT_2D r, FLOAT t, POINT_2D p1, POINT_2D p2)
{
    r[0] = p1[0] + t * (p2[0] - p1[0]);
    r[1] = p1[1] + t * (p2[1] - p1[1]);
}

void lerp_3d(POINT_3D r, FLOAT t, POINT_3D p1, POINT_3D p2)
{
    r[0] = p1[0] + t * (p2[0] - p1[0]);
    r[1] = p1[1] + t * (p2[1] - p1[1]);
    r[2] = p1[2] + t * (p2[2] - p1[2]);
}

int
line_intersect_2d(POINT_2D a, POINT_2D b, POINT_2D c, POINT_2D d,
		  FLOAT eps, FLOAT * t_ab, FLOAT * t_cd)
{
    FLOAT dx_ab, dy_ab, dx_dc, dy_dc, det, dx_ac, dy_ac;

    dx_ab = b[X] - a[X];
    dy_ab = b[Y] - a[Y];
    dx_dc = c[X] - d[X];
    dy_dc = c[Y] - d[Y];
    det = dx_ab * dy_dc - dx_dc * dy_ab;
    if (-eps < det && det < eps)
	return 1;
    dx_ac = c[X] - a[X];
    dy_ac = c[Y] - a[Y];
    *t_ab = (dx_ac * dy_dc - dx_dc * dy_ac) / det;
    *t_cd = (dx_ab * dy_ac - dx_ac * dy_ab) / det;
    return 0;
}

void find_polygon_plane(PLANE * plane, POLYGON_3D * polygon)
{
    int i, j;
    VECTOR_3D sum, dif;

    zero_vec_3d(plane->p);
    zero_vec_3d(plane->n);
    for (i = 0, j = polygon->n_sides - 1; i < polygon->n_sides; j = i++) {
	add_vecs_3d(plane->p, plane->p, polygon->v[i]);
	add_vecs_3d(sum, polygon->v[j], polygon->v[i]);
	sub_vecs_3d(dif, polygon->v[j], polygon->v[i]);
	plane->n[X] += dif[Y] * sum[Z];
	plane->n[Y] += dif[Z] * sum[X];
	plane->n[Z] += dif[X] * sum[Y];
    }
    scale_vec_3d(plane->p, plane->p, 1.0 / polygon->n_sides);
    find_unit_vec_3d(plane->n, plane->n);
    plane->c = -dot_3d(plane->p, plane->n);
}

int pt_side_of_plane(PLANE * plane, POINT_3D p)
{
    FLOAT d = dot_3d(p, plane->n) + plane->c;
    return d < -PLANE_HALF_THICKNESS ? S_IN :
	d > PLANE_HALF_THICKNESS ? S_OUT :
	d < 0 ? S_IN_ON : d > 0 ? S_OUT_ON : S_ON;
}

int polygon_side_of_plane(POLYGON_3D * polygon, PLANE * plane)
{
    int i, j, i_side, j_side, n_in, n_out;

    // initialize with last point in polygon
    // scan for OUT-IN or IN-OUT pair
    j = polygon->n_sides - 1;
    j_side = pt_side_of_plane(plane, polygon->v[j]);
    n_in = n_out = 0;
    for (i = 0; i < polygon->n_sides; i++) {

	// advance to next vertex
	i_side = pt_side_of_plane(plane, polygon->v[i]);

	if ((i_side | j_side) == (S_IN | S_OUT))
	    // found a straddling pair
	    return S_SPLIT;

	if (i_side & (S_IN | S_OUT))
	    // found an IN or an OUT; remember it
	    j_side = i_side;

	// keep counts for polygons entirely inside the thick plane
	if (i_side == S_OUT_ON)
	    n_out++;
	if (i_side == S_IN_ON)
	    n_in++;
    }
    return
	j_side & (S_IN | S_OUT) ? j_side :
	(n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
}

#if TREAT_POLYLINE_POINTS_ON_PLANE_AS_IN_OR_OUT

// this will work only with BSPs, not with depth sort
// it causes polylines that end  on a plane to be split into a line and a point
int polyline_side_of_plane(POLYLINE_3D * polyline, PLANE * plane)
{
    int i, j, i_side, j_side, n_in, n_out;
    //  predicate for "if more than one bit set..."
    //                       0  1  2  3  4  5  6  7
    static int is_split_p[] = { 0, 0, 0, 1, 0, 1, 1, 1 };

    // initialize with first point in polyline
    // scan for OUT-IN or IN-OUT pair
    j = 0;
    i_side = pt_side_of_plane(plane, polyline->v[j]);
    n_in = n_out = 0;
    for (i = 1; i < polyline->n_vertices; i++) {
	// advance to next vertex, remembering side of last
	j_side = i_side;
	i_side = pt_side_of_plane(plane, polyline->v[i]);

	if (is_split_p[(i_side | j_side) & 7])
	    return S_SPLIT;

	// keep counts for polylines entirely inside the thick plane
	if (i_side == S_OUT_ON)
	    n_out++;
	if (i_side == S_IN_ON)
	    n_in++;
    }
    return
	i_side & (S_IN | S_OUT) ? i_side :
	(n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
}

#else

int polyline_side_of_plane(POLYLINE_3D * polyline, PLANE * plane)
{
    int i, j, i_side, j_side, n_in, n_out;

    // initialize with last point in polygon
    // scan for OUT-IN or IN-OUT pair
    j = polyline->n_vertices - 1;
    j_side = pt_side_of_plane(plane, polyline->v[j]);
    n_in = n_out = 0;
    for (i = 0; i < polyline->n_vertices; i++) {

	// advance to next vertex
	i_side = pt_side_of_plane(plane, polyline->v[i]);

	if ((i_side | j_side) == (S_IN | S_OUT))
	    // found a straddling pair
	    return S_SPLIT;

	if (i_side & (S_IN | S_OUT))
	    // found an IN or an OUT; remember it
	    j_side = i_side;

	// keep counts for polylines entirely inside the thick plane
	if (i_side == S_OUT_ON)
	    n_out++;
	if (i_side == S_IN_ON)
	    n_in++;
    }
    return
	j_side & (S_IN | S_OUT) ? j_side :
	(n_out > n_in) ? S_OUT : (n_in > n_out) ? S_IN : S_ON;
}

#endif

void init_box_2d(BOX_2D * b)
{
    b->min[X] = b->min[Y] = FLOAT_MAX;
    b->max[X] = b->max[Y] = -FLOAT_MAX;
}

void init_box_3d(BOX_3D * b)
{
    b->min[X] = b->min[Y] = b->min[Z] = FLOAT_MAX;
    b->max[X] = b->max[Y] = b->max[Z] = -FLOAT_MAX;
}

void fold_min_max_pt_2d(BOX_2D * b, POINT_2D p)
{
    fold_min_pt_2d(b->min, p);
    fold_max_pt_2d(b->max, p);
}

void fold_min_max_pt_3d(BOX_3D * b, POINT_3D p)
{
    fold_min_pt_3d(b->min, p);
    fold_max_pt_3d(b->max, p);
}

void fold_min_max_polygon_2d(BOX_2D * b, POLYGON_2D * polygon)
{
    int i;

    for (i = 0; i < polygon->n_sides; i++)
	fold_min_max_pt_2d(b, polygon->v[i]);
}

void fold_min_max_polygon_3d(BOX_3D * b, POLYGON_3D * polygon)
{
    int i;

    for (i = 0; i < polygon->n_sides; i++)
	fold_min_max_pt_3d(b, polygon->v[i]);
}

void fold_min_max_polyline_2d(BOX_2D * b, POLYLINE_2D * polyline)
{
    int i;

    for (i = 0; i < polyline->n_vertices; i++)
	fold_min_max_pt_2d(b, polyline->v[i]);
}

void fold_min_max_polyline_3d(BOX_3D * b, POLYLINE_3D * polyline)
{
    int i;

    for (i = 0; i < polyline->n_vertices; i++)
	fold_min_max_pt_3d(b, polyline->v[i]);
}

void copy_box_2d(BOX_2D * r, BOX_2D * s)
{
    *r = *s;
}

void copy_box_3d(BOX_3D * r, BOX_3D * s)
{
    *r = *s;
}

int boxes_2d_intersect_p(BOX_2D * a, BOX_2D * b)
{
    if (a->max[X] < b->min[X])	// a left of b
	return 0;
    if (a->min[X] > b->max[X])	// a right of b
	return 0;
    if (a->max[Y] < b->min[Y])	// a below  b
	return 0;
    if (a->min[Y] > b->max[Y])	// a above b
	return 0;
    return 1;
}

int boxes_3d_intersect_p(BOX_2D * a, BOX_2D * b)
{
    if (a->max[X] < b->min[X])	// a left of b
	return 0;
    if (a->min[X] > b->max[X])	// a right of b
	return 0;
    if (a->max[Y] < b->min[Y])	// a below  b
	return 0;
    if (a->min[Y] > b->max[Y])	// a above b
	return 0;
    if (a->max[Z] < b->min[Z])	// a behind  b
	return 0;
    if (a->min[Z] > b->max[Z])	// a in front of b
	return 0;
    return 1;
}

void copy_transform(TRANSFORM r, TRANSFORM s)
{
    int i;

    for (i = 0; i < 16; i++)
	r[i] = s[i];
}

#define R(I,J) r[IT(I,J)]

void set_ident(TRANSFORM r)
{
    R(1, 1) = 1;		// hard code for speed
    R(2, 1) = 0;
    R(3, 1) = 0;
    R(4, 1) = 0;

    R(1, 2) = 0;
    R(2, 2) = 1;
    R(3, 2) = 0;
    R(4, 2) = 0;

    R(1, 3) = 0;
    R(2, 3) = 0;
    R(3, 3) = 1;
    R(4, 3) = 0;

    R(1, 4) = 0;
    R(2, 4) = 0;
    R(3, 4) = 0;
    R(4, 4) = 1;
}

void set_scale(TRANSFORM r, FLOAT sx, FLOAT sy, FLOAT sz)
{
    set_ident(r);
    R(1, 1) = sx;
    R(2, 2) = sy;
    R(3, 3) = sz;
}

void set_translation(TRANSFORM r, FLOAT dx, FLOAT dy, FLOAT dz)
{
    set_ident(r);
    R(1, 4) = dx;
    R(2, 4) = dy;
    R(3, 4) = dz;
}

#define SQR(A) ((A) * (A))

void set_angle_axis_rot(TRANSFORM r, FLOAT theta, VECTOR_3D u)
{
    FLOAT c = cos(theta);
    FLOAT s = sin(theta);
    FLOAT d = 1 - c;

    R(1, 1) = d * (SQR(u[X]) - 1) + 1;
    R(1, 2) = d * u[X] * u[Y] - u[Z] * s;
    R(1, 3) = d * u[X] * u[Z] + u[Y] * s;

    R(2, 1) = d * u[X] * u[Y] + u[Z] * s;
    R(2, 2) = d * (SQR(u[Y]) - 1) + 1;
    R(2, 3) = d * u[Y] * u[Z] - u[X] * s;

    R(3, 1) = d * u[X] * u[Z] - u[Y] * s;
    R(3, 2) = d * u[Y] * u[Z] + u[X] * s;
    R(3, 3) = d * (SQR(u[Z]) - 1) + 1;

    R(1, 4) = R(4, 1) = R(2, 4) = R(4, 2) = R(3, 4) = R(4, 3) = 0;
    R(4, 4) = 1;
}

void
set_angle_axis_rot_about_point(TRANSFORM r, FLOAT theta, POINT_3D p,
			       VECTOR_3D u)
{
    VECTOR_3D u_unit;
    TRANSFORM tmp;

    if (u) {
	find_unit_vec_3d(u_unit, u);
    } else {
	u_unit[X] = u_unit[Y] = 0;
	u_unit[Z] = 1;
    }
    set_angle_axis_rot(r, theta, u_unit);
    if (p) {
	set_translation(tmp, -p[X], -p[Y], -p[Z]);
	compose(r, r, tmp);
	set_translation(tmp, p[X], p[Y], p[Z]);
	compose(r, tmp, r);
    }
}

void set_perspective_projection(TRANSFORM r, FLOAT p)
{
    set_scale(r, p, p, p);
    R(4, 4) = 0;
    R(4, 3) = -1;
}

void set_perspective_transform(TRANSFORM r, FLOAT p)
{
    set_scale(r, p, p, 1);
    R(3, 4) = 1;
    R(4, 3) = -1;
    R(4, 4) = 0;
}

void set_parallel_projection(TRANSFORM r)
{
    set_scale(r, 1, 1, 0);
}

void
set_view_transform(TRANSFORM r, POINT_3D eye, VECTOR_3D vd, VECTOR_3D up)
{
    static VECTOR_3D default_up = { 0, 1, 0 };

    VECTOR_3D unit_vd, unit_up, h, v;
    TRANSFORM tmp;

    if (vd) {
	find_unit_vec_3d(unit_vd, vd);
    } else {
	negate_vec_3d(unit_vd, eye);	// assumes point and vector are compatible
	find_unit_vec_3d(unit_vd, unit_vd);
    }

    if (up)
	find_unit_vec_3d(unit_up, up);
    else
	copy_vec_3d(unit_up, default_up);

    cross(h, unit_vd, unit_up);
    cross(v, h, unit_vd);

    R(1, 1) = h[X];
    R(1, 2) = h[Y];
    R(1, 3) = h[Z];
    R(1, 4) = 0;
    R(2, 1) = v[X];
    R(2, 2) = v[Y];
    R(2, 3) = v[Z];
    R(2, 4) = 0;
    R(3, 1) = -unit_vd[X];
    R(3, 2) = -unit_vd[Y];
    R(3, 3) = -unit_vd[Z];
    R(3, 4) = 0;
    R(4, 1) = 0;
    R(4, 2) = 0;
    R(4, 3) = 0;
    R(4, 4) = 1;

    if (eye) {
	set_translation(tmp, -eye[X], -eye[Y], -eye[Z]);
	compose(r, r, tmp);
    }
}

void
set_view_transform_with_look_at(TRANSFORM r, POINT_3D eye,
				POINT_3D look_at, VECTOR_3D up)
{
    VECTOR_3D vd;
    sub_vecs_3d(vd, look_at, eye);
    set_view_transform(r, eye, vd, up);
}

#define M(I,J) m[IT(I,J)]

// invert a transform using the method of cofactors
// this code was generated by the Perl program geninv.pl
void invert(TRANSFORM r, FLOAT * det_rtn, TRANSFORM m, FLOAT min_det)
{
    int i;
    FLOAT det;
    FLOAT t001, t002, t003, t004, t005, t006, t007, t008,
	t009, t010, t011, t012, t013, t014, t015, t016,
	t017, t018, t019, t020, t021, t022, t023, t024,
	t025, t026, t027, t028, t029, t030, t031, t032,
	t033, t034, t035, t036, t037, t038, t039, t040,
	t057, t058, t061, t062, t065, t066, t072, t073,
	t076, t077, t085, t086, t097, t098, t101, t102,
	t105, t106, t112, t113, t116, t117, t125, t126;
    t001 = M(3, 3) * M(4, 4);
    t002 = M(3, 4) * M(4, 3);
    t003 = t001 - t002;
    t004 = M(2, 2) * t003;
    t005 = M(3, 2) * M(4, 4);
    t006 = M(3, 4) * M(4, 2);
    t007 = t006 - t005;
    t008 = M(2, 3) * t007;
    t009 = M(3, 2) * M(4, 3);
    t010 = M(3, 3) * M(4, 2);
    t011 = t009 - t010;
    t012 = M(2, 4) * t011;
    t013 = t004 + t008 + t012;
    R(1, 1) = t013;
    t014 = t002 - t001;
    t015 = M(2, 1) * t014;
    t016 = M(3, 1) * M(4, 4);
    t017 = M(3, 4) * M(4, 1);
    t018 = t016 - t017;
    t019 = M(2, 3) * t018;
    t020 = M(3, 1) * M(4, 3);
    t021 = M(3, 3) * M(4, 1);
    t022 = t021 - t020;
    t023 = M(2, 4) * t022;
    t024 = t015 + t019 + t023;
    R(2, 1) = t024;
    t025 = t005 - t006;
    t026 = M(2, 1) * t025;
    t027 = t017 - t016;
    t028 = M(2, 2) * t027;
    t029 = M(3, 1) * M(4, 2);
    t030 = M(3, 2) * M(4, 1);
    t031 = t029 - t030;
    t032 = M(2, 4) * t031;
    t033 = t026 + t028 + t032;
    R(3, 1) = t033;
    t034 = t010 - t009;
    t035 = M(2, 1) * t034;
    t036 = t020 - t021;
    t037 = M(2, 2) * t036;
    t038 = t030 - t029;
    t039 = M(2, 3) * t038;
    t040 = t035 + t037 + t039;
    R(4, 1) = t040;
    det =
	(M(1, 1) * t013) + (M(1, 2) * t024) + (M(1, 3) * t033) +
	(M(1, 4) * t040);
    R(1, 2) = (M(1, 2) * t014) + (M(1, 3) * t025) + (M(1, 4) * t034);
    R(2, 2) = (M(1, 1) * t003) + (M(1, 3) * t027) + (M(1, 4) * t036);
    R(3, 2) = (M(1, 1) * t007) + (M(1, 2) * t018) + (M(1, 4) * t038);
    R(4, 2) = (M(1, 1) * t011) + (M(1, 2) * t022) + (M(1, 3) * t031);
    t057 = M(2, 3) * M(4, 4);
    t058 = M(2, 4) * M(4, 3);
    t061 = M(2, 2) * M(4, 4);
    t062 = M(2, 4) * M(4, 2);
    t065 = M(2, 2) * M(4, 3);
    t066 = M(2, 3) * M(4, 2);
    R(1, 3) =
	((t057 - t058) * M(1, 2)) + ((t062 - t061) * M(1,
						       3)) + ((t065 -
							       t066) * M(1,
									 4));
    t072 = M(2, 1) * M(4, 4);
    t073 = M(2, 4) * M(4, 1);
    t076 = M(2, 1) * M(4, 3);
    t077 = M(2, 3) * M(4, 1);
    R(2, 3) =
	((t058 - t057) * M(1, 1)) + ((t072 - t073) * M(1,
						       3)) + ((t077 -
							       t076) * M(1,
									 4));
    t085 = M(2, 1) * M(4, 2);
    t086 = M(2, 2) * M(4, 1);
    R(3, 3) =
	((t061 - t062) * M(1, 1)) + ((t073 - t072) * M(1,
						       2)) + ((t085 -
							       t086) * M(1,
									 4));
    R(4, 3) =
	((t066 - t065) * M(1, 1)) + ((t076 - t077) * M(1,
						       2)) + ((t086 -
							       t085) * M(1,
									 3));
    t097 = M(2, 3) * M(3, 4);
    t098 = M(2, 4) * M(3, 3);
    t101 = M(2, 2) * M(3, 4);
    t102 = M(2, 4) * M(3, 2);
    t105 = M(2, 2) * M(3, 3);
    t106 = M(2, 3) * M(3, 2);
    R(1, 4) =
	((t098 - t097) * M(1, 2)) + ((t101 - t102) * M(1,
						       3)) + ((t106 -
							       t105) * M(1,
									 4));
    t112 = M(2, 1) * M(3, 4);
    t113 = M(2, 4) * M(3, 1);
    t116 = M(2, 1) * M(3, 3);
    t117 = M(2, 3) * M(3, 1);
    R(2, 4) =
	((t097 - t098) * M(1, 1)) + ((t113 - t112) * M(1,
						       3)) + ((t116 -
							       t117) * M(1,
									 4));
    t125 = M(2, 1) * M(3, 2);
    t126 = M(2, 2) * M(3, 1);
    R(3, 4) =
	((t102 - t101) * M(1, 1)) + ((t112 - t113) * M(1,
						       2)) + ((t126 -
							       t125) * M(1,
									 4));
    R(4, 4) =
	((t105 - t106) * M(1, 1)) + ((t117 - t116) * M(1,
						       2)) + ((t125 -
							       t126) * M(1,
									 3));
    if (-min_det <= det && det <= min_det) {
	*det_rtn = 0;
    } else {
	*det_rtn = det;
	for (i = 0; i < 16; i++)
	    r[i] *= 1 / det;
    }
}

#define A(I,J) a[IT(I,J)]
#define B(I,J) b[IT(I,J)]
void compose_unsafe(TRANSFORM r, TRANSFORM a, TRANSFORM b)
{
    int i, j;
    FLOAT *p = r;

    for (j = 1; j <= 4; j++)
	for (i = 1; i <= 4; i++)
	    *p++ =
		A(i, 1) * B(1, j) + A(i, 2) * B(2, j) + A(i, 3) * B(3,
								    j) +
		A(i, 4) * B(4, j);
}

void compose(TRANSFORM r, TRANSFORM a, TRANSFORM b)
{
    TRANSFORM t;
    compose_unsafe(t, a, b);
    copy_transform(r, t);
}

void transform_pt_3d(POINT_3D r, TRANSFORM m, POINT_3D p)
{
    POINT_3D t;
    FLOAT wi;

    wi = 1 / (M(4, 1) * p[X] + M(4, 2) * p[Y] + M(4, 3) * p[Z] + M(4, 4));
    t[X] =
	(M(1, 1) * p[X] + M(1, 2) * p[Y] + M(1, 3) * p[Z] + M(1, 4)) * wi;
    t[Y] =
	(M(2, 1) * p[X] + M(2, 2) * p[Y] + M(2, 3) * p[Z] + M(2, 4)) * wi;
    t[Z] =
	(M(3, 1) * p[X] + M(3, 2) * p[Y] + M(3, 3) * p[Z] + M(3, 4)) * wi;
    copy_pt_3d(r, t);
}

void transform_vec_3d(VECTOR_3D r, TRANSFORM m, VECTOR_3D v)
{
    VECTOR_3D t;

    t[X] = M(1, 1) * v[X] + M(1, 2) * v[Y] + M(1, 3) * v[Z];
    t[Y] = M(2, 1) * v[X] + M(2, 2) * v[Y] + M(2, 3) * v[Z];
    t[Z] = M(3, 1) * v[X] + M(3, 2) * v[Y] + M(3, 3) * v[Z];
    copy_vec_3d(r, t);
}

void set_ident_quat(QUATERNION q)
{
    q[W] = 1;
    q[X] = q[Y] = q[Z] = 0;
}

void set_angle_axis_quat(QUATERNION q, FLOAT theta, VECTOR_3D axis)
{
    VECTOR_3D v;

    find_unit_vec_3d(v, axis);
    scale_vec_3d(&q[X], v, sin(theta));
    q[W] = cos(theta);
}

void mult_quat(QUATERNION r, QUATERNION a, QUATERNION b)
{
    r[W] = a[W] * b[W] - a[X] * b[X] - a[Y] * b[Y] - a[Z] * b[Z];
    r[X] = a[W] * b[X] + a[X] * b[W] + a[Y] * b[Z] - a[Z] * b[Y];
    r[Y] = a[W] * b[Y] - a[X] * b[Z] + a[Y] * b[W] + a[Z] * b[X];
    r[Z] = a[W] * b[Z] + a[X] * b[Y] - a[Y] * b[X] + a[Z] * b[W];
}

#define R(I,J) r[IT(I,J)]
#define SQR(A) ((A) * (A))

void find_rot_from_quat(TRANSFORM r, QUATERNION q)
{
    FLOAT len2 = SQR(q[W]) + SQR(q[X]) + SQR(q[Y]) + SQR(q[Z]);
    FLOAT s = len2 > 0 ? 2 / len2 : 0;

    R(1, 1) = 1 - s * (SQR(q[Y]) + SQR(q[Z]));
    R(1, 2) = s * (q[X] * q[Y] - q[W] * q[Z]);
    R(1, 3) = s * (q[X] * q[Z] + q[W] * q[Y]);

    R(2, 1) = s * (q[X] * q[Y] + q[W] * q[Z]);
    R(2, 2) = 1 - s * (SQR(q[X]) + SQR(q[Z]));
    R(2, 3) = s * (q[Y] * q[Z] - q[W] * q[X]);

    R(3, 1) = s * (q[X] * q[Z] - q[W] * q[Y]);
    R(3, 2) = s * (q[Y] * q[Z] + q[W] * q[X]);
    R(3, 3) = 1 - s * (SQR(q[X]) + SQR(q[Y]));

    R(1, 4) = R(4, 1) = R(2, 4) = R(4, 2) = R(3, 4) = R(4, 3) = 0;
    R(4, 4) = 1;
}

void find_quat_from_rot(QUATERNION q, TRANSFORM r)
{
    if (R(1, 1) + R(2, 2) + R(3, 3) >= 0) {	// w first
	FLOAT w2 = sqrt(R(1, 1) + R(2, 2) + R(3, 3) + 1);
	q[W] = 0.5 * w2;	// 1st
	q[X] = (0.5 / w2) * (R(3, 2) - R(2, 3));	// (f)
	q[Y] = (0.5 / w2) * (R(1, 3) - R(3, 1));	// (d)
	q[Z] = (0.5 / w2) * (R(2, 1) - R(1, 2));	// (b)
	return;
    }
    // x, y, or z first
    if (R(1, 1) > R(2, 2))
	if (R(1, 1) > R(3, 3))
	    goto x_first;
	else
	    goto z_first;
    else			// R(2,2) >= R(1,1)
    if (R(2, 2) > R(3, 3))
	goto y_first;
    else
	goto z_first;

  x_first:{
	FLOAT x2 = sqrt(R(1, 1) - R(2, 2) - R(3, 3) + 1);
	q[W] = (0.5 / x2) * (R(3, 2) - R(2, 3));	// (f)
	q[X] = 0.5 * x2;	// 1st
	q[Y] = (0.5 / x2) * (R(2, 1) + R(1, 2));	// (a)
	q[Z] = (0.5 / x2) * (R(1, 3) + R(3, 1));	// (c)
	return;
    }

  y_first:{
	FLOAT y2 = sqrt(-R(1, 1) + R(2, 2) - R(3, 3) + 1);
	q[W] = (0.5 / y2) * (R(1, 3) - R(3, 1));	// (d)
	q[X] = (0.5 / y2) * (R(2, 1) + R(1, 2));	// (a)
	q[Y] = 0.5 * y2;	// 1st
	q[Z] = (0.5 / y2) * (R(3, 2) + R(2, 3));	// (e)
	return;
    }

  z_first:{
	FLOAT z2 = sqrt(-R(1, 1) - R(2, 2) + R(3, 3) + 1);
	q[W] = (0.5 / z2) * (R(2, 1) - R(1, 2));	// (b)
	q[X] = (0.5 / z2) * (R(1, 3) + R(3, 1));	// (c)
	q[Y] = (0.5 / z2) * (R(3, 2) + R(2, 3));	// (e)
	q[Z] = 0.5 * z2;	// 1st
	return;
    }
}

#undef R

void
make_cso_polygon_2d(POLYGON_2D * r, POLYGON_2D * a, POINT_2D p,
		    POLYGON_2D * b)
{
    int j, ia, ja, ib, jb, ir, nb;
    FLOAT x, y, dx_a, dy_a, dx_b, dy_b;

    setup_polygon_2d(r, a->n_sides + b->n_sides);
    r->n_sides = a->n_sides + b->n_sides;

    ja = 0;
    x = a->v[ja][X];
    for (j = 1; j < a->n_sides; j++)
	if (a->v[j][X] < x) {
	    x = a->v[j][X];
	    ja = j;
	}

    jb = 0;
    x = b->v[0][X];
    for (j = 1; j < b->n_sides; j++)
	if (b->v[j][X] > x) {
	    x = b->v[j][X];
	    jb = j;
	}
    // this point is certain to be an extreme point of the cso
    x = b->v[jb][X] + (p[X] - a->v[ja][X]);
    y = b->v[jb][Y] + (p[Y] - a->v[ja][Y]);

    ia = (ja + 1) % a->n_sides;
    dx_a = a->v[ja][X] - a->v[ia][X];
    dy_a = a->v[ja][Y] - a->v[ia][Y];
    ib = (jb + 1) % b->n_sides;
    dx_b = b->v[ib][X] - b->v[jb][X];
    dy_b = b->v[ib][Y] - b->v[jb][Y];
    nb = b->n_sides;
    ir = 0;
    for (;;) {

	// record obstacle polygon point and quit if done
	r->v[ir][X] = x;
	r->v[ir][Y] = y;
	if (++ir == r->n_sides)
	    break;

	// merge next edge of lowest theta. */
	if (nb == 0 || dx_a * dy_b - dy_a * dx_b > 0) {
	    x += dx_a;
	    y += dy_a;
	    ja = ia;
	    ia = (ja + 1) % a->n_sides;
	    dx_a = a->v[ja][X] - a->v[ia][X];
	    dy_a = a->v[ja][Y] - a->v[ia][Y];
	} else {
	    x += dx_b;
	    y += dy_b;
	    jb = ib;
	    ib = (jb + 1) % b->n_sides;
	    dx_b = b->v[ib][X] - b->v[jb][X];
	    dy_b = b->v[ib][Y] - b->v[jb][Y];
	    nb--;
	}
    }
}

int point_near_convex_polygon_2d_p(POINT_2D p, POLYGON_2D * a, FLOAT eps)
{
    int i, j;
    VECTOR_2D vji_perp, vjp;

    // if the point is more than eps right of any edge, we're outside
    for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++) {
	vji_perp[X] = a->v[j][Y] - a->v[i][Y];
	vji_perp[Y] = a->v[i][X] - a->v[j][X];
	find_unit_vec_2d(vji_perp, vji_perp);
	sub_pts_2d(vjp, p, a->v[j]);
	if (dot_2d(vjp, vji_perp) <= eps)
	    return 0;
    }
    // else we're inside!
    return 1;
}

int point_inside_convex_polygon_2d_p(POINT_2D p, POLYGON_2D * a)
{
    int i, j;

    // if the point is right of any edge, we're outside
    for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++)
	if ((p[X] - a->v[j][X]) * (a->v[i][Y] - a->v[j][Y]) -
	    (p[Y] - a->v[j][Y]) * (a->v[i][X] - a->v[j][X]) >= 0)
	    return 0;

    // else we're inside!
    return 1;
}

// The Franklin code...
int point_inside_polygon_2d_p(POINT_2D p, POLYGON_2D * a)
{
    int i, j, r = 0;
    for (i = 0, j = a->n_sides - 1; i < a->n_sides; j = i++) {
	if (((a->v[i][Y] <= p[Y] && p[Y] < a->v[j][Y]) ||
	     (a->v[j][Y] <= p[Y] && p[Y] < a->v[i][Y])) &&
	    (p[X] < (a->v[j][X] - a->v[i][X]) * (p[Y] - a->v[i][Y]) /
	     (a->v[j][Y] - a->v[i][Y]) + a->v[i][X]))
	    r ^= 1;
    }
    return r;
}

#ifdef TEST_INVERT

void print_transform(TRANSFORM m)
{
    int i, j;
    printf("[\n");
    for (i = 1; i <= 4; i++) {
	printf("[");
	for (j = 1; j <= 4; j++) {
	    printf(" %8.3g", m[IT(i, j)]);
	}
	printf("]\n");
    }
    printf("]\n");
}

int main(void)
{
    TRANSFORM m =
	{ 1, 0, 1, 1, 2, 4, 0, 19, 3, 5, 6, 57, 14, -3, 34, 1 }, r;
    FLOAT det;
    VECTOR_3D axis = { 1, 2, 3 };
    POINT_3D pt = { -10, 2, 41 };

    // set_angle_axis_rot_about_point(m, 30, pt, axis);
    print_transform(m);
    invert(r, &det, m, 1e-4);
    printf("det=%.3g\n", det);
    print_transform(r);
    invert(m, &det, r, 1e-4);
    printf("det=%.3g\n", det);
    print_transform(m);
}

#endif

#ifdef TEST_DYNARRAY_H

// we need a dynamic arrao of these things
typedef struct foo_t {
    char *name;
    int count;
} FOO;

typedef struct foo_array_t {
    DYNAMIC_ARRAY_FIELDS(FOO, val, n_vals);
} FOO_ARRAY;

// do the prototypes for the constructor, destructor, and accessor functions
DECLARE_DYNAMIC_ARRAY_PROTOS(FOO_ARRAY, FOO, foo_list, val, n_vals)
// ---- in foo.c ----
// create the bodies for the constructor, destructor, and accessor functions
    DECLARE_DYNAMIC_ARRAY_FUNCS(FOO_ARRAY, FOO, foo_list, val, n_vals)
// use all the new stuff!
void do_stuff_with_foos(void)
{
    int i;
    char buf[100];
    FOO_ARRAY list[1];		// or FOO_ARRAY list; but then we're forever &'ing
    FOO_ARRAY copy[1];

    init_foo_list(list);	// do this JUST ONCE right after declaration
    init_foo_list(copy);	// (not necessary for static/global decls)

    setup_foo_list(list, 10);	// allow for 10 elements

    // read some data and push it on the list tail
    while (scanf("%d %s", &i, buf) == 2) {
	// get pointer to new (empty) element at the end of array
	FOO *p = pushed_foo_list_val(list);
	// fill in field values
	p->name = strdup(buf);
	p->count = i;
    }

    // shows unsafe access to elements
    printf("forward listing:\n");
    for (i = 0; i < list->n_vals; i++)
	printf("name=%s count=%d (%d)\n", list->val[i].name,	// fast unsafe access
	       foo_list_val_ptr(list, i)->count,	// slower safe pointer access
	       foo_list_val(list, i).count);	// copying access

    copy_foo_list_filled(copy, list);	// copies only filled elements

    // print in reverse order by popping from tail
    printf("backward listing:\n");
    while (copy->n_vals > 0) {
	FOO *p = popped_foo_list_val(copy);
	printf("name=%s count=%d\n", p->name, p->count);
    }

    // clear out all the allocated storage for the ilst
    clear_foo_list(list);
    clear_foo_list(copy);
}

#endif