summaryrefslogtreecommitdiff
path: root/src/BulletSoftBody/btSoftBodyInternals.h
diff options
context:
space:
mode:
Diffstat (limited to 'src/BulletSoftBody/btSoftBodyInternals.h')
-rw-r--r--src/BulletSoftBody/btSoftBodyInternals.h141
1 files changed, 131 insertions, 10 deletions
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 9b3e45ea4..8cf0ed3c0 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 <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();
}
}
}
View Lorry Controller Status