added BVH

14 files changed, 487 insertions, 135 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 05caac30633..63961cbb334 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -24,7 +24,7 @@ set(INC
 
 set(INC_SYS
   ${EIGEN3_INCLUDE_DIRS}
-  ../../source/blender/blenlib
+  ../../source/blender/blenlib # BLI_task for threading
 )
 
 set(SRC
@@ -38,6 +38,10 @@ set(SRC
   src/admmpd_solver.cpp
   src/admmpd_collision.h
   src/admmpd_collision.cpp
+  src/admmpd_bvh.h
+  src/admmpd_bvh.cpp
+  src/admmpd_bvh_traverse.h
+  src/admmpd_bvh_traverse.cpp
 )
 
 set(LIB
diff --git a/extern/softbody/LICENSE b/extern/softbody/LICENSE
new file mode 100644
index 00000000000..dcd0e693ed1
--- /dev/null
+++ b/extern/softbody/LICENSE
@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2020 Matthew Overby
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/extern/softbody/src/admmpd_bvh.cpp b/extern/softbody/src/admmpd_bvh.cpp
new file mode 100644
index 00000000000..97c39271199
--- /dev/null
+++ b/extern/softbody/src/admmpd_bvh.cpp
@@ -0,0 +1,222 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_bvh.h"
+#include <numeric> // iota
+
+// Adapted from:
+// https://github.com/mattoverby/mclscene/blob/master/include/MCL/BVH.hpp
+
+namespace admmpd {
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::clear()
+{
+    root = std::make_shared<Node>();
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::init(const std::vector<AABB> &leaves)
+{
+    root = std::make_shared<Node>();
+    int np = leaves.size();
+    if (np==0)
+        return;
+    std::vector<int> queue(np);
+	std::iota(queue.begin(), queue.end(), 0);
+    create_children(root.get(), queue, leaves);
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::update(const std::vector<AABB> &leaves)
+{
+    if (!root || (int)leaves.size()==0)
+        return;
+    update_children(root.get(), leaves);
+}
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse(Traverser<T,DIM> &traverser) const
+{
+    if (!root)
+        return false;
+    return traverse_children(root.get(), traverser);
+}
+
+// If we are traversing with function pointers, we'll just
+// wrap our own traverser that calls these functions to
+// avoid duplicate traverse_children code.
+template <typename T, int DIM>
+class TraverserFromFunctionPtrs : public Traverser<T,DIM>
+{
+using typename Traverser<T,DIM>::AABB;
+public:
+    std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t;
+    std::function<bool(const AABB&, int)> s;
+    void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first)
+    {
+        t(left_aabb, go_left, right_aabb, go_right, go_left_first);
+    }
+	bool stop_traversing(const AABB &aabb, int prim)
+    {
+        return s(aabb,prim);
+    }
+};
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse(
+    std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t,
+    std::function<bool(const AABB&, int)> s) const
+{
+    if (!root)
+        return false;
+    TraverserFromFunctionPtrs<T,DIM> traverser;
+    traverser.t = t;
+    traverser.s = s;
+    return traverse_children(root.get(), traverser);
+}
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::create_children(
+    Node *node,
+    std::vector<int> &queue,
+    const std::vector<AABB> &leaves)
+{
+	node->aabb.setEmpty();
+    int n_queue = queue.size();
+	if (n_queue == 1)
+    {
+	    node->prims.emplace_back(queue[0]);
+		node->aabb = leaves[queue[0]];
+		return;
+	}
+
+    for (int i=0; i<n_queue; ++i)
+    {
+        int q = queue[i];
+        node->aabb.extend(leaves[q]);
+    }
+
+    struct SortByAxis
+    {
+        int axis;
+        const std::vector<AABB> &aabbs;
+        SortByAxis(int axis_, const std::vector<AABB> &aabbs_) :
+            axis(axis_), aabbs(aabbs_) {}
+        bool operator()(size_t l, size_t r) const
+        {
+            return aabbs[l].center()[axis] < aabbs[r].center()[axis];
+        }
+    };
+
+    // Sort tree and split queue
+    int sort_axis = 0;
+    VecType sizes = node->aabb.sizes();
+    sizes.maxCoeff(&sort_axis);
+    std::sort(queue.begin(), queue.end(), SortByAxis(sort_axis,leaves));
+	std::vector<int> left_queue(queue.begin(), queue.begin()+(n_queue/2));
+	std::vector<int> right_queue(queue.begin()+(n_queue/2), queue.end());
+
+    // Recursive top-down constructrion
+    node->left = new Node();
+	create_children(node->left, left_queue, leaves);
+    node->right = new Node();
+	create_children(node->right, right_queue, leaves);
+
+} // end create children
+
+template <typename T, int DIM>
+void AABBTree<T,DIM>::update_children(
+    Node *node,
+    const std::vector<AABB> &leaves)
+{
+	node->aabb.setEmpty();
+
+    if (node->is_leaf())
+    {
+        int np = node->prims.size();
+        for (int i=0; i<np; ++i)
+        {
+            node->aabb.extend(leaves[node->prims[i]]);
+        }
+        return;
+    }
+
+	if (node->left != nullptr)
+    {
+		update_children(node->left, leaves);
+		node->aabb.extend(node->left->aabb);
+	}
+	if (node->right != nullptr)
+    {
+		update_children(node->right, leaves);
+		node->aabb.extend(node->right->aabb);
+	}
+
+} // end update children
+
+template <typename T, int DIM>
+bool AABBTree<T,DIM>::traverse_children(
+    const Node *node,
+    Traverser<T,DIM> &traverser ) const
+{
+	if( node->is_leaf() ){
+		int np = node->prims.size();
+		for(int i=0; i<np; ++i)
+        {
+			if(traverser.stop_traversing(node->aabb, node->prims[i]))
+                return true;
+		}
+		return false;
+	}
+
+	bool go_left = true;
+	bool go_right = true;
+	bool go_left_first = true;
+    const AABB &left_aabb = (node->left == nullptr ? AABB() : node->left->aabb);
+    const AABB &right_aabb = (node->right == nullptr ? AABB() : node->right->aabb);
+	traverser.traverse(
+		left_aabb, go_left,
+		right_aabb, go_right,
+		go_left_first );
+
+	if (go_left && go_right)
+    {
+        if (go_left_first)
+        {
+            if (traverse_children(node->left, traverser)) { return true; }
+		    else { return traverse_children(node->right, traverser); }
+        }
+        else
+        {
+            if (traverse_children(node->right, traverser)) { return true; }
+		    else { return traverse_children(node->left, traverser); }
+        }
+	}
+	if (go_left && !go_right)
+    {
+		return traverse_children(node->left, traverser);
+	}
+	if (!go_left && go_right)
+    {
+		return traverse_children(node->right, traverser);
+	}
+
+	return false;
+
+} // end traverse children
+
+// Compile types
+template class admmpd::AABBTree<double,2>;
+template class admmpd::AABBTree<double,3>;
+template class admmpd::AABBTree<float,2>;
+template class admmpd::AABBTree<float,3>;
+template class admmpd::TraverserFromFunctionPtrs<double,2>;
+template class admmpd::TraverserFromFunctionPtrs<double,3>;
+template class admmpd::TraverserFromFunctionPtrs<float,2>;
+template class admmpd::TraverserFromFunctionPtrs<float,3>;
+
+} // namespace admmpd
\ No newline at end of file
diff --git a/extern/softbody/src/admmpd_bvh.h b/extern/softbody/src/admmpd_bvh.h
new file mode 100644
index 00000000000..dca84e6803f
--- /dev/null
+++ b/extern/softbody/src/admmpd_bvh.h
@@ -0,0 +1,81 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_H_
+#define ADMMPD_BVH_H_ 1
+
+#include "admmpd_bvh_traverse.h"
+#include <vector>
+#include <memory>
+#include <functional>
+
+namespace admmpd {
+
+template <typename T, int DIM>
+class AABBTree
+{
+protected:
+	typedef Eigen::AlignedBox<T,DIM> AABB;
+	typedef Eigen::Matrix<T,DIM,1> VecType;
+public:
+	// Removes all BVH data
+	void clear();
+
+	// Initializes the BVH with a list of leaf bounding boxes.
+	// Sorts each split by largest axis.
+	void init(const std::vector<AABB> &leaves);
+
+	// Recomputes the bounding boxes of leaf and parent
+	// nodes but does not sort the tree.
+	void update(const std::vector<AABB> &leaves);
+
+	// Traverse the tree. Returns result of traverser
+	bool traverse(Traverser<T,DIM> &traverser) const;
+
+	// Traverse the tree with function pointers instead of class:
+	// void traverse(
+	//	const AABB &left_aabb, bool &go_left,
+	//	const AABB &right_aabb, bool &go_right,
+	//	bool &go_left_first);
+	// bool stop_traversing( const AABB &aabb, int prim );
+	bool traverse(
+		std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t,
+		std::function<bool(const AABB&, int)> s) const;
+
+protected:
+
+	struct Node
+	{
+		AABB aabb;
+		Node *left, *right;
+		std::vector<int> prims;
+		bool is_leaf() const { return prims.size()>0; }
+		Node() : left(nullptr), right(nullptr) {}
+		~Node()
+		{
+			if(left){ delete left; }
+			if(right){ delete right; }
+		}
+	};
+
+	std::shared_ptr<Node> root;
+
+	void create_children(
+		Node *node,
+		std::vector<int> &queue,
+		const std::vector<AABB> &leaves);
+
+	void update_children(
+		Node *node,
+		const std::vector<AABB> &leaves);
+
+	bool traverse_children(
+		const Node *node,
+		Traverser<T,DIM> &traverser ) const;
+
+}; // class AABBtree
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_H_
+
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 0cd828011ad..7a53c91a150 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -1,4 +1,5 @@
-
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
 #include "admmpd_collision.h"
 
@@ -32,4 +33,4 @@ void FloorCollider::jacobian(
 	}
 } // end floor collider Jacobian
 
-} // namespace admmpd
\ No newline at end of file
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 274be91be6d..c3fd9cb8c4f 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -1,8 +1,8 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
-
-
-#ifndef _ADMMPD_COLLISION_H
-#define _ADMMPD_COLLISION_H
+#ifndef ADMMPD_COLLISION_H_
+#define ADMMPD_COLLISION_H_
 
 #include <Eigen/Sparse>
 #include <vector>
@@ -39,4 +39,4 @@ public:
 
 } // namespace admmpd
 
-#endif //_ADMMPD_COLLISION_H
+#endif // ADMMPD_COLLISION_H_
diff --git a/extern/softbody/src/admmpd_energy.cpp b/extern/softbody/src/admmpd_energy.cpp
index 41aebbe0739..a454bf1a86c 100644
--- a/extern/softbody/src/admmpd_energy.cpp
+++ b/extern/softbody/src/admmpd_energy.cpp
@@ -1,5 +1,5 @@
-
-
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
 #include "admmpd_energy.h"
 #include <iostream>
diff --git a/extern/softbody/src/admmpd_energy.h b/extern/softbody/src/admmpd_energy.h
index e58ba36a9e7..6a659613661 100644
--- a/extern/softbody/src/admmpd_energy.h
+++ b/extern/softbody/src/admmpd_energy.h
@@ -1,8 +1,8 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
-
-
-#ifndef _ADMMPD_ENERGY_H
-#define _ADMMPD_ENERGY_H 1
+#ifndef ADMMPD_ENERGY_H_
+#define ADMMPD_ENERGY_H_ 1
 
 #include <Eigen/Sparse>
 #include <Eigen/Geometry>
@@ -54,7 +54,7 @@ public:
 
 } // end namespace admmpd
 
-#endif // _ADMMPD_ENERGY_H
+#endif // ADMMPD_ENERGY_H_
 
 
 
diff --git a/extern/softbody/src/admmpd_lattice.cpp b/extern/softbody/src/admmpd_lattice.cpp
index b0c8f6ca556..2440c6f4042 100644
--- a/extern/softbody/src/admmpd_lattice.cpp
+++ b/extern/softbody/src/admmpd_lattice.cpp
@@ -1,55 +1,17 @@
-
-
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
 #include "admmpd_lattice.h"
 #include "admmpd_math.h"
+#include "admmpd_bvh.h"
+#include "admmpd_bvh_traverse.h"
 #include <iostream>
 #include <unordered_map>
-
-//#include "vdb.h"
+#include "BLI_task.h" // threading
 
 namespace admmpd {
 using namespace Eigen;
 
-// We store our deformable data in column major to make
-// matrix-vector mults and solves faster, but we want
-// to map raw data as row major.
-inline void map_to_x3d(const std::vector<Vector3d> &x_vec, Eigen::MatrixXd *x)
-{
-	int nx = x_vec.size();
-	if (nx==0)
-	{
-		*x = MatrixXd();
-		return;
-	}
-
-	x->resize(nx,3);
-	for (int i=0; i<nx; ++i)
-	{
-		x->operator()(i,0) = x_vec[i][0];
-		x->operator()(i,1) = x_vec[i][1];
-		x->operator()(i,2) = x_vec[i][2];
-	}
-}
-
-inline void map_to_x4i(const std::vector<Vector4i> &x_vec, Eigen::MatrixXi *x)
-{
-	int nx = x_vec.size();
-	if (nx==0)
-	{
-		*x = MatrixXi();
-		return;
-	}
-
-	x->resize(nx,4);
-	for (int i=0; i<nx; ++i)
-	{
-		x->operator()(i,0) = x_vec[i][0];
-		x->operator()(i,1) = x_vec[i][1];
-		x->operator()(i,2) = x_vec[i][2];
-		x->operator()(i,3) = x_vec[i][3];
-	}
-}
 
 bool Lattice::generate(
 	const Eigen::MatrixXd &V,
@@ -162,6 +124,40 @@ bool Lattice::generate(
 
 } // end gen lattice
 
+typedef struct FindTetThreadData {
+	AABBTree<double,3> *tree;
+	const MatrixXd *pts;
+	VectorXi *pts_to_tet;
+	MatrixXd *barys;
+	const MatrixXd *tet_x;
+	const MatrixXi *tets;
+} FindTetThreadData;
+
+static void parallel_point_in_tet(
+	void *__restrict userdata,
+	const int i,
+	const TaskParallelTLS *__restrict UNUSED(tls))
+{
+	FindTetThreadData *td = (FindTetThreadData*)userdata;
+	Vector3d pt = td->pts->row(i);
+	PointInTetTraverse<double> traverser(pt, td->tet_x, td->tets);
+	bool success = td->tree->traverse(traverser);
+	int tet_idx = traverser.output.prim;
+	if (success && tet_idx >= 0)
+	{
+		RowVector4i tet = td->tets->row(tet_idx);
+		Vector3d t[4] = {
+			td->tet_x->row(tet[0]),
+			td->tet_x->row(tet[1]),
+			td->tet_x->row(tet[2]),
+			td->tet_x->row(tet[3])
+		};
+		td->pts_to_tet->operator[](i) = tet_idx;
+		Vector4d b = barycoords::point_tet(pt,t[0],t[1],t[2],t[3]);
+		td->barys->row(i) = b;
+	}
+} // end parallel lin solve
+
 bool Lattice::compute_vtx_tet_mapping(
 	const Eigen::MatrixXd *vtx_, // embedded vertices, p x 3
 	Eigen::VectorXi *vtx_to_tet_, // what tet vtx is embedded in, p x 1
@@ -176,34 +172,59 @@ bool Lattice::compute_vtx_tet_mapping(
 	if (nv==0)
 		return false;
 
-	// TODO:
-	// Use AABB Tree to do point-in-tet and compute bary weighting
-	// For now the dumb approach and loop all.
-
 	barys_->resize(nv,4);
-	barys_->setZero();
+	barys_->setOnes();
 	vtx_to_tet_->resize(nv);
 	int nt = tets_->rows();
 
-	for (int i=0; i<nv; ++i)
+	// BVH tree for finding point-in-tet and computing
+	// barycoords for each embedded vertex
+	std::vector<AlignedBox<double,3> > tet_aabbs;
+	tet_aabbs.resize(nt);
+	for (int i=0; i<nt; ++i)
 	{
-		Vector3d v = vtx_->row(i);
-		for (int j=0; j<nt; ++j)
+		tet_aabbs[i].setEmpty();
+		RowVector4i tet = tets_->row(i);
+		for (int j=0; j<4; ++j)
 		{
-			RowVector4i tet = tets_->row(j);
-			Vector3d t[4] = {
-				x_->row(tet[0]),
-				x_->row(tet[1]),
-				x_->row(tet[2]),
-				x_->row(tet[3])
-			};
-			if (!barycoords::point_in_tet(v,t[0],t[1],t[2],t[3]))
-				continue;
+			tet_aabbs[i].extend(x_->row(tet[j]).transpose());
+		}
+	}
 
-			Vector4d b = barycoords::point_tet(v,t[0],t[1],t[2],t[3]);
-			vtx_to_tet_->operator[](i) = j;
-			barys_->row(i) = b;
-			break;
+	AABBTree<double,3> tree;
+	tree.init(tet_aabbs);
+
+	FindTetThreadData thread_data = {
+		.tree = &tree,
+		.pts = vtx_,
+		.pts_to_tet = vtx_to_tet_,
+		.barys = barys_,
+		.tet_x = x_,
+		.tets = tets_
+	};
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	BLI_task_parallel_range(0, nv, &thread_data, parallel_point_in_tet, &settings);
+
+	// Double check we set (valid) barycoords for every embedded vertex
+	const double eps = 1e-8;
+	for (int i=0; i<nv; ++i)
+	{
+		RowVector4d b = barys_->row(i);
+		if (b.minCoeff() < -eps)
+		{
+			printf("**Lattice::generate Error: negative barycoords\n");
+			return false;
+		}
+		if (b.maxCoeff() > 1 + eps)
+		{
+			printf("**Lattice::generate Error: max barycoord > 1\n");
+			return false;
+		}
+		if (b.sum() > 1 + eps)
+		{
+			printf("**Lattice::generate Error: barycoord sum > 1\n");
+			return false;
 		}
 	}
 
@@ -226,4 +247,4 @@ Eigen::Vector3d Lattice::get_mapped_vertex(
 		x_or_v->row(tet[3]) * b[3]);
 }
 
-} // namespace admmpd
\ No newline at end of file
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_lattice.h b/extern/softbody/src/admmpd_lattice.h
index 15e2d391d11..0190e8eaf40 100644
--- a/extern/softbody/src/admmpd_lattice.h
+++ b/extern/softbody/src/admmpd_lattice.h
@@ -1,8 +1,8 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
-
-
-#ifndef _ADMMPD_LATTICE_H
-#define _ADMMPD_LATTICE_H
+#ifndef ADMMPD_LATTICE_H_
+#define ADMMPD_LATTICE_H_
 
 #include <Eigen/Dense>
 #include <vector>
@@ -40,4 +40,4 @@ protected:
 
 } // namespace admmpd
 
-#endif // _ADMMPD_LATTICE_H
\ No newline at end of file
+#endif // ADMMPD_LATTICE_H_
diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp
index 7c76dfb8b3a..7d1a4f82844 100644
--- a/extern/softbody/src/admmpd_math.cpp
+++ b/extern/softbody/src/admmpd_math.cpp
@@ -1,4 +1,5 @@
-
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
 #include "admmpd_math.h"
 
@@ -34,34 +35,35 @@ namespace barycoords {
 		return Eigen::Matrix<double,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
     } // end point tet 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)
+} // 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)
+{
+	using namespace Eigen;
+	auto check_face = [](
+		const Vector3d &point,
+		const Vector3d &p0,
+		const Vector3d &p1,
+		const Vector3d &p2,
+		const Vector3d &p3 )
 	{
-		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);
-	}
+		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 barycoords
-} // namespace admmpd
\ No newline at end of file
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h
index 25ff2d3114f..e577d6b9dd7 100644
--- a/extern/softbody/src/admmpd_math.h
+++ b/extern/softbody/src/admmpd_math.h
@@ -1,8 +1,8 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
-
-
-#ifndef _ADMMPD_MATH_H
-#define _ADMMPD_MATH_H
+#ifndef ADMMPD_MATH_H_
+#define ADMMPD_MATH_H_
 
 #include <Eigen/Geometry>
 
@@ -16,6 +16,8 @@ Eigen::Vector4d point_tet(
     const Eigen::Vector3d &c,
     const Eigen::Vector3d &d);
 
+} // namespace barycoords
+
 bool point_in_tet(
     const Eigen::Vector3d &p,
     const Eigen::Vector3d &a,
@@ -23,8 +25,6 @@ bool point_in_tet(
     const Eigen::Vector3d &c,
     const Eigen::Vector3d &d);
 
-} // namespace barycoords
-
 } // namespace admmpd
 
-#endif //_ADMMPD_MATH_H
+#endif // ADMMPD_MATH_H_
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 298ba7ff173..48f89a5c7ed 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -1,5 +1,5 @@
-
-
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
 #include "admmpd_solver.h"
 #include "admmpd_lattice.h"
@@ -141,7 +141,6 @@ void Solver::update_constraints(
 	const Options *options,
 	Data *data)
 {
-
 	std::vector<double> l_coeffs;
 	std::vector<Eigen::Triplet<double> > trips_x;
     std::vector<Eigen::Triplet<double> > trips_y;
@@ -181,8 +180,8 @@ void Solver::update_constraints(
 
 typedef struct LinSolveThreadData {
 	Data *data;
-	MatrixXd *x;
-	MatrixXd *b;
+	MatrixXd *ls_x;
+	MatrixXd *ls_b;
 } LinSolveThreadData;
 
 static void parallel_lin_solve(
@@ -191,7 +190,7 @@ static void parallel_lin_solve(
 	const TaskParallelTLS *__restrict UNUSED(tls))
 {
 	LinSolveThreadData *td = (LinSolveThreadData*)userdata;
-	td->x->col(i) = td->data->ldltA.solve(td->b->col(i));
+	td->ls_x->col(i) = td->data->ldltA.solve(td->ls_b->col(i));
 } // end parallel lin solve
 
 void Solver::solve_conjugate_gradients(
@@ -204,7 +203,7 @@ void Solver::solve_conjugate_gradients(
 		MatrixXd *x_,
 		MatrixXd *b_)
 	{
-		LinSolveThreadData thread_data = {.data=data_, .x=x_, .b=b_};
+		LinSolveThreadData thread_data = {.data=data_, .ls_x=x_, .ls_b=b_};
 		TaskParallelSettings settings;
 		BLI_parallel_range_settings_defaults(&settings);
 		BLI_task_parallel_range(0, 3, &thread_data, parallel_lin_solve, &settings);
@@ -427,6 +426,7 @@ void Solver::append_energies(
 		data->indices.emplace_back(energy_index, energy_dim);
 		energy_index += energy_dim;
 	}
+
 } // end append energies
 
-} // namespace admmpd
\ No newline at end of file
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index c690bae189c..6a066f5c05f 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -1,8 +1,8 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
 
-
-
-#ifndef _ADMMPD_H
-#define _ADMMPD_H
+#ifndef ADMMPD_SOLVER_H_
+#define ADMMPD_SOLVER_H_
 
 #include <Eigen/Geometry>
 #include <Eigen/Sparse>
@@ -124,4 +124,4 @@ protected:
 
 } // namespace admmpd
 
-#endif // _ADMMPD_H
+#endif // ADMMPD_SOLVER_H_