diff options
Diffstat (limited to 'extern/softbody/src/admmpd_math.cpp')
-rw-r--r-- | extern/softbody/src/admmpd_math.cpp | 71 |
1 files changed, 49 insertions, 22 deletions
diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp index 7d1a4f82844..88451e60ee1 100644 --- a/extern/softbody/src/admmpd_math.cpp +++ b/extern/softbody/src/admmpd_math.cpp @@ -4,47 +4,74 @@ #include "admmpd_math.h" namespace admmpd { +using namespace Eigen; + namespace barycoords { - Eigen::Matrix<double,4,1> point_tet( - const Eigen::Vector3d &p, - const Eigen::Vector3d &a, - const Eigen::Vector3d &b, - const Eigen::Vector3d &c, - const Eigen::Vector3d &d) + 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 Eigen::Vector3d &u, - const Eigen::Vector3d &v, - const Eigen::Vector3d &w ) + const Vector3d &u, + const Vector3d &v, + const Vector3d &w ) { return u.dot(v.cross(w)); }; - Eigen::Vector3d ap = p - a; - Eigen::Vector3d bp = p - b; - Eigen::Vector3d ab = b - a; - Eigen::Vector3d ac = c - a; - Eigen::Vector3d ad = d - a; - Eigen::Vector3d bc = c - b; - Eigen::Vector3d bd = d - b; + 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 Eigen::Matrix<double,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6); + 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 Eigen::Vector3d &p, - const Eigen::Vector3d &a, - const Eigen::Vector3d &b, - const Eigen::Vector3d &c, - const Eigen::Vector3d &d) + const Vector3d &p, + const Vector3d &a, + const Vector3d &b, + const Vector3d &c, + const Vector3d &d) { using namespace Eigen; auto check_face = []( |