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

add correct impulse matrix to multibody-deformable contact

Browse Source
This commit is contained in:
Xuchen Han
2019-07-24 16:01:47 -07:00
parent 243b9fc8c7
commit 233a381e7c
14 changed files with 618 additions and 594 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -422,7 +422,11 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
solveExternalForces(solverInfo);
solveInternalConstraints(solverInfo);
}
void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
{
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);

1
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -113,6 +113,7 @@ public:
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

27
src/BulletSoftBody/btCGProjection.h
View File

@@ -21,11 +21,10 @@ struct DeformableContactConstraint
btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
btAlignedObjectArray<btMultiBodyJacobianData> m_normal_jacobian;
DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
DeformableContactConstraint(const btSoftBody::RContact& rcontact)
{
append(rcontact, jacobian);
append(rcontact);
}
DeformableContactConstraint(const btVector3 dir)
@@ -34,8 +33,6 @@ struct DeformableContactConstraint
m_direction.push_back(dir);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
btMultiBodyJacobianData j;
m_normal_jacobian.push_back(j);
}
DeformableContactConstraint()
@@ -44,17 +41,14 @@ struct DeformableContactConstraint
m_direction.push_back(btVector3(0,0,0));
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
btMultiBodyJacobianData j;
m_normal_jacobian.push_back(j);
}
void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
void append(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
m_normal_jacobian.push_back(jacobian);
}
~DeformableContactConstraint()
@@ -77,8 +71,6 @@ struct DeformableFrictionConstraint
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_released; // whether the contact is released
btAlignedObjectArray<btMultiBodyJacobianData> m_complementary_jacobian;
btAlignedObjectArray<btVector3> m_complementaryDirection;
// the total impulse the node applied to the rb in the tangential direction in the cg solve
@@ -89,12 +81,6 @@ struct DeformableFrictionConstraint
append();
}
DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
{
append();
addJacobian(complementaryDir, jacobian);
}
void append()
{
m_static.push_back(false);
@@ -112,13 +98,6 @@ struct DeformableFrictionConstraint
m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
m_released.push_back(false);
}
void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
{
m_complementary_jacobian.push_back(jacobian);
m_complementaryDirection.push_back(complementaryDir);
}
};
class btCGProjection

488
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -1,488 +0,0 @@
//
// btDeformableContactProjection.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 7/4/19.
//
#include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
#include <algorithm>
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
btScalar ux = u.getX();
btScalar uy = u.getY();
btScalar uz = u.getZ();
btScalar ax = std::abs(ux);
btScalar ay = std::abs(uy);
btScalar az = std::abs(uz);
btVector3 v;
if (ax <= ay && ax <= az)
v = btVector3(0, -uz, uy);
else if (ay <= ax && ay <= az)
v = btVector3(-uz, 0, ux);
else
v = btVector3(-uy, ux, 0);
v.normalize();
return v;
}
void btDeformableContactProjection::update()
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 10;
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values
for (auto& it : m_constraints)
{
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
if (constraint.m_contact[j] == nullptr)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
// normal jacobian is precompute but tangent jacobian is not
const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
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* jac_normal = &jacobianData_normal.m_jacobians[0];
deltaV_normal = &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 += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
}
va = cti.m_normal * vel * m_dt;
// add in complementary direction of va
const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
}
va += friction.m_complementaryDirection[j] * 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);
const btVector3 impulse_tangent = impulse - impulse_normal;
// start friction handling
// copy old data
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
// get the current tangent direction
btScalar local_tangent_norm = impulse_tangent.norm();
btVector3 local_tangent_dir = btVector3(0,0,0);
if (local_tangent_norm > SIMD_EPSILON)
local_tangent_dir = impulse_tangent.normalized();
// accumulated impulse on the rb in this and all prev cg iterations
constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
// the total tangential impulse required to stop sliding
btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
btScalar tangent_norm = tangent.norm();
if (accumulated_normal < 0)
{
friction.m_direction[j] = -local_tangent_dir;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
{
friction.m_static[j] = false;
friction.m_impulse[j] = -accumulated_normal*c->m_c3;
}
else
{
friction.m_static[j] = true;
friction.m_impulse[j] = tangent_norm;
}
}
else
{
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_impulse[j] = 0;
friction.m_direction[j] = btVector3(0,0,0);
}
friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
// the incremental impulse applied to rb in the tangential direction
btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// TODO cleanup
if (1) // in the same CG solve, the set of constraits doesn't change
{
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
btScalar dvn = dv.dot(cti.m_normal);
*/
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + incremental_tangent;
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)
{
double multiplier = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
if (incremental_tangent.norm() > SIMD_EPSILON)
{
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
}
}
}
}
}
}
}
}
}
void btDeformableContactProjection::setConstraints()
{
// set Dirichlet constraint
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
btAlignedObjectArray<DeformableContactConstraint> c;
c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
m_constraints[&(psb->m_nodes[j])] = c;
btAlignedObjectArray<DeformableFrictionConstraint> f;
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
m_frictions[&(psb->m_nodes[j])] = f;
}
}
}
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)
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
{
continue;
}
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
{
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)
{
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)
{
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
btScalar vel = 0.0;
const btScalar* jac = &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];
std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
std::cout << jac[j] << std::endl;
}
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)
{
// find complementary jacobian
btVector3 complementaryDirection;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
}
}
if (m_constraints.find(c.m_node) == m_constraints.end())
{
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
m_constraints[c.m_node] = constraints;
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
m_frictions[c.m_node] = frictions;
}
else
{
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
btScalar dot_prod = dirs[0].dot(cti.m_normal);
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
// group the constraints
constraints[j].append(c, jacobianData_normal);
// push in an empty friction
frictions[j].append();
frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
merged = true;
break;
}
}
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
}
}
}
}
}
}
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
else
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
for (int f = 0; f < frictions.size(); ++f)
{
const DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
}
}
}
void btDeformableContactProjection::project(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
else
x[i].setZero();
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
bool has_static_constraint = false;
for (int f = 0; f < frictions.size(); ++f)
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
}
for (int f = 0; f < frictions.size(); ++f)
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
{
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
}
}
}
}
}
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_frictions.clear();
}

72
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -45,7 +45,7 @@ void btDeformableContactProjection::update()
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 10;
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values
for (auto& it : m_constraints)
@@ -66,10 +66,6 @@ void btDeformableContactProjection::update()
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
// normal jacobian is precompute but tangent jacobian is not
const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
@@ -89,25 +85,31 @@ void btDeformableContactProjection::update()
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
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];
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 += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k];
}
va = cti.m_normal * vel * m_dt;
// add in complementary direction of va
const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
}
va += friction.m_complementaryDirection[j] * vel * m_dt;
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
}
va += c->t2 * vel * m_dt;
}
}
@@ -143,8 +145,6 @@ void btDeformableContactProjection::update()
friction.m_direction[j] = -local_tangent_dir;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
btScalar comp1 = -accumulated_normal*c->m_c3;
btScalar comp2 = tangent_norm;
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
{
friction.m_static[j] = false;
@@ -194,19 +194,15 @@ void btDeformableContactProjection::update()
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{
double multiplier = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (incremental_tangent.norm() > SIMD_EPSILON)
{
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
}
}
}
@@ -274,15 +270,12 @@ void btDeformableContactProjection::setConstraints()
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
btScalar vel = 0.0;
const btScalar* jac = &jacobianData_normal.m_jacobians[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];
std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
std::cout << jac[j] << std::endl;
}
va = cti.m_normal * vel * m_dt;
}
@@ -293,25 +286,13 @@ void btDeformableContactProjection::setConstraints()
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
// find complementary jacobian
btVector3 complementaryDirection;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
}
}
if (m_constraints.find(c.m_node) == m_constraints.end())
{
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
constraints.push_back(DeformableContactConstraint(c));
m_constraints[c.m_node] = constraints;
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
frictions.push_back(DeformableFrictionConstraint());
m_frictions[c.m_node] = frictions;
}
else
@@ -328,10 +309,9 @@ void btDeformableContactProjection::setConstraints()
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
// group the constraints
constraints[j].append(c, jacobianData_normal);
constraints[j].append(c);
// push in an empty friction
frictions[j].append();
frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
merged = true;
break;
}
@@ -340,8 +320,8 @@ void btDeformableContactProjection::setConstraints()
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
constraints.push_back(DeformableContactConstraint(c));
frictions.push_back(DeformableFrictionConstraint());
}
}
}

2
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -40,4 +40,4 @@ public:
virtual void reinitialize(bool nodeUpdated);
};
#endif /* btContactProjection_h */
#endif /* btDeformableContactProjection_h */

37
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -69,6 +69,43 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
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];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k];
}
va = cti.m_normal * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
}
va += c->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[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())
{

10
src/BulletSoftBody/btSoftBody.h
View File

@@ -26,7 +26,8 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "btSparseSDF.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
//#ifdef BT_USE_DOUBLE_PRECISION
//#define btRigidBodyData btRigidBodyDoubleData
//#define btRigidBodyDataName "btRigidBodyDoubleData"
@@ -300,6 +301,13 @@ public:
btScalar m_c2; // ima*dt
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;
};
/* SContact */
struct SContact

141
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -25,7 +25,41 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <string.h> //for memset
#include <iostream>
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
btScalar ux = u.getX();
btScalar uy = u.getY();
btScalar uz = u.getZ();
btScalar ax = std::abs(ux);
btScalar ay = std::abs(uy);
btScalar az = std::abs(uz);
btVector3 v;
if (ax <= ay && ax <= az)
v = btVector3(0, -uz, uy);
else if (ay <= ax && ay <= az)
v = btVector3(-uz, 0, ux);
else
v = btVector3(-uy, ux, 0);
v.normalize();
return v;
}
//
// btSymMatrix
//
@@ -298,6 +332,46 @@ static inline btMatrix3x3 Diagonal(btScalar x)
m[2] = btVector3(0, 0, x);
return (m);
}
static inline btMatrix3x3 Diagonal(const btVector3& v)
{
btMatrix3x3 m;
m[0] = btVector3(v.getX(), 0, 0);
m[1] = btVector3(0, v.getY(), 0);
m[2] = btVector3(0, 0, v.getZ());
return (m);
}
static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
{
btScalar result = 0;
for (int i = 0; i < ndof; ++i)
result += a[i] * b[i];
return result;
}
static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
{
btMatrix3x3 m;
btScalar a11 = Dot(v1,u1,ndof);
btScalar a12 = Dot(v1,u2,ndof);
btScalar a13 = Dot(v1,u3,ndof);
btScalar a21 = Dot(v2,u1,ndof);
btScalar a22 = Dot(v2,u2,ndof);
btScalar a23 = Dot(v2,u3,ndof);
btScalar a31 = Dot(v3,u1,ndof);
btScalar a32 = Dot(v3,u2,ndof);
btScalar a33 = Dot(v3,u3,ndof);
m[0] = btVector3(a11, a12, a13);
m[1] = btVector3(a21, a22, a23);
m[2] = btVector3(a31, a32, a33);
return (m);
}
//
static inline btMatrix3x3 Add(const btMatrix3x3& a,
const btMatrix3x3& b)
@@ -879,21 +953,68 @@ struct btSoftColliders
if (ms > 0)
{
psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
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 = n.m_q - wtr.getOrigin();
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
auto& cti = c.m_cti;
c.m_node = &n;
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
c.m_c1 = ra;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_c2 = ima * psb->m_sst.sdt;
// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
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 = n.m_q - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, 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, c.m_node->m_q, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, 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, t1, t2); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btVector3 u_dot_J(0,0,0);
for (int i = 0; i < ndof; ++i)
{
u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]);
}
btVector3 impulse_matrix_diag;
btScalar dt = psb->m_sst.sdt;
impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt));
impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt));
impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt));
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();
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_rcontacts.push_back(c);
if (m_rigidBody)
m_rigidBody->activate();
}
}
}