unsigned int halfspace_locator(const std::array<point*, 3> &f, const point *a, const point *b) {
Eigen::Matrix<double, 3, 3> A;
A.col(0) << a->x - b->x,
a->y - b->y,
a->z - b->z;
A.col(1) << f[1]->x - f[0]->x,
f[1]->y - f[0]->y,
f[1]->z - f[0]->z;
A.col(2) << f[2]->x - f[0]->x,
f[2]->y - f[0]->y,
f[2]->z - f[0]->z;
bool invertible = false;
Eigen::Matrix<double, 3, 3> inverse;
A.computeInverseWithCheck(inverse, invertible);
if (!invertible)
return -1;
Eigen::Matrix<double, 3, 1> B;
B << a->x - f[0]->x,
a->y - f[0]->y,
a->z - f[0]->z;
Eigen::Matrix<double, 3, 1> x = inverse * B;
double t(x[0]), u(x[1]), v(x[2]);
assert(t == t && u == u && v == v);
if (std::abs(u) < 1e-10)
u = 0;
if (std::abs(v) < 1e-10)
v = 0;
double sum = u + v;
if (std::abs(1 - sum) < 1e-10)
sum = 1;
unsigned int location = 0;
/*
half-space 21
*/
if (sum > 1) {
location |= 1 << 0;
location |= 1 << 1;
}
else if (sum < 1) {
location |= 1 << 0;
}
/*
half-space 02
*/
if (u > 0) {
location |= 1 << 2;
}
else if (u < 0) {
location |= 1 << 2;
location |= 1 << 3;
}
/*
half-space 10
*/
if (v > 0) {
location |= 1 << 4;
}
else if (v < 0) {
location |= 1 << 4;
location |= 1 << 5;
}
return location;
}
unsigned int fast_triangle_ray_inclusion(const std::array<point*, 3> &f, const point *a, const point *b) {
Eigen::Matrix<double, 3, 3> A;
A.col(0) << a->x - b->x,
a->y - b->y,
a->z - b->z;
A.col(1) << f[1]->x - f[0]->x,
f[1]->y - f[0]->y,
f[1]->z - f[0]->z;
A.col(2) << f[2]->x - f[0]->x,
f[2]->y - f[0]->y,
f[2]->z - f[0]->z;
Eigen::Matrix<double, 3, 1> B;
B << a->x - f[0]->x,
a->y - f[0]->y,
a->z - f[0]->z;
Eigen::Matrix<double, 3, 3> inverse;
bool invertible = false;
A.computeInverseWithCheck(inverse, invertible);
if (!invertible) {
//if (verbose >= 0)
//std::cout << "Uninvertible matrix. Skipping..." << std::endl;
return 0;
}
Eigen::Matrix<double, 3, 1> x = inverse * B;
for (int i = 0; i < 3; ++i)
if (std::abs(x[i]) < 1e-10)
x[i] = 0;
double t(x[0]), u(x[1]), v(x[2]);
if (verbose >= 3) {
std::cout << "\nplane being examined : " << std::endl;
for (int i = 0; i < 3; ++i)
std::cout << *f[i] << std::endl;
std::cout << "\ntwo points : " << std::endl;
std::cout << *a << " <------> " << *b << std::endl;
std::cout << "A : " << std::endl << A << std::endl;
std::cout << "B : " << std::endl << B << std::endl;
std::cout << "x : " << std::endl << x << std::endl;
}
assert(t == t && u == u && v == v); // NaN checks
assert(t != 0);
if (t == 1) {
assert(u < 0 || v < 0 || u + v > 1);
return 0; // outside triangle's interior
}
if (t > 0 && t < 1) {
double sum = 0;
for (int i = 1; i < 3; ++i) {
if (x[i] < 0)
return 0; // outside
sum += x[i];
}
if (std::abs(sum - 1) < 1e-10)
sum = 1;
if (sum > 1)
return 0; // outside
if (u == 1) {
return 2; // vertex 1
}
else if (u > 0) {
if (v > 0) {
if (sum == 1) {
return 6; // edge 21
} else {
return 7; // inside
}
}
else {
return 3; // edge 10
}
}
else {
if (v == 1)
return 4; // vertex 2
else if (v > 0) {
return 5; // edge 02
}else
return 1; // vertex 0
}
}
return 0;
}