From 403eb62dfaec584e2706e29230adcc45b68131ce Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sun, 15 Sep 2019 12:06:28 -0700 Subject: [PATCH] code clean up and optimization --- src/BulletSoftBody/btDeformableBodySolver.cpp | 41 +++------- src/BulletSoftBody/btDeformableBodySolver.h | 3 +- .../btDeformableContactConstraint.cpp | 4 +- .../btDeformableContactProjection.cpp | 76 +++---------------- .../btDeformableMultiBodyConstraintSolver.cpp | 2 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 19 ++--- .../btDeformableNeoHookeanForce.h | 66 +++++++--------- src/BulletSoftBody/btSoftBody.cpp | 9 ++- src/BulletSoftBody/btSoftBody.h | 11 +++ src/BulletSoftBody/btSoftBodyHelpers.cpp | 2 + 10 files changed, 86 insertions(+), 147 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 5292d7ecc..7342c26a6 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -22,7 +22,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(20) -, m_maxNewtonIterations(5) +, m_maxNewtonIterations(10) , m_newtonTolerance(1e-4) , m_lineSearch(true) { @@ -82,13 +82,12 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) do { scale *= beta; if (scale < 1e-8) { - //std::cout << "Could not find sufficient descent!" << std::endl; return; } updateEnergy(scale); f1 = m_objective->totalEnergy()+kineticEnergy(); f2 = f0 - alpha * scale * inner_product; - } while (!(f1 < f2)); // if anything here is nan then the search continues + } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues revertDv(); updateDv(scale); } @@ -97,7 +96,6 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) computeStep(m_ddv, m_residual); updateDv(); } - for (int j = 0; j < m_numNodes; ++j) { m_ddv[j].setZero(); @@ -157,7 +155,8 @@ btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); btScalar inner_product = m_cg.dot(residual, m_ddv); - btScalar tol = 1e-5 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv); + btScalar tol = 1e-3 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv); + btScalar res_norm = m_objective->computeNorm(residual); if (inner_product < -tol) { std::cout << "Looking backwards!" << std::endl; @@ -170,7 +169,6 @@ btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& else if (std::abs(inner_product) < tol) { std::cout << "Gradient Descent!" << std::endl; - btScalar res_norm = m_objective->computeNorm(residual); btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; for (int i = 0; i < m_ddv.size();++i) { @@ -236,23 +234,7 @@ void btDeformableBodySolver::setConstraints() btScalar btDeformableBodySolver::solveContactConstraints() { BT_PROFILE("setConstraint"); - for (int i = 0; i < m_dv.size();++i) - { - m_dv[i].setZero(); - } btScalar maxSquaredResidual = m_objective->projection.update(); -// m_objective->enforceConstraint(m_dv); -// m_objective->updateVelocity(m_dv); - int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - ++counter; - } - } return maxSquaredResidual; } @@ -304,7 +286,7 @@ void btDeformableBodySolver::backupVelocity() } } -void btDeformableBodySolver::backupVn() +void btDeformableBodySolver::setupDeformableSolve(bool implicit) { int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) @@ -312,17 +294,12 @@ void btDeformableBodySolver::backupVn() btSoftBody* psb = m_softBodySet[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - // Here: - // dv = 0 for nodes not in constraints - // dv = v_{n+1} - v_{n+1}^* for nodes in constraints - if (m_objective->projection.m_projectionsDict.find(psb->m_nodes[j].index)!=NULL) + if (implicit) { - m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; } - // Now: - // dv = 0 for nodes not in constraints - // dv = v_{n+1} - v_n for nodes in constraints - m_backupVelocity[counter++] = psb->m_nodes[j].m_vn; + m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + ++counter; } } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index e0629466a..31fe01934 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -77,13 +77,14 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); - void backupVn(); + void setupDeformableSolve(bool implicit); void revertVelocity(); void updateVelocity(); bool updateNodes(); void computeStep(TVStack& dv, const TVStack& residual); + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual); virtual void predictMotion(btScalar solverdt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 0dafb9c64..8ee2806af 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -92,8 +92,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() btVector3 vb = getVb(); btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); -// if (dn > 0) -// return 0; // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btScalar residualSquare = dn*dn; btVector3 impulse = m_contact->m_c0 * vr; @@ -136,8 +134,10 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() } } impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities applyImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their velocities if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { btRigidBody* rigidCol = 0; diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 9691a2421..f98c73538 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -174,12 +174,6 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) btAlignedObjectArray& constraintsList = *m_faceRigidConstraints.getAtIndex(index); // i is node index int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1(); - int numConstraints = 1; -// int numConstraints = constraintsList.size(); -// if (m_nodeRigidConstraints.find(i) != NULL) -// { -// numConstraints += m_nodeRigidConstraints[i]->size(); -// } for (int j = 0; j < constraintsList.size(); ++j) { const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; @@ -194,7 +188,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) break; } } - x[i] += constraint->getDv(node) / btScalar(numConstraints); + x[i] += constraint->getDv(node); } } // todo xuchenhan@: add deformable deformable constraints' contribution to dv @@ -206,31 +200,6 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) int i = m_staticConstraints.getKeyAtIndex(index).getUid1(); x[i] = constraint.getDv(constraint.m_node); } -// -// for (int i = 0; i < x.size(); ++i) -// { -// x[i].setZero(); -// if (m_staticConstraints.find(i) != NULL) -// { -// // if a node is fixed, dv = 0 -// continue; -// } -// if (m_nodeRigidConstraints.find(i) != NULL) -// { -// btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; -// for (int j = 0; j < constraintsList.size(); ++j) -// { -// const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; -//// x[i] += constraint.getDv(m_nodes->at(i)); -// x[i] += constraint.getDv(constraint.getContact()->m_node); -// } -// } - // todo xuchenhan@ -// if (m_faceRigidConstraints.find(i) != NULL) -// { -// -// } -// } } void btDeformableContactProjection::project(TVStack& x) @@ -376,6 +345,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) } } + // add friction contribution from Face vs. Node if (m_nodeRigidConstraints.find(i) != NULL) { btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; @@ -388,7 +358,8 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) f[i] += constraint.getDv(node)* (1./node->m_im); } } - // todo xuchenhan@ + + // add friction contribution from Face vs. Rigid if (m_faceRigidConstraints.find(i) != NULL) { btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[i]; @@ -398,40 +369,17 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) btSoftBody::Face* face = constraint->getContact()->m_face; // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - // todo xuchenhan@: figure out the index for m_faceRigidConstraints -// f[i] += constraint.getDv(face->m_n[0])* (1./face->m_n[0]->m_im); -// f[i] += constraint.getDv(face->m_n[1])* (1./face->m_n[1]->m_im); -// f[i] += constraint.getDv(face->m_n[2])* (1./face->m_n[2]->m_im); + for (int k = 0; k < 3; ++k) + { + if (face->m_n[0]->index == i) + { + f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); + break; + } + } } } } -// for (int index = 0; index < m_constraints.size(); ++index) -// { -// const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index); -// const btSoftBody::Node* node = constraint.m_node; -// if (node == NULL) -// continue; -// size_t i = m_constraints.getKeyAtIndex(index).getUid1(); -// bool has_static_constraint = false; -// -// // apply dynamic friction force (scaled by dt) if the node does not have static friction constraint -// for (int j = 0; j < constraint.m_static.size(); ++j) -// { -// if (constraint.m_static[j]) -// { -// has_static_constraint = true; -// break; -// } -// } -// for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j) -// { -// btVector3 friction_force = constraint.m_total_tangent_dv[j] * (1./node->m_im); -// if (!has_static_constraint) -// { -// f[i] += friction_force; -// } -// } -// } } void btDeformableContactProjection::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 4b972217c..80a79c978 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -39,7 +39,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif m_analyticsData.m_numSolverCalls++; - std::cout << m_leastSquaresResidual << std::endl; + std::cout << "Contact Residual = " << m_leastSquaresResidual << std::endl; m_analyticsData.m_numIterationsUsed = iteration+1; m_analyticsData.m_islandId = -2; if (numBodies>0) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 0eae6a26a..5cd72f597 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -185,20 +185,21 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); + if (!m_implicit) + { + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + } // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); solveMultiBodyRelatedConstraints(); m_deformableBodySolver->m_objective->projection.setProjection(); - if (m_implicit) - { - // at this point dv = v_{n+1} - v_{n+1}^* - // modify dv such that dv = v_{n+1} - v_n - // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton - m_deformableBodySolver->backupVn(); - } + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); // At this point, dv should be golden for nodes in contact m_deformableBodySolver->solveDeformableConstraints(timeStep); diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index f3e8c0773..37803e6e3 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -31,7 +31,7 @@ public: m_lambda_damp = damping * m_lambda; } - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda) { m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; @@ -50,6 +50,8 @@ public: virtual void addScaledDampingForce(btScalar scale, TVStack& force) { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; int numNodes = getNumNodes(); btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); @@ -69,10 +71,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(tetra.m_F, dF, dP); + firstPiolaDampingDifferential(psb->m_tetraScratches[j], dF, dP); btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - + // damping force differential btScalar scale1 = scale * tetra.m_element_measure; force[id0] -= scale1 * df_on_node0; @@ -89,25 +91,22 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_tetras.size(); ++j) + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(tetra); + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); } } return energy; } - double elasticEnergyDensity(const btSoftBody::Tetra& t) + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) { double density = 0; - btMatrix3x3 F = t.m_F; - btMatrix3x3 C = F.transpose()*F; - double J = F.determinant(); - double trace = C[0].getX() + C[1].getY() + C[2].getZ(); - density += m_mu * 0.5 * (trace - 3.); - density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda); - density -= m_mu * 0.5 * log(trace+1); + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace+1); return density; } @@ -123,7 +122,7 @@ public: { btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P; - firstPiola(tetra.m_F,P); + firstPiola(psb->m_tetraScratches[j],P); btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); @@ -148,6 +147,8 @@ public: virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; int numNodes = getNumNodes(); btAssert(numNodes <= df.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); @@ -167,10 +168,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(tetra.m_F, dF, dP); + firstPiolaDampingDifferential(psb->m_tetraScratches[j], dF, dP); btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - + // damping force differential btScalar scale1 = scale * tetra.m_element_measure; df[id0] -= scale1 * df_on_node0; @@ -202,7 +203,7 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDifferential(tetra.m_F, dF, dP); + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); @@ -216,34 +217,25 @@ public: } } - void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) { - btMatrix3x3 C = F.transpose()*F; - btScalar J = F.determinant(); - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu); + P = s.m_F * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_cofF * (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); } - void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - btScalar J = F.determinant(); - btMatrix3x3 C = F.transpose()*F; - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); + dP = dF * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP); - dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); + dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF); } - void firstPiolaDampingDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - btScalar J = F.determinant(); - btMatrix3x3 C = F.transpose()*F; - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - dP = dF * m_mu_damp * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu_damp) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); - - addScaledCofactorMatrixDifferential(F, dF, m_lambda_damp*(J-1.) - 0.75*m_mu_damp, dP); - dP += F.adjoint().transpose() * m_lambda_damp * DotProduct(F.adjoint().transpose(), dF); + dP = dF * m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu_damp) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); + + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); + dP += s.m_cofF * m_lambda_damp * DotProduct(s.m_cofF, dF); } btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 24c6cf21a..0c4f50206 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2905,6 +2905,13 @@ void btSoftBody::updateDeformation() c1.getY(), c2.getY(), c3.getY(), c1.getZ(), c2.getZ(), c3.getZ()); t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose()*t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); } } // @@ -3431,7 +3438,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideFace.m_rigidBody = prb1; docollideFace.dynmargin = basemargin + timemargin; docollideFace.stamargin = basemargin; -// m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } break; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 9d3402834..f6be6655e 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -300,6 +300,16 @@ public: btMatrix3x3 m_F; btScalar m_element_measure; }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + }; + /* RContact */ struct RContact { @@ -745,6 +755,7 @@ public: tFaceArray m_faces; // Faces tFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras + btAlignedObjectArray m_tetraScratches; tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts btAlignedObjectArray m_nodeRigidContacts; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 1a93a3091..ed99ce943 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1233,6 +1233,7 @@ if(face&&face[0]) } } psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1374,6 +1375,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size());