added goal positions

1 files changed, 74 insertions, 0 deletions

diff --git a/extern/softbody/src/admmpd_pin.cpp b/extern/softbody/src/admmpd_pin.cpp
new file mode 100644
index 00000000000..ff983a08e87
--- /dev/null
+++ b/extern/softbody/src/admmpd_pin.cpp
@@ -0,0 +1,74 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_pin.h"
+#include "BLI_assert.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+void EmbeddedMeshPin::clear()
+{
+	pin_k.clear();
+	pin_pos.clear();
+}
+
+void EmbeddedMeshPin::set_pin(
+		int idx,
+		const Eigen::Vector3d &qi,
+		const Eigen::Vector3d &ki_)
+{
+	if (!mesh)
+		return;
+
+	if (idx<0 || idx>=mesh->emb_rest_x.rows())
+		return;
+
+	// Clamp
+	Vector3d ki = ki_;
+	for (int i=0; i<3; ++i)
+		ki[i] = std::max(0.0, ki[i]);
+
+	pin_k[idx] = ki;
+	pin_pos[idx] = qi;
+}
+
+void EmbeddedMeshPin::linearize(
+		const Eigen::MatrixXd *x, // not used yet
+		std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *q)
+{
+
+	(void)(x);
+	int np = pin_k.size();
+	trips->reserve((int)trips->size() + np*3*4);
+	q->reserve((int)q->size() + np*3);
+
+	std::unordered_map<int,Eigen::Vector3d>::const_iterator it_k = pin_k.begin();
+	for (; it_k != pin_k.end(); ++it_k)
+	{
+		int emb_idx = it_k->first;
+		const Vector3d &qi = pin_pos[emb_idx];
+		const Vector3d &ki = it_k->second;
+
+		int tet_idx = mesh->emb_vtx_to_tet[emb_idx];
+		RowVector4d bary = mesh->emb_barys.row(emb_idx);
+		RowVector4i tet = mesh->tets.row(tet_idx);
+
+		for (int i=0; i<3; ++i)
+		{
+			int p_idx = q->size();
+			q->emplace_back(qi[i]*ki[i]);
+			for (int j=0; j<4; ++j)
+				trips->emplace_back(p_idx, tet[j]*3+i, bary[j]*ki[i]);
+		}
+	}
+
+} // end linearize
+
+//bool EmbeddedMeshPin::has_pin(int idx) const
+//{
+//	return pin_k.count(idx);
+//}
+
+} // namespace admmpd