Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

switch from std::unordered_map to btHashMap

Browse Source
This commit is contained in:
Xuchen Han
2019-08-02 17:46:26 -07:00
parent dae230912b
commit 8c04a78c9b
13 changed files with 68 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/BulletSoftBody/btCGProjection.h
View File

@@ -10,7 +10,6 @@
#include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <unordered_map>
class btDeformableRigidDynamicsWorld;
@@ -21,6 +20,7 @@ struct DeformableContactConstraint
btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
btSoftBody::Node* node;
DeformableContactConstraint(const btSoftBody::RContact& rcontact)
{
@@ -58,7 +58,6 @@ struct DeformableContactConstraint
struct DeformableFrictionConstraint
{
btAlignedObjectArray<bool> m_static; // whether the friction is static
btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
btAlignedObjectArray<btScalar> 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<btVector3> m_accumulated_tangent_impulse;
btSoftBody::Node* node;
DeformableFrictionConstraint()
{
@@ -103,22 +103,18 @@ struct DeformableFrictionConstraint
class btCGProjection
{
public:
// static const int dim = 3;
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
// using TVStack = btAlignedObjectArray<btVector3>;
// using TVArrayStack = btAlignedObjectArray<btAlignedObjectArray<btVector3> >;
// using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
btAlignedObjectArray<btSoftBody *> m_softBodies;
btDeformableRigidDynamicsWorld* m_world;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
const btScalar& m_dt;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const btAlignedObjectArray<btSoftBody::Node*>* nodes)
: m_softBodies(softBodies)
, m_dt(dt)
, m_indices(indices)
, m_nodes(nodes)
{
}

9
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -9,7 +9,7 @@
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& 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];
}
}

13
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -29,7 +29,7 @@ public:
Preconditioner* m_preconditioner;
btDeformableContactProjection projection;
const TVStack& m_backupVelocity;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
btAlignedObjectArray<btSoftBody::Node* > m_nodes;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& 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<btSoftBody::Node *, size_t>* getIndices()
const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
{
return &m_indices;
return &m_nodes;
}
};

2
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -50,6 +50,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
m_backupVelocity.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
@@ -121,7 +122,6 @@ bool btDeformableBodySolver::updateNodes()
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
m_backupVelocity.resize(numNodes);
return true;
}
return false;

1
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -33,6 +33,7 @@ protected:
btScalar m_dt;
btConjugateGradient<btDeformableBackwardEulerObjective> cg;
public:
btDeformableBackwardEulerObjective* m_objective;

38
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -8,7 +8,7 @@
#include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
#include <algorithm>
#include <math.h>
#include <cmath>
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<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& 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<DeformableFrictionConstraint> 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<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c));
m_constraints[c.m_node] = constraints;
m_constraints.insert(c.m_node->index,constraints);
btAlignedObjectArray<DeformableFrictionConstraint> 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<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& 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<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices->at(it.first);
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
const btAlignedObjectArray<DeformableFrictionConstraint>& 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<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices->at(it.first);
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)

13
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -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<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
// std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
// std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
: btCGProjection(softBodies, dt, indices)
// map from node index to constraints
btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const btAlignedObjectArray<btSoftBody::Node*>* nodes)
: btCGProjection(softBodies, dt, nodes)
{
}

2
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -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;

9
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -9,7 +9,8 @@
#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
#include "btSoftBody.h"
#include <unordered_map>
#include <LinearMath/btHashMap.h>
enum btDeformableLagrangianForceType
{
BT_GRAVITY_FORCE = 1,
@@ -22,7 +23,7 @@ public:
// using TVStack = btAlignedObjectArray<btVector3>;
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *> m_softBodies;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btDeformableLagrangianForce()
{
@@ -57,9 +58,9 @@ public:
m_softBodies.push_back(psb);
}
virtual void setIndices(const std::unordered_map<btSoftBody::Node *, size_t>* indices)
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
{
m_indices = indices;
m_nodes = nodes;
}
};
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

12
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -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;

7
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -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<DeformableFrictionConstraint>& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
DeformableContactConstraint& constraint = constraints[i];

1
src/BulletSoftBody/btSoftBody.h
View File

@@ -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)

4
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -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();