From 80acb571199314851b14145e67dd512074a522fc Mon Sep 17 00:00:00 2001
From: Chuyuan Fu <fuchuyuan.kelly@gmail.com>
Date: Thu, 7 May 2020 17:21:55 -0700
Subject: [PATCH 1/2] fix softbody GetRigidTransform

---
 src/BulletSoftBody/btSoftBody.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index e70e94f5f..f1ab3460f 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -1046,10 +1046,14 @@ btTransform btSoftBody::getRigidTransform()
     btVector3 t = getCenterOfMass();
     btMatrix3x3 S;
     S.setZero();
-    // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+    // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+    // It's important to make sure that S has the correct signs.
+    // SVD is only unique up to the ordering of singular values.
+    // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
+    // vaues are negative, SVD will permute colums of U to make two of them positive.
     for (int i = 0; i < m_nodes.size(); ++i)
     {
-        S += OuterProduct(m_X[i], t-m_nodes[i].m_x);
+        S -= OuterProduct(m_X[i], t-m_nodes[i].m_x);
     }
     btVector3 sigma;
     btMatrix3x3 U,V;

From 9d6053a8ffc671bc0e02342a37ad8d8d0dc36ce3 Mon Sep 17 00:00:00 2001
From: Chuyuan Fu <fuchuyuan.kelly@gmail.com>
Date: Thu, 7 May 2020 17:23:12 -0700
Subject: [PATCH 2/2] Add writing surface mesh file for tetrahedron mesh

---
 src/BulletSoftBody/btSoftBodyHelpers.cpp | 85 +++++++++++++++++-------
 1 file changed, 62 insertions(+), 23 deletions(-)

diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp
index c1a87c7d5..5265ab035 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.cpp
+++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -1420,31 +1420,70 @@ void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
     }
 }
 
+//Write the surface mesh to an obj file.
 void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 {
-    std::ofstream fs;
-    fs.open(filename);
-    btAssert(fs);
-    for (int i = 0; i < psb->m_nodes.size(); ++i)
-    {
-        fs << "v";
-        for (int d = 0; d < 3; d++)
-        {
-             fs << " " << psb->m_nodes[i].m_x[d];
-        }
-        fs << "\n";
-    }
-    
-    for (int i = 0; i < psb->m_faces.size(); ++i)
-    {
-        fs << "f";
-        for (int n = 0; n < 3; n++)
-        {
-            fs << " " << psb->m_faces[i].m_n[n]->index + 1;
-        }
-        fs << "\n";
-    }
-    fs.close();
+	std::ofstream fs;
+	fs.open(filename);
+	btAssert(fs);
+
+	if (psb->m_tetras.size() > 0)
+	{
+        // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ 
+		std::map<int, int> dict;
+		for (int i = 0; i < psb->m_faces.size(); i++)
+		{
+			for (int d = 0; d < 3; d++)
+			{
+				int index = psb->m_faces[i].m_n[d]->index;
+				if (dict.find(index) == dict.end())
+				{
+					int dict_size = dict.size();
+					dict[index] = dict_size;
+					fs << "v";
+					for (int k = 0; k < 3; k++)
+					{
+						fs << " " << psb->m_nodes[index].m_x[k];
+					}
+					fs << "\n";
+				}
+			}
+		}
+                // Write surface mesh.
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
+			}
+			fs << "\n";
+		}
+	}
+	else
+	{
+          // For trimesh, directly write out all the nodes and faces.xs
+		for (int i = 0; i < psb->m_nodes.size(); ++i)
+		{
+			fs << "v";
+			for (int d = 0; d < 3; d++)
+			{
+				fs << " " << psb->m_nodes[i].m_x[d];
+			}
+			fs << "\n";
+		}
+
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << psb->m_faces[i].m_n[n]->index + 1;
+			}
+			fs << "\n";
+		}
+	}
+	fs.close();
 }
 
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)