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 "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <unordered_map>
class btDeformableRigidDynamicsWorld; class btDeformableRigidDynamicsWorld;
@@ -21,6 +20,7 @@ struct DeformableContactConstraint
btAlignedObjectArray<btScalar> m_value; btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve // 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; btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
btSoftBody::Node* node;
DeformableContactConstraint(const btSoftBody::RContact& rcontact) DeformableContactConstraint(const btSoftBody::RContact& rcontact)
{ {
@@ -58,7 +58,6 @@ struct DeformableContactConstraint
struct DeformableFrictionConstraint struct DeformableFrictionConstraint
{ {
btAlignedObjectArray<bool> m_static; // whether the friction is static btAlignedObjectArray<bool> m_static; // whether the friction is static
btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
btAlignedObjectArray<btScalar> m_dv; // the dv magnitude of the node 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 // the total impulse the node applied to the rb in the tangential direction in the cg solve
btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse; btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
btSoftBody::Node* node;
DeformableFrictionConstraint() DeformableFrictionConstraint()
{ {
@@ -103,22 +103,18 @@ struct DeformableFrictionConstraint
class btCGProjection class btCGProjection
{ {
public: public:
// static const int dim = 3;
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
// using TVStack = btAlignedObjectArray<btVector3>;
// using TVArrayStack = btAlignedObjectArray<btAlignedObjectArray<btVector3> >;
// using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
btAlignedObjectArray<btSoftBody *> m_softBodies; btAlignedObjectArray<btSoftBody *> m_softBodies;
btDeformableRigidDynamicsWorld* m_world; btDeformableRigidDynamicsWorld* m_world;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices; const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
const btScalar& m_dt; 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_softBodies(softBodies)
, m_dt(dt) , 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) btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: m_softBodies(softBodies) : m_softBodies(softBodies)
, projection(m_softBodies, m_dt, &m_indices) , projection(m_softBodies, m_dt, &m_nodes)
, m_backupVelocity(backup_v) , m_backupVelocity(backup_v)
{ {
m_preconditioner = new DefaultPreconditioner(); m_preconditioner = new DefaultPreconditioner();
@@ -63,10 +63,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
{ {
// only the velocity of the constrained nodes needs to be updated during CG solve // 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]; int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
it.first->m_v = m_backupVelocity[i] + dv[i]; 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; Preconditioner* m_preconditioner;
btDeformableContactProjection projection; btDeformableContactProjection projection;
const TVStack& m_backupVelocity; 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); btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
@@ -97,20 +97,23 @@ public:
virtual void updateId() virtual void updateId()
{ {
size_t index = 0; size_t id = 0;
m_nodes.clear();
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) 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_dv.resize(m_numNodes);
m_residual.resize(m_numNodes); m_residual.resize(m_numNodes);
m_backupVelocity.resize(m_numNodes);
} }
for (int i = 0; i < m_dv.size(); ++i) for (int i = 0; i < m_dv.size(); ++i)
@@ -121,7 +122,6 @@ bool btDeformableBodySolver::updateNodes()
if (numNodes != m_numNodes) if (numNodes != m_numNodes)
{ {
m_numNodes = numNodes; m_numNodes = numNodes;
m_backupVelocity.resize(numNodes);
return true; return true;
} }
return false; return false;

1
src/BulletSoftBody/btDeformableBodySolver.h
View File

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

38
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -8,7 +8,7 @@
#include "btDeformableContactProjection.h" #include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h" #include "btDeformableRigidDynamicsWorld.h"
#include <algorithm> #include <algorithm>
#include <math.h> #include <cmath>
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData, btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point, const btVector3& contact_point,
@@ -49,10 +49,10 @@ void btDeformableContactProjection::update()
m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values // 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<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i) for (int i = 0; i < constraints.size(); ++i)
{ {
DeformableContactConstraint& constraint = constraints[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(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0))); c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1))); 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; btAlignedObjectArray<DeformableFrictionConstraint> f;
f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint()); 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); const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON) 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; btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c)); constraints.push_back(DeformableContactConstraint(c));
m_constraints[c.m_node] = constraints; m_constraints.insert(c.m_node->index,constraints);
btAlignedObjectArray<DeformableFrictionConstraint> frictions; btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint()); frictions.push_back(DeformableFrictionConstraint());
m_frictions[c.m_node] = frictions; m_frictions.insert(c.m_node->index,frictions);
} }
else else
{ {
// group colinear constraints into one // group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false; bool merged = false;
btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node]; btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node]; btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
for (int j = 0; j < constraints.size(); ++j) for (int j = 0; j < constraints.size(); ++j)
{ {
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction; const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@@ -337,11 +337,11 @@ void btDeformableContactProjection::setConstraints()
void btDeformableContactProjection::enforceConstraint(TVStack& x) void btDeformableContactProjection::enforceConstraint(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (auto& it : m_constraints) for (int index = 0; index < m_constraints.size(); ++index)
{ {
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
size_t i = m_indices->at(it.first); size_t i = m_constraints.getKeyAtIndex(index).getUid1();
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first]; const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim); btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0); btAssert(constraints.size() > 0);
if (constraints.size() == 1) if (constraints.size() == 1)
@@ -400,11 +400,11 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
void btDeformableContactProjection::project(TVStack& x) void btDeformableContactProjection::project(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (auto& it : m_constraints) for (int index = 0; index < m_constraints.size(); ++index)
{ {
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
size_t i = m_indices->at(it.first); size_t i = m_constraints.getKeyAtIndex(index).getUid1();
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first]; btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim); btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0); btAssert(constraints.size() > 0);
if (constraints.size() == 1) if (constraints.size() == 1)

13
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -11,14 +11,19 @@
#include "btSoftBody.h" #include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "LinearMath/btHashMap.h"
class btDeformableContactProjection : public btCGProjection class btDeformableContactProjection : public btCGProjection
{ {
public: public:
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints; // std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions; // 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) // map from node index to constraints
: btCGProjection(softBodies, dt, indices) 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) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
btSoftBody::Node& n = psb->m_nodes[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; btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
btVector3 scaled_force = scale * m_gravity * mass; btVector3 scaled_force = scale * m_gravity * mass;
force[id] += scaled_force; force[id] += scaled_force;

9
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -9,7 +9,8 @@
#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
#include "btSoftBody.h" #include "btSoftBody.h"
#include <unordered_map> #include <LinearMath/btHashMap.h>
enum btDeformableLagrangianForceType enum btDeformableLagrangianForceType
{ {
BT_GRAVITY_FORCE = 1, BT_GRAVITY_FORCE = 1,
@@ -22,7 +23,7 @@ public:
// using TVStack = btAlignedObjectArray<btVector3>; // using TVStack = btAlignedObjectArray<btVector3>;
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *> m_softBodies; btAlignedObjectArray<btSoftBody *> m_softBodies;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices; const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btDeformableLagrangianForce() btDeformableLagrangianForce()
{ {
@@ -57,9 +58,9 @@ public:
m_softBodies.push_back(psb); 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 */ #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]; const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1]; btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = m_indices->at(node1); size_t id1 = node1->index;
size_t id2 = m_indices->at(node2); size_t id2 = node2->index;
// damping force // damping force
btVector3 v_diff = (node2->m_v - node1->m_v); btVector3 v_diff = (node2->m_v - node1->m_v);
@@ -68,8 +68,8 @@ public:
btSoftBody::Node* node2 = link.m_n[1]; btSoftBody::Node* node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST; btScalar kLST = link.Feature::m_material->m_kLST;
btScalar r = link.m_rl; btScalar r = link.m_rl;
size_t id1 = m_indices->at(node1); size_t id1 = node1->index;
size_t id2 = m_indices->at(node2); size_t id2 = node2->index;
// elastic force // elastic force
// explicit elastic force // explicit elastic force
@@ -94,8 +94,8 @@ public:
btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1]; btSoftBody::Node* node2 = link.m_n[1];
btScalar k_damp = psb->m_dampingCoefficient; btScalar k_damp = psb->m_dampingCoefficient;
size_t id1 = m_indices->at(node1); size_t id1 = node1->index;
size_t id2 = m_indices->at(node2); size_t id2 = node2->index;
btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
df[id1] += local_scaled_df; df[id1] += local_scaled_df;
df[id2] -= 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) void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
{ {
// perform position correction for all constraints // 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<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i) for (int i = 0; i < constraints.size(); ++i)
{ {
DeformableContactConstraint& constraint = constraints[i]; DeformableContactConstraint& constraint = constraints[i];

1
src/BulletSoftBody/btSoftBody.h
View File

@@ -258,6 +258,7 @@ public:
btScalar m_area; // Area btScalar m_area; // Area
btDbvtNode* m_leaf; // Leaf data btDbvtNode* m_leaf; // Leaf data
int m_battach : 1; // Attached int m_battach : 1; // Attached
int index;
}; };
/* Link */ /* Link */
ATTRIBUTE_ALIGNED16(struct) 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_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.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; const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btScalar dt = psb->m_sst.sdt; 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(); 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();