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

support anchor constraint between deformable and multibody

Browse Source
This commit is contained in:
Xuchen Han
2019-10-17 16:45:28 -07:00
parent 60dfe1fe69
commit 36f7441790
9 changed files with 523 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -45,8 +45,6 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// multibody anchor not supported yet
btAssert(false);
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{

47
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -33,7 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
@@ -146,6 +146,51 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
// update multibody anchor info
if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 nrm;
const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
const btTransform& wtr = multibodyLinkCol->getWorldTransform();
psb->m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(n->m_x),
shp,
nrm,
0);
a.m_cti.m_normal = wtr.getBasis() * nrm;
btVector3 normal = a.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, 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(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
a.jacobianData_normal = jacobianData_normal;
a.jacobianData_t1 = jacobianData_t1;
a.jacobianData_t2 = jacobianData_t2;
a.t1 = t1;
a.t2 = t2;
}
}
}
psb->interpolateRenderMesh();
}

57
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -443,9 +443,66 @@ void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
btVector3 nrm;
const btCollisionShape* shp = link->getCollisionShape();
const btTransform& wtr = link->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = link;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * link->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
btVector3 normal = c.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
findJacobian(link, jacobianData_t2, c.m_node->m_x, 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 = link->m_multiBody->getNumDofs() + 6;
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;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c1 = ra;
c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
{

1
src/BulletSoftBody/btSoftBody.h
View File

@@ -905,6 +905,7 @@ public:
/* Append anchor */
void appendDeformableAnchor(int node, btRigidBody* body);
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
void appendAnchor(int node,
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);