AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 14:10:11 +00:00
Code Issues Releases Wiki Activity

bug fix in CR multiply

Browse Source
This commit is contained in:
Xuchen Han 2020-04-08 21:24:02 -07:00
parent 6469e0f181
commit 6ef1a4acbc
13 changed files with 67 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/BulletSoftBody/btConjugateResidual.h
View File

@ -35,7 +35,7 @@ public:
btConjugateResidual(const int max_it_in)
: max_iterations(max_it_in)
{
tolerance_squared = 1e-5;
tolerance_squared = 1e-2;
}
virtual ~btConjugateResidual(){}
@ -71,11 +71,6 @@ public:
for (int k = 1; k <= max_iterations; k++) {
// z = M^(-1) * Ap
A.precondition(temp_p, z);
// for (int i = 0; i < r.size(); ++i)
// {
// printf("temp_p = %f %f %f\n", temp_p[i][0], temp_p[i][1], temp_p[i][2]);
// printf("z = %f %f %f\n", z[i][0], z[i][1], z[i][2]);
// }
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
btScalar alpha = r_dot_Ar / dot(temp_p, z);
// x += alpha * p;

14
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -86,33 +86,27 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
b[i].setZero();
}
// add in the lagrange multiplier terms
// C * \lambda
for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
{
// C * lambda
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i)
{
for (int j = 0; j < lm.m_num_constraints; ++j)
{
b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j];
assert(x[offset+c][j] == x[offset+c][j]);
assert(b[lm.m_indices[i]] == b[lm.m_indices[i]]);
}
}
// C^T * x
for (int d = 0; d < lm.m_num_constraints; ++d)
{
for (int i = 0; i < lm.m_num_nodes; ++i)
{
b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
assert(b[offset+c][d] == b[offset+c][d]);
}
}
}
for (int i = 0; i < b.size(); ++i)
{
assert(b[i] == b[i]);
}
}
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
@ -167,7 +161,7 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
m_lf[i]->addScaledDampingForce(dt, residual);
}
}
m_projection.project(residual);
// m_projection.project(residual);
}
btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const

24
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -141,7 +141,29 @@ public:
int offset = vec.size();
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
{
extended_vec[offset + i] = btVector3(0,0,0);
extended_vec[offset + i].setZero();
}
}
void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
{
extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
for (int i = 0; i < residual.size(); ++i)
{
extended_residual[i] = residual[i];
}
int offset = residual.size();
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
{
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
extended_residual[offset + i].setZero();
for (int d = 0; d < lm.m_num_constraints; ++d)
{
for (int n = 0; n < lm.m_num_nodes; ++n)
{
extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
}
}
}
}
};

16
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -18,7 +18,7 @@
#include "btDeformableBodySolver.h"
#include "btSoftBodyInternals.h"
#include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 50;
static const int kMaxConjugateGradientIterations = 10;
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(kMaxConjugateGradientIterations)
@ -44,13 +44,15 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
m_objective->applyDynamicFriction(m_residual);
// computeStep(m_dv, m_residual);
TVStack rhs, x;
m_objective->addLagrangeMultiplier(m_residual, rhs);
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
m_objective->addLagrangeMultiplier(m_dv, x);
computeStep(x, rhs);
for (int i = 0; i<m_dv.size(); ++i)
{
m_dv[i] = x[i];
}
m_objective->m_projection.checkConstraints(x);
updateVelocity();
}
else
@ -209,7 +211,7 @@ void btDeformableBodySolver::updateDv(btScalar scale)
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
m_cr.solve(*m_objective, ddv, residual);
m_cr.solve(*m_objective, ddv, residual, true);
}
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@ -251,12 +253,6 @@ btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** def
return maxSquaredResidual;
}
btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveSplitImpulse");
return m_objective->m_projection.solveSplitImpulse(infoGlobal);
}
void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
m_objective->m_projection.splitImpulseSetup(infoGlobal);
@ -344,7 +340,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
}
else
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
}
}

8
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -63,15 +63,11 @@ public:
// update soft body normals
virtual void updateSoftBodies();
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// solve the contact between deformable and rigid as well as among deformables
btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// set up the position error in split impulse
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);

58
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -289,37 +289,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
}
return residualSquare;
}
btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
const btScalar dn = m_penetration;
if (dn != 0)
{
const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
// one iteration of the position impulse corrects all the position error at this timestep
m_penetration -= dn;
// apply impulse to deformable nodes involved and change their position
applySplitImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their position
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// todo xuchenhan@
}
return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
}
return 0;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
@ -351,13 +320,6 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_vsplit -= dv;
};
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal)
: m_face(contact.m_face)
@ -469,26 +431,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
// v2 += dv2;
}
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_vsplit;
btVector3& v1 = face->m_n[1]->m_vsplit;
btVector3& v2 = face->m_n[2]->m_vsplit;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)

38
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -52,9 +52,6 @@ public:
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
// solve the position error by applying an inelastic impulse that changes only the position (not velocity)
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
// get the velocity of the object A in the contact
virtual btVector3 getVa() const = 0;
@ -67,9 +64,6 @@ public:
// apply impulse to the soft body node and/or face involved
virtual void applyImpulse(const btVector3& impulse) = 0;
// apply position based impulse to the soft body node and/or face involved
virtual void applySplitImpulse(const btVector3& impulse) = 0;
// scale the penetration depth by erp
virtual void setPenetrationScale(btScalar scale) = 0;
};
@ -97,11 +91,6 @@ public:
{
return 0;
}
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
return 0;
}
virtual btVector3 getVa() const
{
@ -119,7 +108,6 @@ public:
}
virtual void applyImpulse(const btVector3& impulse){}
virtual void applySplitImpulse(const btVector3& impulse){}
virtual void setPenetrationScale(btScalar scale){}
};
@ -137,11 +125,7 @@ public:
{
}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo xuchenhan@
return 0;
}
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the velocity of the deformable node in contact
@ -151,10 +135,7 @@ public:
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
};
virtual void setPenetrationScale(btScalar scale){}
};
@ -181,8 +162,6 @@ public:
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)
{
m_penetration *= scale;
@ -217,7 +196,6 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@ -246,7 +224,6 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@ -266,12 +243,6 @@ public:
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo: xuchenhan@
return 0;
}
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
@ -288,10 +259,7 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
}
virtual void setPenetrationScale(btScalar scale){}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

52
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -77,37 +77,6 @@ void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo&
}
}
btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
// node constraints
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// anchor constraints
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("setConstraints");
@ -342,6 +311,26 @@ void btDeformableContactProjection::setProjection()
m_projections = mgs.m_out;
}
void btDeformableContactProjection::checkConstraints(const TVStack& x)
{
for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
{
btVector3 d(0,0,0);
const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
for (int j = 0; j < lm.m_num_constraints; ++j)
{
for (int k = 0; k < lm.m_num_nodes; ++k)
{
d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
}
}
// if (d.safeNorm() > 1e-5)
// {
// exit(1);
// }
}
}
void btDeformableContactProjection::setLagrangeMultiplier()
{
for (int i = 0; i < m_softBodies.size(); ++i)
@ -426,6 +415,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
lm.m_num_constraints = 1;
lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
}
m_lagrangeMultipliers.push_back(lm);
}
}
}

5
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -79,9 +79,6 @@ public:
// update and solve the constraints
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
@ -93,5 +90,7 @@ public:
virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier();
void checkConstraints(const TVStack& x);
};
#endif /* btDeformableContactProjection_h */

2
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -330,8 +330,6 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_v -= node.m_vsplit;
node.m_vsplit.setZero();
node.m_q = node.m_x;
node.m_vn = node.m_v;
}

4
src/BulletSoftBody/btPreconditioner.h
View File

@ -73,6 +73,10 @@ public:
{
b[i] = x[i] * m_inv_mass[i];
}
for (int i = m_inv_mass.size(); i < b.size(); ++i)
{
b[i] = x[i] / (240*240);
}
}
};

2
src/BulletSoftBody/btSoftBody.cpp
View File

@ -4043,7 +4043,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
docollideNode.m_rigidBody = prb1;
docollideNode.dynmargin = basemargin + timemargin;
docollideNode.stamargin = basemargin;
m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
// m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
{

1
src/BulletSoftBody/btSoftBody.h
View File

@ -262,7 +262,6 @@ public:
btVector3 m_x; // Position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal