From dbf2050269f51974f6f978a64aa3294db36895c0 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 29 Feb 2020 13:10:35 -0800 Subject: [PATCH] extrapolate render mesh --- .../PhysicsServerCommandProcessor.cpp | 8 ++- src/BulletSoftBody/btSoftBody.cpp | 39 ++++++++--- src/BulletSoftBody/btSoftBody.h | 1 + src/BulletSoftBody/btSoftBodyHelpers.cpp | 68 ++++++++++++++++++- src/BulletSoftBody/btSoftBodyHelpers.h | 4 ++ 5 files changed, 109 insertions(+), 11 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c6db34cac..50486b5e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8299,9 +8299,15 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->m_renderFaces.push_back(ff); } } - } + if (out_sim_type == UrdfGeometry::FILE_VTK) + { btSoftBodyHelpers::interpolateBarycentricWeights(psb); + } + else if (out_sim_type == UrdfGeometry::FILE_OBJ) + { + btSoftBodyHelpers::extrapolateBarycentricWeights(psb); + } } else { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index d14ef82dc..3ab5dab82 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3612,18 +3612,39 @@ void btSoftBody::setMaxStress(btScalar maxStress) // 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 < 4; ++j) - { - if (m_renderNodesParents[i].size()) + if (m_z.size() > 0) + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + const Node* p0 = m_renderNodesParents[i][0]; + const Node* p1 = m_renderNodesParents[i][1]; + const Node* p2 = m_renderNodesParents[i][2]; + btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x); + btVector3 unit_normal = normal.normalized(); + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 3; ++j) { n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; } - } - } + n.m_x += m_z[i] * unit_normal; + } + } + else + { + for (int i = 0; i < m_renderNodes.size(); ++i) + { + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 4; ++j) + { + if (m_renderNodesParents[i].size()) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + } + } + } } void btSoftBody::setCollisionQuadrature(int N) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 0c26a3481..055b67758 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -812,6 +812,7 @@ public: btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; + btAlignedObjectArray m_z; // vertical distance used in extrapolation bool m_useSelfCollision; bool m_usePostCollisionDamping; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 649d6f58c..9115e6333 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1500,10 +1500,27 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); } +// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[0] = (d11 * d20 - d01 * d21) * invDenom; + bary[1] = (d00 * d21 - d01 * d20) * invDenom; + bary[2] = 1.0 - bary[0] - bary[1]; + bary[3] = 0; +} + // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) { + psb->m_z.resize(0); 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) @@ -1513,7 +1530,6 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) btVector4 optimal_bary; btScalar min_bary_weight = -1e3; btAlignedObjectArray optimal_parents; - bool found = false; for (int j = 0; j < psb->m_tetras.size(); ++j) { const btSoftBody::Tetra& t = psb->m_tetras[j]; @@ -1544,3 +1560,53 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) psb->m_renderNodesParents[i] = optimal_parents; } } + + +// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. +void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.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; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray optimal_parents; + btScalar dist; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 unit_n = n.normalized(); + dist = p.dot(unit_n); + btVector3 proj_p = p - dist*unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if the projected p is inside the triangle at hand + if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = dist; + } +} diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index b20f2f6d6..abe187089 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -148,8 +148,12 @@ struct btSoftBodyHelpers static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); + static void interpolateBarycentricWeights(btSoftBody* psb); + static void extrapolateBarycentricWeights(btSoftBody* psb); + static void generateBoundaryFaces(btSoftBody* psb); static void duplicateFaces(const char* filename, const btSoftBody* psb);