272 lines
6.2 KiB
C++
272 lines
6.2 KiB
C++
/*
|
|
1 ointersect.cpp: 3D trapezium intersection test.
|
|
|
|
Based on a function from Tomas M[oe]ller:
|
|
Tomas M[oe]ller. 1997. A fast triangle-triangle intersection test. J. Graph.
|
|
Tools 2, 2 (November 1997), 25-30. DOI=10.1080/10867651.1997.10487472
|
|
http://dx.doi.org/10.1080/10867651.1997.10487472
|
|
|
|
*/
|
|
|
|
#include "interpolate.h"
|
|
|
|
|
|
#include <math.h>
|
|
|
|
#define EPSILON 0.001
|
|
|
|
/*
|
|
1.1 ~Vec~
|
|
Helper class for 3d vector operations.
|
|
|
|
*/
|
|
class Vec {
|
|
public:
|
|
double x, y, z;
|
|
|
|
Vec() : x(0), y(0), z(0) {
|
|
}
|
|
|
|
Vec(double x, double y, double z) : x(x), y(y), z(z) {
|
|
}
|
|
|
|
Vec operator+(const Vec &v) const {
|
|
return Vec(x + v.x, y + v.y, z + v.z);
|
|
}
|
|
|
|
Vec operator-(const Vec &v) const {
|
|
return Vec(x - v.x, y - v.y, z - v.z);
|
|
}
|
|
|
|
double operator*(const Vec &v) const {
|
|
return x * v.x + y * v.y + z * v.z;
|
|
}
|
|
|
|
Vec operator&(const Vec &v) const {
|
|
return Vec(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
|
|
}
|
|
double operator[](int index) const {
|
|
return (index == 0) ? x : ((index == 1) ? y : z);
|
|
}
|
|
|
|
Vec abs() const {
|
|
return Vec(fabs(x), fabs(y), fabs(z));
|
|
}
|
|
};
|
|
|
|
static inline void sort(double v[]) {
|
|
if (v[0] > v[1]) {
|
|
double tmp = v[0];
|
|
v[0] = v[1];
|
|
v[1] = tmp;
|
|
}
|
|
}
|
|
|
|
static inline bool compute_intervals(double vv0, double vv1, double vv2,
|
|
double d0, double d1, double d2, double d0d1, double d0d2,
|
|
double &a, double &b, double &c, double &x0, double &x1) {
|
|
if (d0d1 > 0) {
|
|
a = vv2;
|
|
b = (vv0 - vv2) * d2;
|
|
c = (vv1 - vv2) * d2;
|
|
x0 = d2 - d0;
|
|
x1 = d2 - d1;
|
|
} else if (d0d2 > 0) {
|
|
a = vv1;
|
|
b = (vv0 - vv1) * d1;
|
|
c = (vv2 - vv1) * d1;
|
|
x0 = d1 - d0;
|
|
x1 = d1 - d2;
|
|
} else if (d1 * d2 > 0 || d0) {
|
|
a = vv0;
|
|
b = (vv1 - vv0) * d0;
|
|
c = (vv2 - vv0) * d0;
|
|
x0 = d0 - d1;
|
|
x1 = d0 - d2;
|
|
} else if (d1) {
|
|
a = vv1;
|
|
b = (vv0 - vv1) * d1;
|
|
c = (vv2 - vv1) * d1;
|
|
x0 = d1 - d0;
|
|
x1 = d1 - d2;
|
|
} else if (d2) {
|
|
a = vv2;
|
|
b = (vv0 - vv2) * d2;
|
|
c = (vv1 - vv2) * d2;
|
|
x0 = d2 - d0;
|
|
x1 = d2 - d1;
|
|
} else {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool TriangleIntersection(Vec v0, Vec v1, Vec v2,
|
|
Vec u0, Vec u1, Vec u2) {
|
|
Vec e1, e2;
|
|
Vec n1, n2;
|
|
Vec dd;
|
|
double d1, d2;
|
|
double du0, du1, du2, dv0, dv1, dv2;
|
|
double isect1[2], isect2[2];
|
|
double du0du1, du0du2, dv0dv1, dv0dv2;
|
|
double vp0, vp1, vp2;
|
|
double up0, up1, up2;
|
|
double bb, cc, max;
|
|
|
|
e1 = v1 - v0;
|
|
e2 = v2 - v0;
|
|
n1 = e1 & e2;
|
|
d1 = -(n1 * v0);
|
|
|
|
du0 = n1 * u0 + d1;
|
|
du1 = n1 * u1 + d1;
|
|
du2 = n1 * u2 + d1;
|
|
|
|
#ifdef EPSILON
|
|
if (fabs(du0) < EPSILON) du0 = 0.0;
|
|
if (fabs(du1) < EPSILON) du1 = 0.0;
|
|
if (fabs(du2) < EPSILON) du2 = 0.0;
|
|
#endif
|
|
du0du1 = du0 * du1;
|
|
du0du2 = du0 * du2;
|
|
|
|
if (du0du1 >= 0 && du0du2 >= 0)
|
|
return 0;
|
|
|
|
e1 = u1 - u0;
|
|
e2 = u2 - u0;
|
|
n2 = e1 & e2;
|
|
d2 = -(n2 * u0);
|
|
|
|
dv0 = n2 * v0 + d2;
|
|
dv1 = n2 * v1 + d2;
|
|
dv2 = n2 * v2 + d2;
|
|
|
|
#ifdef EPSILON
|
|
if (fabs(dv0) < EPSILON) dv0 = 0.0;
|
|
if (fabs(dv1) < EPSILON) dv1 = 0.0;
|
|
if (fabs(dv2) < EPSILON) dv2 = 0.0;
|
|
#endif
|
|
|
|
dv0dv1 = dv0*dv1;
|
|
dv0dv2 = dv0*dv2;
|
|
|
|
if (dv0dv1 >= 0.0f && dv0dv2 >= 0.0f)
|
|
return 0;
|
|
|
|
dd = n1 & n2;
|
|
|
|
max = fabs(dd.x);
|
|
vp0 = v0.x;
|
|
vp1 = v1.x;
|
|
vp2 = v2.x;
|
|
up0 = u0.x;
|
|
up1 = u1.x;
|
|
up2 = u2.x;
|
|
bb = fabs(dd.y);
|
|
cc = fabs(dd.z);
|
|
if (bb > max) {
|
|
max = bb;
|
|
vp0 = v0.y;
|
|
vp1 = v1.y;
|
|
vp2 = v2.y;
|
|
up0 = u0.y;
|
|
up1 = u1.y;
|
|
up2 = u2.y;
|
|
}
|
|
if (cc > max) {
|
|
max = cc;
|
|
vp0 = v0.z;
|
|
vp1 = v1.z;
|
|
vp2 = v2.z;
|
|
up0 = u0.z;
|
|
up1 = u1.z;
|
|
up2 = u2.z;
|
|
}
|
|
|
|
double a, b, c, d, e, f, x0, y0, x1, y1;
|
|
if (!compute_intervals(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2,
|
|
a, b, c, x0, x1) ||
|
|
!compute_intervals(up0, up1, up2, du0, du1, du2, du0du1, du0du2,
|
|
d, e, f, y0, y1)) {
|
|
return false;
|
|
}
|
|
|
|
double xx, yy, xxyy, tmp;
|
|
xx = x0*x1;
|
|
yy = y0*y1;
|
|
xxyy = xx*yy;
|
|
|
|
tmp = a*xxyy;
|
|
isect1[0] = tmp + b * x1*yy;
|
|
isect1[1] = tmp + c * x0*yy;
|
|
|
|
tmp = d*xxyy;
|
|
isect2[0] = tmp + e * xx*y1;
|
|
isect2[1] = tmp + f * xx*y0;
|
|
|
|
sort(isect1);
|
|
sort(isect2);
|
|
|
|
return (isect1[1] > isect2[0] && isect2[1] > isect1[0]);
|
|
}
|
|
|
|
static bool _trapeziumIntersects(MSeg m, MSeg a) {
|
|
if (!(m.is == m.ie) && !(m.fs == m.fe)) {
|
|
MSeg ms1(m.is, m.ie, m.fs, m.fs);
|
|
MSeg ms2(m.is, m.is, m.fs, m.fe);
|
|
return _trapeziumIntersects(ms1, a) || _trapeziumIntersects(ms2, a);
|
|
}
|
|
|
|
if (!(a.is == a.ie) && !(a.fs == a.fe)) {
|
|
MSeg ms1(a.is, a.ie, a.fs, a.fs);
|
|
MSeg ms2(a.is, a.is, a.fs, a.fe);
|
|
return _trapeziumIntersects(m, ms1) || _trapeziumIntersects(m, ms2);
|
|
}
|
|
|
|
Vec v0, v1, v2;
|
|
Vec u0, u1, u2;
|
|
|
|
if (m.is.x == m.ie.x && m.is.y == m.ie.y) {
|
|
v0 = Vec(m.is.x, m.is.y, 0);
|
|
v1 = Vec(m.fs.x, m.fs.y, 1);
|
|
v2 = Vec(m.fe.x, m.fe.y, 1);
|
|
} else if (m.fs.x == m.fe.x && m.fs.y == m.fe.y) {
|
|
v0 = Vec(m.is.x, m.is.y, 0);
|
|
v1 = Vec(m.ie.x, m.ie.y, 0);
|
|
v2 = Vec(m.fs.x, m.fs.y, 1);
|
|
} else {
|
|
cerr << "ERROR: src-triangle is a trapezium!\n";
|
|
assert(false);
|
|
}
|
|
|
|
if (a.is.x == a.ie.x && a.is.y == a.ie.y) {
|
|
u0 = Vec(a.is.x, a.is.y, 0);
|
|
u1 = Vec(a.fs.x, a.fs.y, 1);
|
|
u2 = Vec(a.fe.x, a.fe.y, 1);
|
|
} else if (a.fs.x == a.fe.x && a.fs.y == a.fe.y) {
|
|
u0 = Vec(a.is.x, a.is.y, 0);
|
|
u1 = Vec(a.ie.x, a.ie.y, 0);
|
|
u2 = Vec(a.fs.x, a.fs.y, 1);
|
|
} else {
|
|
cerr << "ERROR: dst-triangle is a trapezium!\n";
|
|
assert(false);
|
|
}
|
|
|
|
bool ret = TriangleIntersection(v0, v1, v2, u0, u1, u2);
|
|
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
1.2 ~trapeziumIntersects~ checks, if the moving segments ~m~ and ~a~,
|
|
which can be taken as 3d trapezoids, intersect.
|
|
|
|
*/
|
|
bool trapeziumIntersects(MSeg m, MSeg a, unsigned int& detailedResult) {
|
|
detailedResult = 0;
|
|
|
|
return _trapeziumIntersects(m, a);
|
|
}
|