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

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();
}
}
}