diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4489296f0..431a07d28 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -174,7 +174,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d { btSoftBody::Node& n = psb->m_nodes[i]; n.m_q = n.m_x; - n.m_x += n.m_v * dt; + n.m_q += n.m_v * dt; } /* Bounds */ psb->updateBounds(); @@ -184,7 +184,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg); + vol = btDbvtVolume::FromCR(n.m_q, psb->m_sst.radmrg); psb->m_ndbvt.update(n.m_leaf, vol, n.m_v * psb->m_sst.velmrg, diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 14896514e..240dbb5d4 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -92,7 +92,7 @@ public: { // btMatrix3x3 JFinvT = F.adjoint(); btScalar J = F.determinant(); - P = F.adjoint() * (m_lambda * (J-1)); + P = F.adjoint().transpose() * (m_lambda * (J-1)); if (m_mu > SIMD_EPSILON) { btMatrix3x3 R,S; diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 14d4e8260..f36e54e41 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -61,7 +61,7 @@ public: btVector3 scaled_force = scale * m_dampingStiffness * v_diff; if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { btVector3 dir = (node2->m_x - node1->m_x).normalized(); scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; @@ -91,7 +91,7 @@ public: // elastic force // explicit elastic force - btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir = (node2->m_x - node1->m_x); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); force[id1] += scaled_force; @@ -118,7 +118,7 @@ public: btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { btVector3 dir = (node2->m_x - node1->m_x).normalized(); local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 58907fcd3..c6a0417bc 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -166,7 +166,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_v[c] = -clampDeltaV; } } - node.m_x = node.m_q + timeStep * node.m_v; + node.m_x = node.m_x + timeStep * node.m_v; } } m_deformableBodySolver->revertVelocity(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index fcfb330c3..c4de6a823 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2819,9 +2819,9 @@ void btSoftBody::initializeDmInverse() for (int i = 0; i < m_tetras.size(); ++i) { Tetra &t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; - btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; - btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), c1.getY(), c2.getY(), c3.getY(), c1.getZ(), c2.getZ(), c3.getZ()); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index a39394e0b..fc4c6b556 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -998,7 +998,7 @@ struct btSoftColliders if (!n.m_battach) { // check for collision at x_{n+1}^* - if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true)) + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; @@ -1006,7 +1006,7 @@ struct btSoftColliders if (ms > 0) { // resolve contact at x_n - psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ false); + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); @@ -1019,7 +1019,7 @@ struct btSoftColliders 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 btVector3 ra = n.m_x - wtr.getOrigin(); c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); c.m_c1 = ra; @@ -1035,9 +1035,9 @@ struct btSoftColliders 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); + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); btScalar* J_n = &jacobianData_normal.m_jacobians[0]; btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];