From ec403f790d271af9b33eabaab07cd454eb1437c3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 25 Jul 2019 13:51:44 -0700 Subject: factor out force; now btDeformableLagrangianceForce can be specified at configuration time and to specific softbody --- examples/DeformableContact/DeformableContact.cpp | 11 ++- examples/DeformableDemo/DeformableDemo.cpp | 7 +- examples/Pinch/Pinch.cpp | 9 +- .../VolumetricDeformable/VolumetricDeformable.cpp | 7 +- src/BulletSoftBody/btBackwardEulerObjective.h | 100 --------------------- src/BulletSoftBody/btCGProjection.h | 21 +---- .../btDeformableBackwardEulerObjective.cpp | 13 +-- .../btDeformableBackwardEulerObjective.h | 19 ++++ .../btDeformableContactProjection.cpp | 17 ++-- src/BulletSoftBody/btDeformableContactProjection.h | 4 +- src/BulletSoftBody/btDeformableGravityForce.h | 11 ++- src/BulletSoftBody/btDeformableLagrangianForce.h | 39 ++++---- src/BulletSoftBody/btDeformableMassSpringForce.h | 24 ++--- .../btDeformableRigidDynamicsWorld.cpp | 33 +++++-- .../btDeformableRigidDynamicsWorld.h | 2 + src/BulletSoftBody/btSoftBody.cpp | 10 +-- src/BulletSoftBody/btSoftBody.h | 2 +- src/BulletSoftBody/btSoftBodyInternals.h | 25 +++--- 18 files changed, 150 insertions(+), 204 deletions(-) delete mode 100644 src/BulletSoftBody/btBackwardEulerObjective.h diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index f51978ae3..c40564867 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -127,8 +127,9 @@ void DeformableContact::initPhysics() m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); deformableBodySolver->setWorld(getDeformableDynamicsWorld()); - m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -169,8 +170,8 @@ void DeformableContact::initPhysics() bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = true; - btVector3 linkHalfExtents(1, 1, 1); - btVector3 baseHalfExtents(1, 1, 1); + btVector3 linkHalfExtents(.4, 1, .4); + btVector3 baseHalfExtents(.4, 1, .4); btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); @@ -228,6 +229,8 @@ void DeformableContact::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = .1; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index aa58fbd6f..2411670cf 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -168,8 +168,9 @@ void DeformableDemo::initPhysics() m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality - m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -235,6 +236,8 @@ void DeformableDemo::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // add a few rigid bodies Ctor_RbUpStack(1); diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index cc3008f9c..4252a47b8 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -252,8 +252,9 @@ void Pinch::initPhysics() m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality - m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -336,12 +337,14 @@ void Pinch::initPhysics() // psb->translate(btVector3(-2.5, 4, -2.5)); // psb->getCollisionShape()->setMargin(0.1); // psb->setTotalMass(1); - psb->setSpringStiffness(4); + psb->setSpringStiffness(2); psb->setDampingCoefficient(0.02); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // add a grippers createGrip(); } diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index b30e9bc24..15fb712f6 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -186,8 +186,9 @@ void VolumetricDeformable::initPhysics() m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality - m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -240,6 +241,8 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0.5; + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } // add a few rigid bodies Ctor_RbUpStack(4); diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h deleted file mode 100644 index 16ec0a542..000000000 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ /dev/null @@ -1,100 +0,0 @@ -// -// btDeformableBackwardEulerObjective.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// - -#ifndef BT_BACKWARD_EULER_OBJECTIVE_H -#define BT_BACKWARD_EULER_OBJECTIVE_H -#include -#include "btConjugateGradient.h" -#include "btLagrangianForce.h" -#include "btMassSpring.h" -#include "btDeformableContactProjection.h" -#include "btPreconditioner.h" -#include "btDeformableRigidDynamicsWorld.h" - -class btDeformableRigidDynamicsWorld; -class btDeformableBackwardEulerObjective -{ -public: - using TVStack = btAlignedObjectArray; - btScalar m_dt; - btDeformableRigidDynamicsWorld* m_world; - btAlignedObjectArray m_lf; - btAlignedObjectArray& m_softBodies; - Preconditioner* m_preconditioner; - btDeformableContactProjection projection; - const TVStack& m_backupVelocity; - - btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); - - virtual ~btDeformableBackwardEulerObjective() {} - - void initialize(){} - - // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual) const; - - // add explicit force to the velocity - void applyExplicitForce(TVStack& force); - - // apply force to velocity and optionally reset the force to zero - void applyForce(TVStack& force, bool setZero); - - // compute the norm of the residual - btScalar computeNorm(const TVStack& residual) const; - - // compute one step of the solve (there is only one solve if the system is linear) - void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); - - // perform A*x = b - void multiply(const TVStack& x, TVStack& b) const; - - // set initial guess for CG solve - void initialGuess(TVStack& dv, const TVStack& residual); - - // reset data structure - void reinitialize(bool nodeUpdated); - - void setDt(btScalar dt); - - // enforce constraints in CG solve - void enforceConstraint(TVStack& x) - { - projection.enforceConstraint(x); - updateVelocity(x); - } - - // add dv to velocity - void updateVelocity(const TVStack& dv); - - //set constraints as projections - void setConstraints() - { - projection.setConstraints(); - } - - // update the projections and project the residual - void project(TVStack& r) - { - projection.update(); - // TODO rename - projection.project(r); - } - - // perform precondition M^(-1) x = b - void precondition(const TVStack& x, TVStack& b) - { - m_preconditioner->operator()(x,b); - } - - virtual void setWorld(btDeformableRigidDynamicsWorld* world) - { - m_world = world; - projection.setWorld(world); - } -}; - -#endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 928ee46f8..b4a887fa6 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -109,12 +109,13 @@ public: using TArrayStack = btAlignedObjectArray >; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; - std::unordered_map m_indices; + const std::unordered_map* m_indices; const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) : m_softBodies(softBodies) , m_dt(dt) + , m_indices(indices) { } @@ -132,22 +133,6 @@ public: virtual void reinitialize(bool nodeUpdated) { - if (nodeUpdated) - updateId(); - } - - void updateId() - { - size_t index = 0; - m_indices.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_indices[&(psb->m_nodes[j])] = index++; - } - } } void setSoftBodies(btAlignedObjectArray softBodies) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 31b0905ac..acc9db75f 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -9,21 +9,22 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt) +, 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)); +// 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); +// m_lf.push_back(mass_spring); +// m_lf.push_back(gravity); } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) { if(nodeUpdated) { + updateId(); projection.setSoftBodies(m_softBodies); } for (int i = 0; i < m_lf.size(); ++i) @@ -69,7 +70,7 @@ void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) // only the velocity of the constrained nodes needs to be updated during CG solve for (auto it : projection.m_constraints) { - int i = projection.m_indices[it.first]; + int i = m_indices[it.first]; it.first->m_v = m_backupVelocity[i] + dv[i]; } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index bfd4ca3af..e22b42b0b 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -28,6 +28,7 @@ public: Preconditioner* m_preconditioner; btDeformableContactProjection projection; const TVStack& m_backupVelocity; + std::unordered_map m_indices; btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); @@ -95,6 +96,24 @@ public: m_world = world; projection.setWorld(world); } + + virtual void updateId() + { + size_t index = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_indices[&(psb->m_nodes[j])] = index++; + } + } + } + + std::unordered_map* getIndices() + { + return &m_indices; + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 1a9cad120..4d6ea4e84 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -88,26 +88,27 @@ void btDeformableContactProjection::update() const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; + vel += local_v[k] * J_n[k]; } va = cti.m_normal * vel * m_dt; + // add in the tangential components of the va vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + vel += local_v[k] * J_t1[k]; } va += c->t1 * vel * m_dt; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; + vel += local_v[k] * J_t2[k]; } va += c->t2 * vel * m_dt; } @@ -177,7 +178,7 @@ void btDeformableContactProjection::update() // the following is equivalent /* - btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; + btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)]; btScalar dvn = dv.dot(cti.m_normal); */ @@ -196,9 +197,11 @@ void btDeformableContactProjection::update() { if (multibodyLinkCol) { + // apply normal component of the impulse multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); if (incremental_tangent.norm() > SIMD_EPSILON) { + // apply tangential component of the impulse const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; @@ -336,7 +339,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; + size_t i = m_indices->at(it.first); const btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); @@ -399,7 +402,7 @@ void btDeformableContactProjection::project(TVStack& x) for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; + size_t i = m_indices->at(it.first); btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index e73d8faa4..ea3b00f62 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -18,8 +18,8 @@ public: std::unordered_map > m_constraints; std::unordered_map > m_frictions; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) - : btCGProjection(softBodies, dt) + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) + : btCGProjection(softBodies, dt, indices) { } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index d8571fa73..398662e1f 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -16,7 +16,7 @@ public: using TVStack = btDeformableLagrangianForce::TVStack; btVector3 m_gravity; - btDeformableGravityForce(const btAlignedObjectArray& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g) + btDeformableGravityForce(const btVector3& g) : m_gravity(g) { } @@ -39,14 +39,14 @@ public: virtual void addScaledGravityForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = m_indices[&n]; + size_t id = m_indices->at(&n); btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; btVector3 scaled_force = scale * m_gravity * mass; force[id] += scaled_force; @@ -54,7 +54,10 @@ public: } } - + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } }; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index acb9a28ff..fa4184a14 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -10,16 +10,20 @@ #include "btSoftBody.h" #include +enum btDeformableLagrangianForceType +{ + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2 +}; class btDeformableLagrangianForce { public: using TVStack = btAlignedObjectArray; - const btAlignedObjectArray& m_softBodies; - std::unordered_map m_indices; + btAlignedObjectArray m_softBodies; + const std::unordered_map* m_indices; - btDeformableLagrangianForce(const btAlignedObjectArray& softBodies) - : m_softBodies(softBodies) + btDeformableLagrangianForce() { } @@ -31,23 +35,10 @@ public: virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + virtual btDeformableLagrangianForceType getForceType() = 0; + virtual void reinitialize(bool nodeUpdated) { - if (nodeUpdated) - updateId(); - } - - virtual void updateId() - { - size_t index = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_indices[&(psb->m_nodes[j])] = index++; - } - } } virtual int getNumNodes() @@ -59,5 +50,15 @@ public: } return numNodes; } + + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void setIndices(const std::unordered_map* indices) + { + m_indices = indices; + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 70ad6e69c..c9fca134c 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -14,15 +14,13 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { public: using TVStack = btDeformableLagrangianForce::TVStack; - btDeformableMassSpringForce(const btAlignedObjectArray& softBodies) : btDeformableLagrangianForce(softBodies) + btDeformableMassSpringForce() { - } virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); -// addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -33,7 +31,7 @@ public: virtual void addScaledDampingForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -42,8 +40,8 @@ public: const auto& link = psb->m_links[j]; const auto node1 = link.m_n[0]; const auto node2 = link.m_n[1]; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); // damping force btVector3 v_diff = (node2->m_v - node1->m_v); @@ -58,7 +56,7 @@ public: virtual void addScaledElasticForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -69,8 +67,8 @@ public: const auto node2 = link.m_n[1]; btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); // elastic force // explicit elastic force @@ -95,8 +93,8 @@ public: const auto node1 = link.m_n[0]; const auto node2 = link.m_n[1]; btScalar k_damp = psb->m_dampingCoefficient; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); df[id1] += local_scaled_df; df[id2] -= local_scaled_df; @@ -104,6 +102,10 @@ public: } } + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } }; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index f9c9968c9..b91d9d2c4 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -60,13 +60,12 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) if (c == nullptr || c->m_node->m_im == 0) continue; const btSoftBody::sCti& cti = c->m_cti; - btRigidBody* rigidCol = 0; btVector3 va(0, 0, 0); // grab the velocity of the rigid body if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) @@ -78,25 +77,25 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; - + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; + vel += local_v[k] * J_n[k]; } va = cti.m_normal * vel; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + vel += local_v[k] * J_t1[k]; } va += c->t1 * vel; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; + vel += local_v[k] * J_t2[k]; } va += c->t2 * vel; } @@ -110,7 +109,6 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) if (cti.m_colObj->hasContactResponse()) { btScalar dp = cti.m_offset; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); if (friction.m_static[j] == true) { c->m_node->m_v = va; @@ -214,3 +212,24 @@ void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) m_beforeSolverCallbacks[i](m_internalTime, this); } + +void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } +} diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index f1be4fd85..c6b3dcbc6 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -126,6 +126,8 @@ public: void afterSolverCallbacks(btScalar timeStep); + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index bd7587c47..c31bdb7ed 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2266,14 +2266,15 @@ btVector3 btSoftBody::evaluateCom() const bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, - btSoftBody::sCti& cti) const + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + const btRigidBody *tmpRigid = btRigidBody::upcast(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 ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); +// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); // TODO: get the correct transform for multibody btScalar dst = @@ -2282,11 +2283,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, shp, nrm, margin); - if (dst < 0) + if (dst < 0 || !predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; -// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); cti.m_offset = dst; return (true); } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 95249b093..5e33d74d0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1005,7 +1005,7 @@ public: btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; void updateNormals(); void updateBounds(); void updatePose(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8cf0ed3c0..2ec8c1b55 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -945,14 +945,17 @@ struct btSoftColliders if (!n.m_battach) { - if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + // check for collision at x_{n+1}^* + if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) +// if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false)); { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; const btScalar ms = ima + imb; if (ms > 0) { - psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti); + // resolve contact at x_n + psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); auto& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); @@ -982,8 +985,13 @@ 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(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + + // 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); + } btScalar* J_n = &jacobianData_normal.m_jacobians[0]; btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; @@ -995,16 +1003,7 @@ struct btSoftColliders 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; -- cgit v1.2.1