cae-tools/poly2d/v2d_intersect.c

/*
 * v2d_intersect.c - Intersect two lines
 *
 * Written 2012 by Werner Almesberger
 * Copyright 2012 Werner Almesberger
 *
 * 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.
 */


#include <math.h>

#include "poly2d.h"


#define	EPSILON	1e-6


/*
 * Solve
 *
 * ax+by = e
 * cx+dy = f
 *
 * with Cramer's rule:
 * http://en.wikipedia.org/wiki/Cramer's_rule
 */

static int cramer2(double a, double b, double c, double d, double e, double f,
    double *x, double *y)
{
	double det;

	det = a*d-b*c;
	if (fabs(det) < EPSILON)
		return 0;
	*x = (e*d-b*f)/det;
	*y = (a*f-e*c)/det;
	return 1;
}


int v2d_intersect(const struct v2d *a0, const struct v2d *a1,
    const struct v2d *b0, const struct v2d *b1,
    double *na, double *nb)
{
	double ax, ay, bx, by, dx, dy;
	double a, b;

	ax = a1->x-a0->x;
	ay = a1->y-a0->y;

	bx = b1->x-b0->x;
	by = b1->y-b0->y;

	dx = b0->x-a0->x;
	dy = b0->y-a0->y;

	if (!cramer2(ax, -bx, ay, -by, dx, dy, &a, &b))
		return 0;
	if (na)
		*na = a;
	if (nb)
		*nb = b;
	return a >= 0 && a <= 1 && b >= 0 && b <= 1 ? 1 : -1;
}