1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
// Copyright Matt Overby 2020.
// Distributed under the MIT License.
#include "admmpd_math.h"
namespace admmpd {
using namespace Eigen;
namespace barycoords {
Matrix<double,4,1> point_tet(
const Vector3d &p,
const Vector3d &a,
const Vector3d &b,
const Vector3d &c,
const Vector3d &d)
{
auto scalar_triple_product = [](
const Vector3d &u,
const Vector3d &v,
const Vector3d &w )
{
return u.dot(v.cross(w));
};
Vector3d ap = p - a;
Vector3d bp = p - b;
Vector3d ab = b - a;
Vector3d ac = c - a;
Vector3d ad = d - a;
Vector3d bc = c - b;
Vector3d bd = d - b;
double va6 = scalar_triple_product(bp, bd, bc);
double vb6 = scalar_triple_product(ap, ac, ad);
double vc6 = scalar_triple_product(ap, ad, ab);
double vd6 = scalar_triple_product(ap, ab, ac);
double v6 = 1.0 / scalar_triple_product(ab, ac, ad);
return Matrix<double,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
} // end point tet barycoords
// From Real-Time Collision Detection by Christer Ericson
Vector3d point_triangle(
const Vector3d &p,
const Vector3d &a,
const Vector3d &b,
const Vector3d &c)
{
Vector3d v0 = b - a;
Vector3d v1 = c - a;
Vector3d v2 = p - a;
double d00 = v0.dot(v0);
double d01 = v0.dot(v1);
double d11 = v1.dot(v1);
double d20 = v2.dot(v0);
double d21 = v2.dot(v1);
double denom = d00 * d11 - d01 * d01;
if (std::abs(denom)<=0)
return Vector3d::Zero();
Vector3d r;
r[1] = (d11 * d20 - d01 * d21) / denom;
r[2] = (d00 * d21 - d01 * d20) / denom;
r[0] = 1.0 - r[1] - r[2];
return r;
} // end point triangle barycoords
} // namespace barycoords
// Checks that it's on the "correct" side of the normal
// for each face of the tet. Assumes winding points inward.
bool point_in_tet(
const Vector3d &p,
const Vector3d &a,
const Vector3d &b,
const Vector3d &c,
const Vector3d &d)
{
using namespace Eigen;
auto check_face = [](
const Vector3d &point,
const Vector3d &p0,
const Vector3d &p1,
const Vector3d &p2,
const Vector3d &p3 )
{
Vector3d n = (p1-p0).cross(p2-p0);
double dp3 = n.dot(p3-p0);
double dp = n.dot(point-p0);
return (dp3*dp >= 0);
};
return
check_face(p, a, b, c, d) &&
check_face(p, b, c, d, a) &&
check_face(p, c, d, a, b) &&
check_face(p, d, a, b, c);
}
} // namespace admmpd
|
