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

finished refactoring; start adding face contact

Browse Source
This commit is contained in:
Xuchen Han
2019-08-30 14:16:56 -07:00
parent f813cb1c88
commit f99cf56149
16 changed files with 937 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/CMakeLists.txt
View File

@@ -22,6 +22,7 @@ SET(BulletSoftBody_SRCS
btDeformableMultiBodyConstraintSolver.cpp
btDeformableContactProjection.cpp
btDeformableMultiBodyDynamicsWorld.cpp
btDeformableContactConstraint.cpp
)
@@ -54,6 +55,7 @@ SET(BulletSoftBody_HDRS
btDeformableMultiBodyConstraintSolver.h
btDeformableContactProjection.h
btDeformableMultiBodyDynamicsWorld.h
btDeformableContactConstraint.h
btSoftBodySolverVertexBuffer.h
)

40
src/BulletSoftBody/btCGProjection.h
View File

@@ -20,8 +20,6 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
//class btDeformableMultiBodyDynamicsWorld;
struct DeformableContactConstraint
{
const btSoftBody::Node* m_node;
@@ -64,6 +62,37 @@ struct DeformableContactConstraint
{
}
};
//
//
//struct DeformableFaceContactConstraint
//{
// const btSoftBody::Face* m_face;
// const btSoftBody::FaceRContact* m_contact;
// btVector3 m_total_normal_dv;
// btVector3 m_total_tangent_dv;
// bool m_static;
// bool m_can_be_dynamic;
//
// DeformableFaceContactConstraint(const btSoftBody::FaceRContact& rcontact)
// : m_face(rcontact.m_face),
// m_contact(&rcontact),
// m_total_normal_dv(0,0,0),
// m_total_tangent_dv(0,0,0),
// m_static(false),
// m_can_be_dynamic(true)
// {
// }
//
// void replace(const btSoftBody::FaceRContact& rcontact)
// {
// m_contact = &rcontact;
// m_face = rcontact.m_face;
// }
//
// ~DeformableFaceContactConstraint()
// {
// }
//};
class btCGProjection
{
@@ -73,6 +102,8 @@ public:
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
const btScalar& m_dt;
// map from node indices to node pointers
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies)
@@ -95,6 +126,11 @@ public:
virtual void reinitialize(bool nodeUpdated)
{
}
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
{
m_nodes = nodes;
}
};

2
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -51,7 +51,7 @@ public:
A.project(z);
btScalar r_dot_z = dot(z,r);
btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance);
if (std::sqrt(r_dot_z) < local_tolerance) {
if (std::sqrt(r_dot_z) <= local_tolerance) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;

19
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -47,6 +47,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
m_lf[i]->reinitialize(nodeUpdated);
}
projection.reinitialize(nodeUpdated);
projection.setIndices(getIndices());
m_preconditioner->reinitialize(nodeUpdated);
}
@@ -84,11 +85,21 @@ 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 contact solve
for (int i = 0; i < projection.m_constraints.size(); ++i)
// // only the velocity of the constrained nodes needs to be updated during contact solve
// for (int i = 0; i < projection.m_constraints.size(); ++i)
// {
// int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
// m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
// }
for (int i = 0; i < m_softBodies.size(); ++i)
{
int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
node.m_v = m_backupVelocity[node.index] + dv[node.index];
}
}
}

12
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -101,16 +101,22 @@ public:
virtual void updateId()
{
size_t id = 0;
size_t node_id = 0;
size_t face_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)
{
psb->m_nodes[j].index = id;
psb->m_nodes[j].index = node_id;
m_nodes.push_back(&psb->m_nodes[j]);
++id;
++node_id;
}
for (int j = 0; j < psb->m_faces.size(); ++j)
{
psb->m_faces[j].m_index = face_id;
++face_id;
}
}
}

55
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -16,6 +16,7 @@
#include <stdio.h>
#include <limits>
#include "btDeformableBodySolver.h"
#include "btSoftBodyInternals.h"
#include "LinearMath/btQuickprof.h"
btDeformableBodySolver::btDeformableBodySolver()
@@ -23,11 +24,7 @@ btDeformableBodySolver::btDeformableBodySolver()
, m_cg(20)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
//, m_lineSearch(false)
//, m_cg(10)
//, m_maxNewtonIterations(5)
//, m_newtonTolerance(1e-3)
, m_lineSearch(true)
, m_lineSearch(false)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@@ -155,7 +152,7 @@ void btDeformableBodySolver::updateEnergy(btScalar scale)
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual)
{
btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
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);
@@ -199,7 +196,7 @@ void btDeformableBodySolver::updateDv(btScalar scale)
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
//btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
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);
}
@@ -237,8 +234,18 @@ 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);
// std::cout << "=================" << std::endl;
// for (int i = 0; i < m_dv.size(); ++i)
// {
// std::cout << m_dv[i].getX() << " " << m_dv[i].getY() << " " << m_dv[i].getZ() << std::endl;
// }
m_objective->updateVelocity(m_dv);
return maxSquaredResidual;
}
@@ -302,7 +309,7 @@ void btDeformableBodySolver::backupVn()
// Here:
// dv = 0 for nodes not in constraints
// dv = v_{n+1} - v_{n+1}^* for nodes in constraints
if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL)
if (m_objective->projection.m_projectionsDict.find(psb->m_nodes[j].index)!=NULL)
{
m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
}
@@ -361,6 +368,18 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
{
int i, ni;
/* Update */
if (psb->m_bUpdateRtCst)
{
psb->m_bUpdateRtCst = false;
psb->updateConstants();
psb->m_fdbvt.clear();
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RDF)
{
psb->initializeFaceTree();
}
}
/* Prepare */
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
@@ -388,11 +407,29 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
psb->m_sst.updmrg);
}
if (!psb->m_fdbvt.empty())
{
for (int i = 0; i < psb->m_faces.size(); ++i)
{
btSoftBody::Face& f = psb->m_faces[i];
const btVector3 v = (f.m_n[0]->m_v +
f.m_n[1]->m_v +
f.m_n[2]->m_v) /
3;
vol = VolumeOf(f, psb->m_sst.radmrg);
psb->m_fdbvt.update(f.m_leaf,
vol,
v * psb->m_sst.velmrg,
psb->m_sst.updmrg);
}
}
/* Clear contacts */
psb->m_rcontacts.resize(0);
psb->m_nodeRigidContacts.resize(0);
psb->m_faceRigidContacts.resize(0);
psb->m_scontacts.resize(0);
/* Optimize dbvt's */
psb->m_ndbvt.optimizeIncremental(1);
psb->m_fdbvt.optimizeIncremental(1);
}
void btDeformableBodySolver::updateSoftBodies()

190
src/BulletSoftBody/btDeformableContactConstraint.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btDeformableContactConstraint.h"
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
: m_contact(&c)
, btDeformableContactConstraint(c.m_cti.m_normal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
}
btVector3 btDeformableRigidContactConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += m_contact->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += m_contact->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableRigidContactConstraint::solveConstraint()
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
// 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;
const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
if (m_total_normal_dv.dot(cti.m_normal) < 0)
{
// separating in the normal direction
m_static = false;
m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.norm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3;
}
impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
}
else
{
// static friction
m_static = true;
}
}
impulse = impulse_normal + impulse_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
}
}
}
return residualSquare;
}
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
: m_node(contact.m_node)
, btDeformableRigidContactConstraint(contact)
{
}
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
: m_node(other.m_node)
, btDeformableRigidContactConstraint(other)
{
}
btVector3 btDeformableNodeRigidContactConstraint::getVb() const
{
return m_node->m_v;
}
btVector3 btDeformableNodeRigidContactConstraint::getDv(btSoftBody::Node* node) const
{
return m_total_normal_dv + m_total_tangent_dv;
}

150
src/BulletSoftBody/btDeformableContactConstraint.h Normal file
View File

@@ -0,0 +1,150 @@
/*
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DEFORMABLE_CONTACT_CONSTRAINT_H
#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H
#include "btSoftBody.h"
class btDeformableContactConstraint
{
public:
// True if the friction is static
// False if the friction is dynamic
bool m_static;
// normal of the contact
btVector3 m_normal;
btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
{
}
btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
{
}
btDeformableContactConstraint(const btDeformableContactConstraint& other)
: m_static(other.m_static)
, m_normal(other.m_normal)
{
}
btDeformableContactConstraint(){}
virtual ~btDeformableContactConstraint(){}
// solve the constraint with inelastic impulse and return the error, which is the square of normal component of dt scaled velocity diffrerence
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse.
// if the object is rigid/multibody apply the impulse to change the velocity,
// if the object is deformable node, change the according dv.
virtual btScalar solveConstraint() = 0;
// get the velocity of the object A in the contact
virtual btVector3 getVa() const = 0;
// get the velocity of the object B in the contact
virtual btVector3 getVb() const = 0;
// get the velocity change of the soft body node in the constraint
virtual btVector3 getDv(btSoftBody::Node*) const = 0;
};
class btDeformableStaticConstraint : public btDeformableContactConstraint
{
public:
const btSoftBody::Node* m_node;
btDeformableStaticConstraint(){}
btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
{
}
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
: m_node(other.m_node)
, btDeformableContactConstraint(other)
{
}
virtual ~btDeformableStaticConstraint(){}
virtual btScalar solveConstraint()
{
return 0;
}
virtual btVector3 getVa() const
{
return btVector3(0,0,0);
}
virtual btVector3 getVb() const
{
return btVector3(0,0,0);
}
virtual btVector3 getDv(btSoftBody::Node* n) const
{
return btVector3(0,0,0);
}
};
class btDeformableRigidContactConstraint : public btDeformableContactConstraint
{
public:
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(){}
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
virtual ~btDeformableRigidContactConstraint()
{
}
// object A is the rigid/multi body, and object B is the deformable node
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
};
class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
{
public:
const btSoftBody::Node* m_node;
btDeformableNodeRigidContactConstraint(){}
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
virtual ~btDeformableNodeRigidContactConstraint()
{
}
virtual btVector3 getVb() const;
virtual btVector3 getDv(btSoftBody::Node*) const;
const btSoftBody::DeformableNodeRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

416
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -20,148 +20,33 @@
btScalar btDeformableContactProjection::update()
{
btScalar residualSquare = 0;
btScalar max_impulse = 0;
// loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index)
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
for (int j = 0; j < constraint.m_contact.size(); ++j)
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
if (constraint.m_contact[j] == NULL)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel * m_dt;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += c->t2 * vel * m_dt;
}
}
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j];
constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im;
constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im;
if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0)
{
// separating in the normal direction
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = false;
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = true;
if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON)
{
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
}
else
{
constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3;
}
impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv);
}
else
{
// static friction
constraint.m_static[j] = true;
constraint.m_can_be_dynamic[j] = false;
}
}
impulse = impulse_normal + impulse_tangent;
max_impulse = btMax(impulse.length2(), max_impulse);
// dn is the normal component of velocity diffrerence. Approximates the residual.
residualSquare = btMax(residualSquare, dn*dn);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
{
rigidCol->applyImpulse(impulse, c->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2));
}
}
}
}
btScalar localResidualSquare = constraints[i].solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// face constraints
// for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
// {
// btAlignedObjectArray<btDeformableFaceRigidContactConstraint>& constraints = *m_faceRigidConstraints.getAtIndex(index);
// for (int i = 0; i < constraints.size(); ++i)
// {
// btScalar localResidualSquare = constraints[i].solveConstraint();
// residualSquare = btMax(residualSquare, localResidualSquare);
// }
// }
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
{
BT_PROFILE("setConstraints");
@@ -173,132 +58,98 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint);
}
}
}
// set Deformable Node vs. Rigid constraint
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_complementary;
for (int j = 0; j < psb->m_rcontacts.size(); ++j)
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
// skip fixed points
if (contact.m_node->m_im == 0)
{
continue;
}
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
btDeformableNodeRigidContactConstraint constraint(contact);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList;
constraintsList.push_back(constraint);
m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
else
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btScalar vel = 0.0;
const btScalar* jac = &c.jacobianData_normal.m_jacobians[0];
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * m_dt;
}
}
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
if (m_constraints.find(c.m_node->index) == NULL)
{
m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
}
else
{
DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
bool single_contact = true;
if (single_contact)
{
if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset)
{
constraints.replace(c);
}
}
else
{
constraints.append(c);
}
}
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
constraintsList.push_back(constraint);
}
}
}
}
// todo xuchenhan@: set Deformable Face vs. Rigid constraint
// todo xuchenhan@: set Deformable Face vs. Deformable Node
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
for (int index = 0; index < m_constraints.size(); ++index)
for (int i = 0; i < x.size(); ++i)
{
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
x[i].setZero();
for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
if (m_staticConstraints.find(i) != NULL)
{
x[i] += constraints.m_total_normal_dv[j];
x[i] += constraints.m_total_tangent_dv[j];
// if a node is fixed, dv = 0
continue;
}
if (m_nodeRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& 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));
}
}
// todo xuchenhan@
// if (m_faceRigidConstraints.find(i) != NULL)
// {
//
// }
}
}
void btDeformableContactProjection::project(TVStack& x)
{
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
for (int index = 0; index < m_projectionsDict.size(); ++index)
{
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
if (constraints.m_contact[0] == NULL)
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim)
{
// static node
x[i].setZero();
continue;
}
bool has_static = false;
for (int j = 0; j < constraints.m_static.size(); ++j)
else if (projectionDirs.size() == 2)
{
has_static = has_static || constraints.m_static[j];
}
// static friction => fully constrained
if (has_static)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() >= dim)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() == 2)
{
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0);
btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.norm() < SIMD_EPSILON)
{
@@ -313,48 +164,129 @@ void btDeformableContactProjection::project(TVStack& x)
}
else
{
btAssert(constraints.m_total_normal_dv.size() == 1);
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
void btDeformableContactProjection::setProjection()
{
for (int index = 0; index < m_constraints.size(); ++index)
for (int i = 0; i < m_softBodies.size(); ++i)
{
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;
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
int index = psb->m_nodes[j].index;
bool hasConstraint = false;
bool existStaticConstraint = false;
btVector3 averagedNormal(0,0,0);
btAlignedObjectArray<btVector3> normals;
if (m_staticConstraints.find(index) != NULL)
{
existStaticConstraint = true;
hasConstraint = true;
}
// accumulate normals
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k].m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k].m_normal;
// add another projection direction if it deviates from the average by more than about 15 degrees
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// 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])
// todo: xuchenhan@ implement face
// if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
// {
// }
// build projections
if (!hasConstraint)
{
has_static_constraint = true;
break;
continue;
}
}
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)
btAlignedObjectArray<btVector3> projections;
if (existStaticConstraint)
{
f[i] += friction_force;
projections.push_back(btVector3(1,0,0));
projections.push_back(btVector3(0,1,0));
projections.push_back(btVector3(0,0,1));
}
else
{
bool averageExists = (averagedNormal.length2() > SIMD_EPSILON);
averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0);
if (averageExists)
{
projections.push_back(averagedNormal);
}
for (int k = 0; k < normals.size(); ++k)
{
const btVector3& local_normal = normals[k];
// add another projection direction if it deviates from the average by more than about 15 degrees
if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25)
{
projections.push_back(local_normal);
}
}
}
m_projectionsDict.insert(index, projections);
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
// 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)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_staticConstraints.clear();
m_nodeRigidConstraints.clear();
// m_faceRigidConstraints.clear();
}

17
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -19,12 +19,20 @@
#include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "btDeformableContactConstraint.h"
#include "LinearMath/btHashMap.h"
class btDeformableContactProjection : public btCGProjection
{
public:
// map from node index to constraints
btHashMap<btHashInt, DeformableContactConstraint> m_constraints;
// map from node index to static constraint
btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// map from node index to node rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt)
@@ -37,6 +45,7 @@ public:
// apply the constraints to the rhs
virtual void project(TVStack& x);
// add to friction
virtual void applyDynamicFriction(TVStack& f);
@@ -46,8 +55,12 @@ public:
// update the constraints
virtual btScalar update();
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints();
// Set up projections for each vertex by adding the projection direction to
virtual void setProjection();
virtual void reinitialize(bool nodeUpdated);
};
#endif /* btDeformableContactProjection_h */

4
src/BulletSoftBody/btDeformableCorotatedForce.h
View File

@@ -19,7 +19,7 @@
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btPolarDecomposition.h"
static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
{
static const btPolarDecomposition polar;
return polar.decompose(m, q, s);
@@ -100,7 +100,7 @@ public:
if (J < 1024 * SIMD_EPSILON)
R.setIdentity();
else
PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
P += (F-R) * 2 * m_mu;
}

1
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -23,6 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
m_maxOverrideNumSolverIterations = 150;
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{

151
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -70,78 +70,82 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
// ///////////////////////////////
}
//void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
//{
// // perform position correction for all constraints
// BT_PROFILE("positionCorrection");
// for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
// {
// DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
// for (int j = 0; j < constraint.m_contact.size(); ++j)
// {
// const btSoftBody::RContact* c = constraint.m_contact[j];
// // skip anchor points
// if (c == NULL || c->m_node->m_im == 0)
// continue;
// const btSoftBody::sCti& cti = c->m_cti;
// btVector3 va(0, 0, 0);
//
// // grab the velocity of the rigid body
// if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
// {
// btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
// va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
// }
// else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
// {
// btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
// if (multibodyLinkCol)
// {
// const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
// const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
// const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
// const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
// const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
// // add in the normal component of the va
// btScalar vel = 0.0;
// for (int k = 0; k < ndof; ++k)
// {
// vel += local_v[k] * J_n[k];
// }
// va = cti.m_normal * vel;
//
// vel = 0.0;
// for (int k = 0; k < ndof; ++k)
// {
// vel += local_v[k] * J_t1[k];
// }
// va += c->t1 * vel;
// vel = 0.0;
// for (int k = 0; k < ndof; ++k)
// {
// vel += local_v[k] * J_t2[k];
// }
// va += c->t2 * vel;
// }
// }
// else
// {
// // The object interacting with deformable node is not supported for position correction
// btAssert(false);
// }
//
// if (cti.m_colObj->hasContactResponse())
// {
// btScalar dp = cti.m_offset;
//
// // only perform position correction when penetrating
// if (dp < 0)
// {
// c->m_node->m_v -= dp * cti.m_normal / timeStep;
// }
// }
// }
// }
//}
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
// perform position correction for all constraints
BT_PROFILE("positionCorrection");
for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
{
DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
const btSoftBody::RContact* c = constraint.m_contact[j];
// skip anchor points
if (c == NULL || c->m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c->m_cti;
btVector3 va(0, 0, 0);
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel;
}
}
else
{
// The object interacting with deformable node is not supported for position correction
btAssert(false);
}
if (cti.m_colObj->hasContactResponse())
{
btScalar dp = cti.m_offset;
// only perform position correction when penetrating
if (dp < 0)
{
c->m_node->m_v -= dp * cti.m_normal / timeStep;
}
}
}
}
}
@@ -187,7 +191,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
// 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}^*
@@ -321,11 +325,6 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
m_internalTime += timeStep;
m_deformableBodySolver->setImplicit(m_implicit);
m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
// if (m_implicit)
// {
// // todo: backup v_n velocity somewhere else
// m_deformableBodySolver->backupVelocity();
// }
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;

86
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -20,6 +20,9 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include <iostream>
//
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -1858,7 +1861,8 @@ void btSoftBody::predictMotion(btScalar dt)
}
}
/* Clear contacts */
m_rcontacts.resize(0);
m_nodeRigidContacts.resize(0);
m_faceRigidContacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
@@ -2289,6 +2293,7 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
}
return (false);
}
//
bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
const btVector3& x,
@@ -2321,6 +2326,63 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
return (false);
}
//
// Compute barycentric coordinates (u, v, w) for
// point p with respect to triangle (a, b, c)
static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
{
btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
btScalar d00 = v0.dot(v0);
btScalar d01 = v0.dot(v1);
btScalar d11 = v1.dot(v1);
btScalar d20 = v2.dot(v0);
btScalar d21 = v2.dot(v1);
btScalar denom = d00 * d11 - d01 * d01;
bary.setY((d11 * d20 - d01 * d21) / denom);
bary.setZ((d00 * d21 - d01 * d20) / denom);
bary.setX(btScalar(1) - bary.getY() - bary.getZ());
}
//
bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
const Face& f,
btVector3& contact_point,
btVector3& bary,
btScalar margin,
btSoftBody::sCti& cti, bool predict) const
{
btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape();
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
btScalar dst = results.distance;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
if (!predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();
// cti.m_normal = wtr.getBasis() * results.normal;
cti.m_normal = results.normal;
cti.m_offset = dst;
}
if (dst < 0)
return true;
return (false);
}
//
void btSoftBody::updateNormals()
{
@@ -3339,7 +3401,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
break;
case fCollision::SDF_RD:
{
btSoftColliders::CollideSDF_RD docollide;
btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
btTransform wtr = pcoWrap->getWorldTransform();
@@ -3355,13 +3417,21 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
maxs);
volume = btDbvtVolume::FromMM(mins, maxs);
volume.Expand(btVector3(basemargin, basemargin, basemargin));
docollide.psb = this;
docollide.m_colObj1Wrap = pcoWrap;
docollide.m_rigidBody = prb1;
btSoftColliders::CollideSDF_RD docollideNode;
docollideNode.psb = this;
docollideNode.m_colObj1Wrap = pcoWrap;
docollideNode.m_rigidBody = prb1;
docollideNode.dynmargin = basemargin + timemargin;
docollideNode.stamargin = basemargin;
m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
docollide.dynmargin = basemargin + timemargin;
docollide.stamargin = basemargin;
m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
btSoftColliders::CollideSDF_RDF docollideFace;
docollideFace.psb = this;
docollideFace.m_colObj1Wrap = pcoWrap;
docollideFace.m_rigidBody = prb1;
docollideFace.dynmargin = basemargin + timemargin;
docollideFace.stamargin = basemargin;
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
}
break;
}

40
src/BulletSoftBody/btSoftBody.h
View File

@@ -161,6 +161,7 @@ public:
SDF_RS = 0x0001, ///SDF based rigid vs soft
CL_RS = 0x0002, ///Cluster vs convex rigid vs soft
SDF_RD = 0x0003, ///DF based rigid vs deformable
SDF_RDF = 0x0004, ///DF based rigid vs deformable faces
SVSmask = 0x0030, ///Rigid versus soft mask
VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
@@ -217,6 +218,7 @@ public:
const btCollisionObject* m_colObj; /* Rigid body */
btVector3 m_normal; /* Outward normal */
btScalar m_offset; /* Offset from origin */
btVector3 m_bary; /* Barycentric weights for faces */
};
/* sMedium */
@@ -283,6 +285,7 @@ public:
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data
int m_index;
};
/* Tetra */
struct Tetra : Feature
@@ -315,6 +318,40 @@ public:
btVector3 t1;
btVector3 t2;
};
class DeformableRigidContact
{
public:
sCti m_cti; // Contact infos
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
btScalar m_c2; // inverse mass of node/face
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2;
btVector3 t1;
btVector3 t2;
};
class DeformableNodeRigidContact : public DeformableRigidContact
{
public:
Node* m_node; // Owner node
};
class DeformableFaceRigidContact : public DeformableRigidContact
{
public:
Face* m_face; // Owner face
btVector3 m_contactPoint; // Contact point
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
};
/* SContact */
struct SContact
{
@@ -710,6 +747,8 @@ public:
tTetraArray m_tetras; // Tetras
tAnchorArray m_anchors; // Anchors
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints
tMaterialArray m_materials; // Materials
@@ -1017,6 +1056,7 @@ public:
void initializeFaceTree();
btVector3 evaluateCom() const;
bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
void updateNormals();
void updateBounds();

107
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -993,7 +993,7 @@ struct btSoftColliders
void DoNode(btSoftBody::Node& n) const
{
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
btSoftBody::RContact c;
btSoftBody::DeformableNodeRigidContact c;
if (!n.m_battach)
{
@@ -1010,7 +1010,7 @@ struct btSoftColliders
btSoftBody::sCti& cti = c.m_cti;
c.m_node = &n;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_c2 = ima * psb->m_sst.sdt;
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
@@ -1021,7 +1021,7 @@ struct btSoftColliders
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
@@ -1051,8 +1051,7 @@ struct btSoftColliders
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();
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
@@ -1061,7 +1060,7 @@ struct btSoftColliders
c.t2 = t2;
}
}
psb->m_rcontacts.push_back(c);
psb->m_nodeRigidContacts.push_back(c);
}
}
}
@@ -1072,6 +1071,102 @@ struct btSoftColliders
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideSDF_RDF
//
struct CollideSDF_RDF : btDbvt::ICollide
{
void Process(const btDbvtNode* leaf)
{
btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
DoNode(*face);
}
void DoNode(btSoftBody::Face& f) const
{
btSoftBody::Node* n0 = f.m_n[0];
btSoftBody::Node* n1 = f.m_n[1];
btSoftBody::Node* n2 = f.m_n[2];
const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
btSoftBody::DeformableFaceRigidContact c;
btVector3 contact_point;
btVector3 bary;
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
{
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
btSoftBody::sCti& cti = c.m_cti;
c.m_contactPoint = contact_point;
c.m_bary = bary;
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
c.m_face = &f;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = contact_point - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 normal = cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
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;
btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
}
}
psb->m_faceRigidContacts.push_back(c);
}
}
}
btSoftBody* psb;
const btCollisionObjectWrapper* m_colObj1Wrap;
btRigidBody* m_rigidBody;
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideVF_SS
//