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

double
dpp2(Point2 p0, Point2 p1)
{
	p0.x -= p1.x;
	p0.y -= p1.y;

	return sqrt(p0.x*p0.x + p0.y*p0.y);
}

double
dpl2(Point2 p, Point2 q[2])
{
	Point2 l[2];

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

	return sqrt(l[0].x*l[0].x + l[0].y*l[0].y);
}

double
dll2(Point2 v0[2], Point2 v1[2])
{
	Point2 l[2];

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

	return sqrt(l[0].x*l[0].x + l[0].y*l[0].y);
}

void
vpl2(Point2 l[2], Point2 p, Point2 q[2])
{
	Point2 t;
	double s;

	l[0] = p;

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


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

	s = ((p.x-q[0].x)*t.x + (p.y-q[0].y)*t.y)/(t.x*t.x + t.y*t.y);
	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;
	}
}

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

	/* Algorithm due to lumelsky */

	d0 = psub2(v0[1], v0[0]);
	d1 = psub2(v1[1], v1[0]);

	t0 = d0.x*d0.x + d0.y*d0.y;
	t1 = d1.x*d1.x + d1.y*d1.y;

	/* get rid of degenerate cases - should never occur */
	if (t0 == 0) {
		vpl2(l, v0[0], v1);
		return;
	}
	if (t1 == 0) {
		vpl2(l, v1[0], v0);
		d0 = l[0]; l[0] = l[1]; l[1] = d0;
		return;
	}
			
	
	d01 = psub2(v1[0], v0[0]);
	r = pdot2(d0,d1);
	s0 = pdot2(d0,d01);
	s1 = pdot2(d1,d01);

	den = t0*t1 - r*r;
	
	/* call lines parallel */
	if (den < 1e-6*t0*t1)
		den = 0;

	if (den == 0) {
		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 if (t > 0 && t < 1 && den != 0) {
		/* intercection */
		l[0].x = v0[0].x + t*d0.x;
		l[0].y = v0[0].y + t*d0.y;
		l[1] = l[0];
		return;
	} 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[1].x = v1[0].x + u*d1.x;
	l[1].y = v1[0].y + u*d1.y;

	return;
}