70 lines
1.2 KiB
C
70 lines
1.2 KiB
C
/*
|
|
* v2d_intersect.c - Intersect two lines
|
|
*
|
|
* Written 2012 by Werner Almesberger
|
|
* Copyright 2012 Werner Almesberger
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 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;
|
|
}
|