diff options
Diffstat (limited to 'src/BulletSoftBody/btSoftBodyInternals.h')
| -rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 227 |
1 files changed, 197 insertions, 30 deletions
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 7efe514f3..80545ebad 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -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 <cmath> +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) @@ -854,10 +928,62 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + 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_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // - // CollideSDF_RS + // CollideSDF_RD // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RD : btDbvt::ICollide { void Process(const btDbvtNode* leaf) { @@ -869,34 +995,75 @@ struct btSoftColliders const btScalar m = n.m_im > 0 ? dynmargin : stamargin; btSoftBody::RContact c; - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - 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_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } + if (!n.m_battach) + { + // check for collision at x_{n+1}^* + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) + { + const btScalar ima = n.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->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); + 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_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.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; + 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(); + 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); + } + } } } btSoftBody* psb; |