From 6f3dd32f3737c531e85fafec788b313a846cbcaa Mon Sep 17 00:00:00 2001 From: jingyuc Date: Fri, 23 Jul 2021 16:20:09 -0400 Subject: [PATCH] add new btReducedSoftBody and btReduceDeformableSolver class --- examples/ReducedDeformableDemo/BasicTest.cpp | 10 +- .../BulletReducedSoftBody/btReducedSoftBody.h | 2 +- .../btReducedSoftBodyHelpers.cpp | 6 +- .../btReducedSoftBodySolver.cpp | 68 +++++++++++ .../btReducedSoftBodySolver.h | 24 ++-- src/BulletSoftBody/CMakeLists.txt | 4 +- .../btDeformableBackwardEulerObjective.cpp | 68 +---------- .../btDeformableBackwardEulerObjective.h | 8 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 89 ++++++++++++++- src/BulletSoftBody/btDeformableBodySolver.h | 53 ++++++++- .../btDeformableMultiBodyDynamicsWorld.cpp | 106 ++---------------- .../btDeformableMultiBodyDynamicsWorld.h | 4 +- src/BulletSoftBody/btSoftBody.h | 2 +- 13 files changed, 258 insertions(+), 186 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 425293919..a829314ca 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -17,7 +17,7 @@ #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" #include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h" #include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h" -#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "../CommonInterfaces/CommonParameterInterface.h" @@ -133,7 +133,7 @@ public: void stepSimulation(float deltaTime) { - btReducedSoftBody* rsb = static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]; + btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); // TODO: remove this. very hacky way of adding initial deformation if (first_step && !rsb->m_bUpdateRtCst) @@ -186,13 +186,13 @@ void BasicTest::initPhysics() m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); m_broadphase = new btDbvtBroadphase(); - btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); - sol->setDeformableSolver(deformableBodySolver); + sol->setDeformableSolver(reducedSoftBodySolver); m_solver = sol; - m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver); btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index e9784d220..38adbae7f 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -35,7 +35,7 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this? // full space - TVStack m_x0; // Rest position + tDenseArray m_x0; // Rest position // rigid frame diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index 13c6dfd59..02c582280 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -112,7 +112,7 @@ btReducedSoftBody* btReducedSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldIn } // read in binary files -void btReducedSoftBodyHelpers::readBinary(btSoftBody::tDenseArray>& vec, +void btReducedSoftBodyHelpers::readBinary(btReducedSoftBody::tDenseArray& vec, const unsigned int n_start, // starting index const unsigned int n_modes, // #entries read const unsigned int n_full, // array size @@ -136,7 +136,7 @@ void btReducedSoftBodyHelpers::readBinary(btSoftBody::tDenseArray>& vec, f_in.close(); } -void btReducedSoftBodyHelpers::readBinaryMat(btSoftBody::tDenseMatrix& mat, +void btReducedSoftBodyHelpers::readBinaryMat(btReducedSoftBody::tDenseMatrix& mat, const unsigned int n_start, // starting mode index const unsigned int n_modes, // #modes, outer array size const unsigned int n_full, // inner array size @@ -168,7 +168,7 @@ void btReducedSoftBodyHelpers::readBinaryMat(btSoftBody::tDenseMatrix& mat, f_in.close(); } -void btReducedSoftBodyHelpers::readBinaryModes(btSoftBody::tDenseMatrix& mat, +void btReducedSoftBodyHelpers::readBinaryModes(btReducedSoftBody::tDenseMatrix& mat, const unsigned int n_start, // starting mode index const unsigned int n_modes, // #modes, outer array size const unsigned int n_full, // inner array size diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index c09609e07..0853d1d00 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -1,2 +1,70 @@ #include "btReducedSoftBodySolver.h" +#include "../btDeformableMultiBodyDynamicsWorld.h" +void btReducedSoftBodySolver::applyForce() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // get reduced force + btAlignedObjectArray reduced_force; + reduced_force.resize(rsb->m_reducedDofs.size(), 0); + + // add internal force (elastic force & damping force) + for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) { + // map all force to reduced + // for (int i = 0; i < force.size(); ++i) + // for (int k = 0; k < 3; ++k) + // reduced_force[r] += scale * rsb->m_modes[r][3 * i + k] * force[i][k]; + + // std::cout << reduced_force[r] << '\t'; + + reduced_force[r] += rsb->m_Kr[r] * (rsb->m_reducedDofs[r] + 0.1 * rsb->m_reducedVelocity[r]); + // std::cout << reduced_force[r] << '\n'; + // std::cout << rsb->m_Kr[r] << "\t" << rsb->m_reducedDofs[r] << "\n"; + } + + + // apply impulses to reduced deformable objects + static btScalar sim_time = 0; + static btScalar target_vel = 20; + static bool apply_impulse = true; + if (rsb->m_reducedModel && apply_impulse && sim_time > 1) + { + apply_impulse = false; + + btScalar f_imp = rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + for (int i = 0; i < rsb->m_reducedDofs.size(); ++i) + { + reduced_force[i] += rsb->m_modes[i][0 * 3 + 1] * f_imp; + } + } + + // update reduced velocity + for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) + { + btScalar mass_inv = (rsb->m_Mr[r] == 0) ? 0 : 1.0 / rsb->m_Mr[r]; + btScalar delta_v = m_dt * mass_inv * reduced_force[r]; + + sim_time += m_dt; + rsb->m_reducedVelocity[r] -= delta_v; + } + } +} + +void btReducedSoftBodySolver::applyExplicitForce() +{ + applyForce(); +} + +void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) + rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r]; + } +} \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index e3fe3f215..15d68979d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -1,20 +1,18 @@ #ifndef BT_REDUCED_SOFT_BODY_SOLVER_H #define BT_REDUCED_SOFT_BODY_SOLVER_H -#include "btSoftBodySolver.h" -#include "btDeformableMultiBodyDynamicsWorld.h" +#include "btReducedSoftBody.h" +#include "../btDeformableBodySolver.h" -class btReducedSoftBodySolver : public btSoftBodySolver +class btReducedSoftBody; + +class btReducedSoftBodySolver : public btDeformableBodySolver { - typedef btAlignedObjectArray btReducedSoftBodyArray; - typedef btAlignedObjectArray TVStack; - protected: - btReducedSoftBodyArray m_reducedSoftBodies; - btScalar m_dt; + void applyForce(); public: - btReducedSoftBodySolver() : m_dt(0) {} + btReducedSoftBodySolver() {} ~btReducedSoftBodySolver() {} virtual SolverTypes getSolverType() const @@ -22,9 +20,13 @@ class btReducedSoftBodySolver : public btSoftBodySolver return REDUCED_DEFORMABLE_SOLVER; } - virtual void predictMotion(btScalar solver_dt); + // virtual void predictMotion(btScalar solver_dt); - virtual void solveConstraints(btScalar solver_dt); + // virtual void solveConstraints(btScalar solver_dt); + + virtual void applyExplicitForce(); + + virtual void applyTransforms(btScalar timeStep); }; diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 79313fb4f..12962b28a 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -27,7 +27,7 @@ SET(BulletSoftBody_SRCS BulletReducedSoftBody/btReducedSoftBody.cpp BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp - BulletReducedSoftBody/btReduceSoftBodySolver.cpp + BulletReducedSoftBody/btReducedSoftBodySolver.cpp ) @@ -70,7 +70,7 @@ SET(BulletSoftBody_HDRS BulletReducedSoftBody/btReducedSoftBody.h BulletReducedSoftBody/btReducedSoftBodyHelpers.h - BulletReducedSoftBody/btReduceSoftBodySolver.h + BulletReducedSoftBody/btReducedSoftBodySolver.h ) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 89542daff..a5d4fa7df 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -149,76 +149,20 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero } if (m_implicit) { - if (psb->m_reducedModel) + for (int j = 0; j < psb->m_nodes.size(); ++j) { - // TODO: reduced soft body - } - else - { - for (int j = 0; j < psb->m_nodes.size(); ++j) + if (psb->m_nodes[j].m_im != 0) { - if (psb->m_nodes[j].m_im != 0) - { - psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; - } + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; } } } else { - if (psb->m_reducedModel) + for (int j = 0; j < psb->m_nodes.size(); ++j) { - // get reduced force - btAlignedObjectArray reduced_force; - reduced_force.resize(psb->m_reducedDofs.size(), 0); - - // add internal force (elastic force & damping force) - for (int r = 0; r < psb->m_reducedDofs.size(); ++r) { - // map all force to reduced - // for (int i = 0; i < force.size(); ++i) - // for (int k = 0; k < 3; ++k) - // reduced_force[r] += scale * psb->m_modes[r][3 * i + k] * force[i][k]; - - // std::cout << reduced_force[r] << '\t'; - - reduced_force[r] += psb->m_Kr[r] * (psb->m_reducedDofs[r] + 0.1 * psb->m_reducedVelocity[r]); - // std::cout << reduced_force[r] << '\n'; - // std::cout << psb->m_Kr[r] << "\t" << psb->m_reducedDofs[r] << "\n"; - } - - - // apply impulses to reduced deformable objects - static btScalar sim_time = 0; - static btScalar target_vel = 20; - static bool apply_impulse = true; - if (psb->m_reducedModel && apply_impulse && sim_time > 1) - { - apply_impulse = false; - - btScalar f_imp = psb->m_nodes[i].m_im * (target_vel - psb->m_nodes[0].m_v[1]) / m_dt; - for (int i = 0; i < psb->m_reducedDofs.size(); ++i) - { - reduced_force[i] += psb->m_modes[i][0 * 3 + 1] * f_imp; - } - } - - // update reduced velocity - for (int r = 0; r < psb->m_reducedDofs.size(); ++r) - { - btScalar mass_inv = (psb->m_Mr[r] == 0) ? 0 : 1.0 / psb->m_Mr[r]; - btScalar delta_v = m_dt * mass_inv * reduced_force[r]; - - sim_time += m_dt; - psb->m_reducedVelocity[r] -= delta_v; - } - } - else - { - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; - } + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; } } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index eb05b9f01..60b6fe388 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -25,12 +25,18 @@ #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" #include "btPreconditioner.h" -#include "btDeformableMultiBodyDynamicsWorld.h" +// #include "btDeformableMultiBodyDynamicsWorld.h" #include "LinearMath/btQuickprof.h" class btDeformableBackwardEulerObjective { public: + enum _ + { + Mass_preconditioner, + KKT_preconditioner + }; + typedef btAlignedObjectArray TVStack; btScalar m_dt; btAlignedObjectArray m_lf; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index e81680f01..c91b84d24 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -401,7 +401,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) } } } - m_objective->applyExplicitForce(m_residual); + applyExplicitForce(); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -504,3 +504,90 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch) { m_lineSearch = lineSearch; } + +void btDeformableBodySolver::applyExplicitForce() +{ + m_objective->applyExplicitForce(m_residual); +} + +void btDeformableBodySolver::applyTransforms(btScalar timeStep) +{ + std::cout << "called base\n"; + 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& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, 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; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } +} \ No newline at end of file diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index ae674d6e8..7049f1992 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -24,8 +24,8 @@ #include "btConjugateResidual.h" #include "btConjugateGradient.h" struct btCollisionObjectWrapper; -class btDeformableBackwardEulerObjective; -class btDeformableMultiBodyDynamicsWorld; +// class btDeformableBackwardEulerObjective; +// class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { @@ -150,6 +150,55 @@ public: // used in line search btScalar kineticEnergy(); + // add explicit force to the velocity in the objective class + virtual void applyExplicitForce(); + + // execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world + virtual void applyTransforms(btScalar timeStep); + + virtual void setStrainLimiting(bool opt) + { + m_objective->m_projection.m_useStrainLimiting = opt; + } + + virtual void setPreconditioner(int opt) + { + switch (opt) + { + case btDeformableBackwardEulerObjective::Mass_preconditioner: + m_objective->m_preconditioner = m_objective->m_massPreconditioner; + break; + + case btDeformableBackwardEulerObjective::KKT_preconditioner: + m_objective->m_preconditioner = m_objective->m_KKTPreconditioner; + break; + + default: + btAssert(false); + break; + } + } + + virtual btAlignedObjectArray* getLagrangianForceArray() + { + return &(m_objective->m_lf); + } + + virtual const btAlignedObjectArray* getIndices() + { + return m_objective->getIndices(); + } + + virtual void setProjection() + { + m_objective->m_projection.setProjection(); + } + + virtual void setLagrangeMultiplier() + { + m_objective->m_projection.setLagrangeMultiplier(); + } + // unused functions virtual void optimize(btAlignedObjectArray& softBodies, bool forceUpdate = false) {} virtual void solveConstraints(btScalar dt) {} diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 198a85dd0..1f3922a04 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -297,91 +297,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) BT_PROFILE("integrateTransforms"); positionCorrection(timeStep); btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->m_reducedModel) - { - for (int r = 0; r < psb->m_reducedDofs.size(); ++r) - psb->m_reducedDofs[r] += timeStep * psb->m_reducedVelocity[r]; - } - else - { - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, 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; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } + m_deformableBodySolver->applyTransforms(timeStep); } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) @@ -398,9 +314,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up the directions in which the velocity does not change in the momentum solve if (m_useProjection) - m_deformableBodySolver->m_objective->m_projection.setProjection(); + m_deformableBodySolver->setProjection(); else - m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + m_deformableBodySolver->setLagrangeMultiplier(); // for explicit scheme, m_backupVelocity = v_{n+1}^* // for implicit scheme, m_backupVelocity = v_n @@ -540,14 +456,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; - m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + m_deformableBodySolver->setStrainLimiting(true); + m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner); } else { m_deformableBodySolver->m_useProjection = false; - m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + m_deformableBodySolver->setStrainLimiting(false); + m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner); } } @@ -689,7 +605,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); bool added = false; for (int i = 0; i < forces.size(); ++i) { @@ -703,14 +619,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL if (!added) { force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + force->setIndices(m_deformableBodySolver->getIndices()); forces.push_back(force); } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); int removed_index = -1; for (int i = 0; i < forces.size(); ++i) { @@ -728,7 +644,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); for (int i = 0; i < forces.size(); ++i) { forces[i]->removeSoftBody(psb); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 97512ed9e..8243e3d8a 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -19,9 +19,9 @@ #include "btSoftMultiBodyDynamicsWorld.h" #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" -#include "btDeformableBodySolver.h" +// #include "btDeformableBodySolver.h" #include "btDeformableMultiBodyConstraintSolver.h" -#include "btReducedSoftBody.h" +#include "BulletReducedSoftBody/btReducedSoftBody.h" #include "btSoftBodyHelpers.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index db55d6b58..5510b96b2 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -855,7 +855,7 @@ public: btVector3 m_windVelocity; btScalar m_restLengthScale; - + // // Api //