add render mesh interpolation from simulation mesh

src/BulletSoftBody/btSoftBody.cpp

@@ -3082,6 +3082,20 @@ void btSoftBody::applyForces()
 	}
 }
 
+//
+void btSoftBody::interpolateRenderMesh()
+{
+    for (int i = 0; i < m_renderNodes.size(); ++i)
+    {
+        Node& n = m_renderNodes[i];
+        n.m_x.setZero();
+        for (int j = 0; j < m_renderNodesParents[i].size(); ++j)
+        {
+            n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[j];
+        }
+    }
+}
+
 //
 void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
 {

src/BulletSoftBody/btSoftBody.h

@@ -703,8 +703,10 @@ public:
 	btSoftBodyWorldInfo* m_worldInfo;  // World info
 	tNoteArray m_notes;                // Notes
 	tNodeArray m_nodes;                // Nodes
+    tNodeArray m_renderNodes;                // Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
+    tFaceArray m_renderFaces;                // Faces
 	tTetraArray m_tetras;              // Tetras
 	tAnchorArray m_anchors;            // Anchors
 	tRContactArray m_rcontacts;        // Rigid contacts
@@ -720,6 +722,9 @@ public:
 	tClusterArray m_clusters;          // Clusters
     btScalar m_dampingCoefficient;     // Damping Coefficient
     
+    btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
+    btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
+
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
 	btTransform m_initialWorldTransform;
@@ -1030,6 +1035,7 @@ public:
     void initializeDmInverse();
     void updateDeformation();
 	void applyForces();
+    void interpolateRenderMesh();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -23,8 +23,9 @@ subject to the following restrictions:
 #include "btSoftBodyHelpers.h"
 #include "LinearMath/btConvexHull.h"
 #include "LinearMath/btConvexHullComputer.h"
+#include <map>
+#include <vector>
 
-//
 static void drawVertex(btIDebugDraw* idraw,
 					   const btVector3& x, btScalar s, const btVector3& c)
 {
@@ -1321,6 +1322,57 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
             psb->appendLink(ni[2], ni[3], 0, true);
         }
     }
+    
+    std::map<std::vector<int>, std::vector<int> > dict;
+    for (int i = 0; i < indices.size(); ++i)
+    {
+        for (int j = 0; j < 4; ++j)
+        {
+            std::vector<int> f;
+            if (j == 0)
+            {
+                f.push_back(indices[i][1]);
+                f.push_back(indices[i][0]);
+                f.push_back(indices[i][2]);
+            }
+            if (j == 1)
+            {
+                f.push_back(indices[i][3]);
+                f.push_back(indices[i][0]);
+                f.push_back(indices[i][1]);
+            }
+            if (j == 2)
+            {
+                f.push_back(indices[i][3]);
+                f.push_back(indices[i][1]);
+                f.push_back(indices[i][2]);
+            }
+            if (j == 3)
+            {
+                f.push_back(indices[i][2]);
+                f.push_back(indices[i][0]);
+                f.push_back(indices[i][3]);
+            }
+            std::vector<int> f_sorted = f;
+            std::sort(f_sorted.begin(), f_sorted.end());
+            if (dict.find(f_sorted) != dict.end())
+            {
+                dict.erase(f_sorted);
+            }
+            else
+            {
+                dict.insert(std::make_pair(f_sorted, f));
+            }
+        }
+    }
+
+    for (auto it = dict.begin(); it != dict.end(); ++it)
+    {
+        std::vector<int> f = it->second;
+        psb->appendFace(f[0], f[1], f[2]);
+    }
+    
+    
     psb->initializeDmInverse();
     printf("Nodes:  %u\r\n", psb->m_nodes.size());
     printf("Links:  %u\r\n", psb->m_links.size());
@@ -1330,3 +1382,111 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
     fs.close();
     return psb;
 }
+
+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();
+}
+
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
+{
+    btVector3 vap = p - a;
+    btVector3 vbp = p - b;
+    
+    btVector3 vab = b - a;
+    btVector3 vac = c - a;
+    btVector3 vad = d - a;
+    
+    btVector3 vbc = c - b;
+    btVector3 vbd = d - b;
+    btScalar va6 = (vbp.cross(vbd)).dot(vbc);
+    btScalar vb6 = (vap.cross(vac)).dot(vad);
+    btScalar vc6 = (vap.cross(vad)).dot(vab);
+    btScalar vd6 = (vap.cross(vab)).dot(vac);
+    btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
+    bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
+}
+
+void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb)
+{
+    std::ifstream fs;
+    fs.open(file);
+    std::string line;
+    btVector3 pos;
+    while (std::getline(fs, line))
+    {
+        std::stringstream ss(line);
+        if (line[0] == 'v')
+        {
+            ss.ignore();
+            for (size_t i = 0; i < 3; i++)
+                ss >> pos[i];
+            btSoftBody::Node n;
+            n.m_x = pos;
+            psb->m_renderNodes.push_back(n);
+        }
+        else if (line[0] == 'f')
+        {
+            ss.ignore();
+            int id0, id1, id2;
+            ss >> id0;
+            ss >> id1;
+            ss >> id2;
+            btSoftBody::Face f;
+            f.m_n[0] = &psb->m_renderNodes[id0-1];
+            f.m_n[1] = &psb->m_renderNodes[id1-1];
+            f.m_n[2] = &psb->m_renderNodes[id2-1];
+            psb->m_renderFaces.push_back(f);
+        }
+    }
+    fs.close();
+}
+
+void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
+{
+    psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+    psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+    for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+    {
+        const btVector3& p = psb->m_renderNodes[i].m_x;
+        btVector4 bary;
+        for (int j = 0; j < psb->m_tetras.size(); ++j)
+        {
+            const btSoftBody::Tetra& t = psb->m_tetras[j];
+            getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
+            if (bary[0]>=0 && bary[1]>=0 && bary[2]>=0 && bary[3]>=0)
+            {
+                btAlignedObjectArray<const btSoftBody::Node*> parents;
+                parents.push_back(t.m_n[0]);
+                parents.push_back(t.m_n[1]);
+                parents.push_back(t.m_n[2]);
+                parents.push_back(t.m_n[3]);
+                psb->m_renderNodesParents[i] = parents;
+                break;
+            }
+        }
+        psb->m_renderNodesInterpolationWeights[i] = bary;
+    }
+}

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -144,8 +144,13 @@ struct btSoftBodyHelpers
 											bool bfacesfromtetras);
     static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
 
+    static void writeObj(const char* file, const btSoftBody* psb);
     
+    static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
     
+    static void readRenderMeshFromObj(const char* file, btSoftBody* psb);
+
+    static void interpolateBarycentricWeights(btSoftBody* psb);
 	/// Sort the list of links to move link calculations that are dependent upon earlier
 	/// ones as far as possible away from the calculation of those values
 	/// This tends to make adjacent loop iterations not dependent upon one another,