obstacle collisions. REALLY need to improve mass computation

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 = [](