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

double
dpp3(Point3 p0, Point3 p1)
{
	double r;

	p0.x -= p1.x;
	p0.y -= p1.y;
	p0.z -= p1.z;

	r = sqrt(pdot3(p0,p0));
	return r;
}

double
dpl3(Point3 p, Point3 q[2])
{
	Point3 l[2];

	vpl3(l, p, q);
	
	l[0].x -= l[1].x;
	l[0].y -= l[1].y;
	l[0].z -= l[1].z;

	return pabs3(l[0]);
}

double
dll3(Point3 v0[2], Point3 v1[2])
{
	Point3 l[2];

	vll3(l, v0, v1);
	
	l[0].x -= l[1].x;
	l[0].y -= l[1].y;
	l[0].z -= l[1].z;

	return sqrt(pdot3(l[0],l[0]));
}


void
vpp3(Point3 l[2], Point3 p0, Point3 p1)
{
	l[0] = p0;
	l[1] = p1;
}


void
vpl3(Point3 l[2], Point3 p, Point3 q[2])
{
	Point3 t;
	double s;

	l[0] = p;

	t.x = q[1].x - q[0].x;
	t.y = q[1].y - q[0].y;
	t.z = q[1].z - q[0].z;


	if (t.x == 0 && t.y == 0 && t.z == 0) {
		l[0] = q[0];
		return;
	}

	p = psub3(p, q[0]);

	s = pdot3(p,t)/pdot3(t,t);

	if (s < 0) {
		l[1] = q[0];
	} else if (s > 1) {
		l[1] = q[1];
	} else {
		l[1].x = q[0].x + s*t.x;
		l[1].y = q[0].y + s*t.y;
		l[1].z = q[0].z + s*t.z;
	}

	return;
}

void
vll3(Point3 l[2], Point3 v0[2], Point3 v1[2])
{
	Point3 d0, d1, d01;
	double r, s0, s1, t0, t1, den, t, u;

	/* Algorithm due to lumelsky */

	d0 = psub3(v0[1], v0[0]);
	d1 = psub3(v1[1], v1[0]);
	d01 = psub3(v1[0], v0[0]);
	t0 = pdot3(d0,d0);
	t1 = pdot3(d1,d1);
	r = pdot3(d0, d1);
	s0 = pdot3(d0, d01);
	s1 = pdot3(d1, d01);

	/* get rid of degenerate cases - should never occur */
	if (t0 == 0) {
		vpl3(l, v0[0], v1);
		return;
	}
	if (t1 == 0) {
		vpl3(l, v1[0], v0);
		d0 = l[0]; l[0] = l[1]; l[1] = d0;
		return;
	}

	den = t0*t1 - r*r;
	if (den < 1e-6*t0*t1) {
		t = 0.5;
	} else {
		t = (s0*t1 - s1*r)/den;
		if (t < 0)
			t = 0;
		else if (t > 1)
			t = 1;
	}
	
	u = (t*r - s1)/t1;
	if (u < 0)
		u = 0;
	else if (u > 1)
		u = 1;
	else
		goto done;

	t = (u*r + s0)/t0;
	if (t < 0)
		t = 0;
	else if (t > 1)
		t = 1;

done:

	l[0].x = v0[0].x + t*d0.x;
	l[0].y = v0[0].y + t*d0.y;
	l[0].z = v0[0].z + t*d0.z;

	l[1].x = v1[0].x + u*d1.x;
	l[1].y = v1[0].y + u*d1.y;
	l[1].z = v1[0].z + u*d1.z;
}

double
dist3(Point3 *p0, int n, Point3 *p1, int m)
{
	Point3 l[2];

	vdist3(l, p0, n, p1, m);
	
	l[1].x -= l[0].x;
	l[1].y -= l[0].y;
	l[1].z -= l[0].z;

	return pabs3(l[1]);
}

void
vdist3(Point3 l[2], Point3 *p0, int n, Point3 *p1, int m)
{
	int i;

	double (*zi)[3], (*zj)[3], zisol[3], zjsol[3];

	zi = malloc(3*(n+m)*sizeof(double));
	zj = zi+n;

	for (i = 0; i < n; i++) {
		zi[i][0] = p0[i].x;
		zi[i][1] = p0[i].y;
		zi[i][2] = p0[i].z;
	}

	for (i = 0; i < m; i++) {
		zj[i][0] = p1[i].x;
		zj[i][1] = p1[i].y;
		zj[i][2] = p1[i].z;
	}

	/* call gilberts C routine */
	gilbert(n, m, zi, zj, zisol, zjsol);

	l[0].x = zisol[0];
	l[0].y = zisol[1];
	l[0].z = zisol[2];

	l[1].x = zjsol[0];
	l[1].y = zjsol[1];
	l[1].z = zjsol[2];

	free(zi);
}