From 8c04a78c9b84bfd13235c1a1333c70f32d231059 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 17:46:26 -0700 Subject: switch from std::unordered_map to btHashMap --- src/BulletSoftBody/btCGProjection.h | 14 +++----- .../btDeformableBackwardEulerObjective.cpp | 9 ++--- .../btDeformableBackwardEulerObjective.h | 13 +++++--- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.h | 1 + .../btDeformableContactProjection.cpp | 38 +++++++++++----------- src/BulletSoftBody/btDeformableContactProjection.h | 13 +++++--- src/BulletSoftBody/btDeformableGravityForce.h | 2 +- src/BulletSoftBody/btDeformableLagrangianForce.h | 9 ++--- src/BulletSoftBody/btDeformableMassSpringForce.h | 12 +++---- .../btDeformableRigidDynamicsWorld.cpp | 7 ++-- src/BulletSoftBody/btSoftBody.h | 1 + src/BulletSoftBody/btSoftBodyInternals.h | 4 ++- 13 files changed, 68 insertions(+), 57 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index ca7417f8f..144208908 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -10,7 +10,6 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -#include class btDeformableRigidDynamicsWorld; @@ -21,6 +20,7 @@ struct DeformableContactConstraint btAlignedObjectArray m_value; // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve btAlignedObjectArray m_accumulated_normal_impulse; + btSoftBody::Node* node; DeformableContactConstraint(const btSoftBody::RContact& rcontact) { @@ -58,7 +58,6 @@ struct DeformableContactConstraint struct DeformableFrictionConstraint { - btAlignedObjectArray m_static; // whether the friction is static btAlignedObjectArray m_impulse; // the impulse magnitude the node feels btAlignedObjectArray m_dv; // the dv magnitude of the node @@ -75,6 +74,7 @@ struct DeformableFrictionConstraint // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray m_accumulated_tangent_impulse; + btSoftBody::Node* node; DeformableFrictionConstraint() { @@ -103,22 +103,18 @@ struct DeformableFrictionConstraint class btCGProjection { public: -// static const int dim = 3; typedef btAlignedObjectArray TVStack; typedef btAlignedObjectArray > TVArrayStack; typedef btAlignedObjectArray > TArrayStack; -// using TVStack = btAlignedObjectArray; -// using TVArrayStack = btAlignedObjectArray >; -// using TArrayStack = btAlignedObjectArray >; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; - const std::unordered_map* m_indices; + const btAlignedObjectArray* m_nodes; const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) : m_softBodies(softBodies) , m_dt(dt) - , m_indices(indices) + , m_nodes(nodes) { } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index a9b77e368..50db01ec9 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -9,7 +9,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt, &m_indices) +, projection(m_softBodies, m_dt, &m_nodes) , m_backupVelocity(backup_v) { m_preconditioner = new DefaultPreconditioner(); @@ -63,10 +63,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { // only the velocity of the constrained nodes needs to be updated during CG solve - for (auto it : projection.m_constraints) +// for (auto it : projection.m_constraints) + for (int i = 0; i < projection.m_constraints.size(); ++i) { - int i = m_indices[it.first]; - it.first->m_v = m_backupVelocity[i] + dv[i]; + int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); + m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 22a685631..42d85f538 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -29,7 +29,7 @@ public: Preconditioner* m_preconditioner; btDeformableContactProjection projection; const TVStack& m_backupVelocity; - std::unordered_map m_indices; + btAlignedObjectArray m_nodes; btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); @@ -97,20 +97,23 @@ public: virtual void updateId() { - size_t index = 0; + size_t id = 0; + m_nodes.clear(); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - m_indices[&(psb->m_nodes[j])] = index++; + psb->m_nodes[j].index = id; + m_nodes.push_back(&psb->m_nodes[j]); + ++id; } } } - std::unordered_map* getIndices() + const btAlignedObjectArray* getIndices() const { - return &m_indices; + return &m_nodes; } }; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a1616a460..4be5d4680 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -50,6 +50,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray cg; + public: btDeformableBackwardEulerObjective* m_objective; diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4ae07290c..5a3f9e555 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -8,7 +8,7 @@ #include "btDeformableContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" #include -#include +#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, @@ -49,10 +49,10 @@ void btDeformableContactProjection::update() m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; + btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); for (int i = 0; i < constraints.size(); ++i) { DeformableContactConstraint& constraint = constraints[i]; @@ -230,13 +230,13 @@ void btDeformableContactProjection::setConstraints() c.push_back(DeformableContactConstraint(btVector3(1,0,0))); c.push_back(DeformableContactConstraint(btVector3(0,1,0))); c.push_back(DeformableContactConstraint(btVector3(0,0,1))); - m_constraints[&(psb->m_nodes[j])] = c; + m_constraints.insert(psb->m_nodes[j].index, c); btAlignedObjectArray f; f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); - m_frictions[&(psb->m_nodes[j])] = f; + m_frictions.insert(psb->m_nodes[j].index, f); } } } @@ -290,22 +290,22 @@ void btDeformableContactProjection::setConstraints() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { - if (m_constraints.find(c.m_node) == m_constraints.end()) + if (m_constraints.find(c.m_node->index) == NULL) { btAlignedObjectArray constraints; constraints.push_back(DeformableContactConstraint(c)); - m_constraints[c.m_node] = constraints; + m_constraints.insert(c.m_node->index,constraints); btAlignedObjectArray frictions; frictions.push_back(DeformableFrictionConstraint()); - m_frictions[c.m_node] = frictions; + m_frictions.insert(c.m_node->index,frictions); } else { // group colinear constraints into one const btScalar angle_epsilon = 0.015192247; // less than 10 degree bool merged = false; - btAlignedObjectArray& constraints = m_constraints[c.m_node]; - btAlignedObjectArray& frictions = m_frictions[c.m_node]; + btAlignedObjectArray& constraints = *m_constraints[c.m_node->index]; + btAlignedObjectArray& frictions = *m_frictions[c.m_node->index]; for (int j = 0; j < constraints.size(); ++j) { const btAlignedObjectArray& dirs = constraints[j].m_direction; @@ -337,11 +337,11 @@ void btDeformableContactProjection::setConstraints() void btDeformableContactProjection::enforceConstraint(TVStack& x) { const int dim = 3; - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices->at(it.first); - const btAlignedObjectArray& frictions = m_frictions[it.first]; + const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + const btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -400,11 +400,11 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) void btDeformableContactProjection::project(TVStack& x) { const int dim = 3; - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices->at(it.first); - btAlignedObjectArray& frictions = m_frictions[it.first]; + const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 2ca571195..9e61fed34 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -11,14 +11,19 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "LinearMath/btHashMap.h" class btDeformableContactProjection : public btCGProjection { public: - std::unordered_map > m_constraints; - std::unordered_map > m_frictions; +// std::unordered_map > m_constraints; +// std::unordered_map > m_frictions; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) - : btCGProjection(softBodies, dt, indices) + // map from node index to constraints + btHashMap > m_constraints; + btHashMap >m_frictions; + + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) + : btCGProjection(softBodies, dt, nodes) { } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 88d9107e1..0c2ae535d 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -47,7 +47,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = m_indices->at(&n); + size_t id = n.index; btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; btVector3 scaled_force = scale * m_gravity * mass; force[id] += scaled_force; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index ff75eeb1c..6e1c54352 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -9,7 +9,8 @@ #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H #include "btSoftBody.h" -#include +#include + enum btDeformableLagrangianForceType { BT_GRAVITY_FORCE = 1, @@ -22,7 +23,7 @@ public: // using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; btAlignedObjectArray m_softBodies; - const std::unordered_map* m_indices; + const btAlignedObjectArray* m_nodes; btDeformableLagrangianForce() { @@ -57,9 +58,9 @@ public: m_softBodies.push_back(psb); } - virtual void setIndices(const std::unordered_map* indices) + virtual void setIndices(const btAlignedObjectArray* nodes) { - m_indices = indices; + m_nodes = nodes; } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index d3ccd70dc..841101c8f 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -41,8 +41,8 @@ public: const btSoftBody::Link& link = psb->m_links[j]; btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; // damping force btVector3 v_diff = (node2->m_v - node1->m_v); @@ -68,8 +68,8 @@ public: btSoftBody::Node* node2 = link.m_n[1]; btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; // elastic force // explicit elastic force @@ -94,8 +94,8 @@ public: btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node2 = link.m_n[1]; btScalar k_damp = psb->m_dampingCoefficient; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); df[id1] += local_scaled_df; df[id2] -= local_scaled_df; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 78c5f24a9..6e1ed75bd 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -45,10 +45,11 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { // perform position correction for all constraints - for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) +// for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) + for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; + btAlignedObjectArray& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); for (int i = 0; i < constraints.size(); ++i) { DeformableContactConstraint& constraint = constraints[i]; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 5e33d74d0..87633a83c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -258,6 +258,7 @@ public: btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data int m_battach : 1; // Attached + int index; }; /* Link */ ATTRIBUTE_ALIGNED16(struct) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 77bcdc5d8..eb487a56f 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -995,7 +995,9 @@ struct btSoftColliders btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - btMatrix3x3 rot(normal, t1, t2); // world frame to local frame + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; btScalar dt = psb->m_sst.sdt; btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); -- cgit v1.2.1