diff options
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBody.cpp | 130 | ||||
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBody.h | 25 | ||||
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 22 | ||||
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h | 4 | ||||
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBodyLink.h | 35 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 5 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableContactProjection.cpp | 2 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 2 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.cpp | 8 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 9 |
10 files changed, 196 insertions, 46 deletions
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 3e210d752..80ad19891 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -27,6 +27,7 @@ #include "btMultiBodyJointFeedback.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" +#include <iostream> //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM @@ -100,6 +101,8 @@ btMultiBody::btMultiBody(int n_links, m_baseName(0), m_basePos(0, 0, 0), m_baseQuat(0, 0, 0, 1), + m_basePos_interpolate(0, 0, 0), + m_baseQuat_interpolate(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -449,6 +452,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const return m_links[i].m_cachedRotParentToThis; } +const btVector3 &btMultiBody::getInterpolateRVector(int i) const +{ + return m_links[i].m_cachedRVector_interpolate; +} + +const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis_interpolate; +} + btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { btAssert(i >= -1); @@ -1581,17 +1594,37 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar //printf("]\n"); ///////////////// } +void btMultiBody::predictPositionsMultiDof(btScalar dt) +{ + stepPositionsMultiDof(dt, 0, 0, true); +} -void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict) { int num_links = getNumLinks(); // step position by adding dt * velocity //btVector3 v = getBaseVel(); //m_basePos += dt * v; // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // + btScalar *pBasePos; + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + if (!predict) + { + pBasePos = (pq ? &pq[4] : m_basePos); + } // + else + { + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + } + + + pBasePos[0] += dt * pBaseVel[0]; pBasePos[1] += dt * pBaseVel[1]; pBasePos[2] += dt * pBaseVel[2]; @@ -1645,7 +1678,18 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseQuat; + if (!predict) + pBaseQuat = pq ? pq : m_baseQuat; + else + { + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + } btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // btQuaternion baseQuat; @@ -1670,7 +1714,12 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointPos; + if (!predict) + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + else + pJointPos = &m_links[i].m_jointPos_interpolate[0]; + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); switch (m_links[i].m_jointType) @@ -1678,12 +1727,23 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { + //reset to current pos + if (predict) + { + pJointPos[0] = m_links[i].m_jointPos[0]; + } btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { + //reset to current pos + if (predict) + { + for (int i = 0; i < 4; ++i) + pJointPos[i] = m_links[i].m_jointPos[i]; + } btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); btQuaternion jointOri; @@ -1697,6 +1757,11 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::ePlanar: { + if (predict) + { + for (int i = 0; i < 3; ++i) + pJointPos[i] = m_links[i].m_jointPos[i]; + } pJointPos[0] += dt * getJointVelMultiDof(i)[0]; btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); @@ -1711,7 +1776,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } } - m_links[i].updateCacheMultiDof(pq); + m_links[i].updateCacheMultiDof(pq, predict); if (pq) pq += m_links[i].m_posVarCount; @@ -2006,6 +2071,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu } } +void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) +{ + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getBaseCollider()->setInterpolationWorldTransform(tr); + } + + for (int k = 0; k < getNumLinks(); k++) + { + const int parent = getParent(k); + world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); + } + + for (int m = 0; m < getNumLinks(); m++) + { + btMultiBodyLinkCollider *col = getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + col->setInterpolationWorldTransform(tr); + } + } +} + int btMultiBody::calculateSerializeBufferSize() const { int sz = sizeof(btMultiBodyData); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c0b0d003b..fe60af991 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -193,12 +193,24 @@ public: const btQuaternion &getWorldToBaseRot() const { return m_baseQuat; - } // rotates world vectors into base frame + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame void setBasePos(const btVector3 &pos) { m_basePos = pos; + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -224,6 +236,7 @@ public: void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } void setBaseOmega(const btVector3 &omega) { @@ -273,6 +286,8 @@ public: const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. // // transform vectors in local frame of link i to world frame (or vice versa) @@ -420,7 +435,10 @@ public: } // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); // // contacts @@ -581,6 +599,7 @@ public: void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); virtual int calculateSerializeBufferSize() const; @@ -664,7 +683,9 @@ private: const char *m_baseName; //memory needs to be manager by user! btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; btScalar m_baseMass; // mass of the base btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index d4a9a754f..d1ac5e26f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) m_multiBodies.remove(body); } +void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + integrateMultiBodyTransforms(timeStep, /*predict = */ true); + +} void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); @@ -778,8 +784,11 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); + integrateMultiBodyTransforms(timeStep); +} - { +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict) +{ BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -802,7 +811,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) int nLinks = bod->getNumLinks(); ///base + num m_links - + if (!predict) { if (!bod->isPosUpdated()) bod->stepPositionsMultiDof(timeStep); @@ -815,18 +824,21 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) bod->setPosUpdated(false); } } + else + bod->predictPositionsMultiDof(timeStep); m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + if (predict) + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + else + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { bod->clearVelocities(); } } - } } void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 4f48f07d2..e82c17b31 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -96,7 +96,9 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - + void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false); + + virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 92d41dfac..e7966b852 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -111,6 +111,10 @@ struct btMultibodyLink btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame @@ -119,6 +123,7 @@ struct btMultibodyLink btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; + btScalar m_jointPos_interpolate[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. //It gets set to zero after each internal stepSimulation call @@ -186,44 +191,50 @@ struct btMultibodyLink } // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCacheMultiDof(btScalar *pq = 0) + void updateCacheMultiDof(btScalar *pq = 0, bool predict = false) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - + btScalar *pJointPos; + + if (!predict) + pJointPos = (pq ? pq : &m_jointPos[0]); + else + pJointPos = &m_jointPos_interpolate[0]; + btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis; + btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector; switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); break; } case eFixed: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index acc9db75f..9ffd63a3c 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -12,12 +12,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned , projection(m_softBodies, m_dt, &m_indices) , m_backupVelocity(backup_v) { - // TODO: this should really be specified in initialization instead of here -// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies); -// btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0)); m_preconditioner = new DefaultPreconditioner(); -// m_lf.push_back(mass_spring); -// m_lf.push_back(gravity); } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4d6ea4e84..734fbb451 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -44,7 +44,7 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u) void btDeformableContactProjection::update() { ///solve rigid body constraints - m_world->getSolverInfo().m_numIterations = 10; + m_world->getSolverInfo().m_numIterations = 1; m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index b91d9d2c4..6ff5a9a41 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -161,7 +161,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(float(timeStep)); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c31bdb7ed..06e0b41d0 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2270,12 +2270,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg - const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); -// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); - // TODO: get the correct transform for multibody + const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); +// const btTransform &wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 2ec8c1b55..34302ccc4 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -985,13 +985,8 @@ struct btSoftColliders btVector3 t2 = btCross(normal, t1); btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); - - // findJacobian is hella expensive, avoid calling if possible - if (fc != 0) - { - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); - } + 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]; |