diff --git a/examples/ReducedDeformableDemo/ReducedCollide.cpp b/examples/ReducedDeformableDemo/ReducedCollide.cpp index a7f684ea8..f657c5ee2 100644 --- a/examples/ReducedDeformableDemo/ReducedCollide.cpp +++ b/examples/ReducedDeformableDemo/ReducedCollide.cpp @@ -56,22 +56,22 @@ public: void resetCamera() { - float dist = 20; - float pitch = -30; - float yaw = 125; - float targetPos[3] = {-2, 0, 2}; + float dist = 15; + float pitch = -10; + float yaw = 90; + float targetPos[3] = {0, 3, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } void Ctor_RbUpStack() { - float mass = 0.5; + float mass = 8; btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2)); btTransform startTransform; startTransform.setIdentity(); startTransform.setOrigin(btVector3(0,-2,0)); btRigidBody* rb = createRigidBody(mass, startTransform, shape); - rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0)); + rb->setLinearVelocity(btVector3(0, +COLLIDING_VELOCITY, 0)); } void stepSimulation(float deltaTime) @@ -146,7 +146,7 @@ void ReducedCollide::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - rsb->translate(btVector3(0, 10, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 2, 0)); //TODO: add back translate and scale rsb->setStiffnessScale(10); rsb->setDamping(damping_alpha, damping_beta); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index b1aeb68dd..ca2f5c9da 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -40,7 +40,6 @@ void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse) m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); } - // ================= base contact constraints =================== btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint( btReducedSoftBody* rsb, @@ -61,12 +60,43 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr m_impulseFactor = c.m_c0; m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA); m_tangentImpulseFactor = 0; + + m_relPosA = c.m_c1; + + btRigidBody* rb = m_contact->m_cti.m_colObj ? (btRigidBody*)btRigidBody::upcast(m_contact->m_cti.m_colObj) : nullptr; + if (!rb) + { + btAssert(false); + } + else + { + m_collideStatic = rb->isStaticObject(); + } +} + +void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body) +{ + m_solverBody = &solver_body; + m_linearComponent = m_contactNormalA * m_solverBody->internalGetInvMass(); + btVector3 torqueAxis = m_relPosA.cross(m_contactNormalA); + m_angularComponent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis; +} + +btVector3 btReducedDeformableRigidContactConstraint::getVa() const +{ + btVector3 Va(0, 0, 0); + if (!m_collideStatic) + { + Va = btDeformableRigidContactConstraint::getVa(); + } + return Va; } btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { btVector3 Va = getVa(); - btVector3 deltaVa = Va - m_bufferVelocityA; + // btVector3 deltaVa = Va - m_bufferVelocityA; + btVector3 deltaVa = getDeltaVa(); btVector3 deltaVb = getDeltaVb(); std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; @@ -74,6 +104,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // get delta relative velocity and magnitude (i.e., how much impulse has been applied?) btVector3 deltaV_rel = deltaVa - deltaVb; btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA); + std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; // get the normal impulse to be applied btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor; @@ -150,36 +181,34 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont applyImpulse(impulse); // apply impulse to the rigid/multibodies involved and change their velocities - const btSoftBody::sCti& cti = m_contact->m_cti; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (!m_collideStatic) { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) + const btSoftBody::sCti& cti = m_contact->m_cti; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - std::cout << "called here??\n"; - rigidCol->applyImpulse(impulse, m_contact->m_c1); + m_solverBody->internalApplyImpulse(m_linearComponent, m_angularComponent, -deltaImpulse); + // m_solverBody->internalApplyImpulse(m_linearComponent, m_angularComponent, -deltaImpulse_tangent); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btAssert(false); //TODO: unsupported yet + // btMultiBodyLinkCollider* multibodyLinkCol = 0; + // multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + // if (multibodyLinkCol) + // { + // const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // // apply normal component of the impulse + // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // if (impulse_tangent.norm() > SIMD_EPSILON) + // { + // // apply tangential component of the impulse + // const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + // const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + // } + // } } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btAssert(false); //TODO: unsupported yet - // btMultiBodyLinkCollider* multibodyLinkCol = 0; - // multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - // if (multibodyLinkCol) - // { - // const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // // apply normal component of the impulse - // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - // if (impulse_tangent.norm() > SIMD_EPSILON) - // { - // // apply tangential component of the impulse - // const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); - // const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); - // } - // } } return residualSquare; } @@ -192,7 +221,6 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta btScalar dt) : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) { - m_relPosA = contact.m_c1; m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin(); warmStarting(); } @@ -233,6 +261,16 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const return m_node->m_v; } +btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const +{ + btVector3 deltaVa(0, 0, 0); + if (!m_collideStatic) + { + deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_relPosA.cross(m_solverBody->internalGetDeltaAngularVelocity()); + } + return deltaVa; +} + btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const { return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h index 9e448eb14..9494c5d99 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h @@ -31,6 +31,8 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint { public: + bool m_collideStatic; // flag for collision with static object + btReducedSoftBody* m_rsb; btSolverBody* m_solverBody; btScalar m_dt; @@ -48,12 +50,14 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac btVector3 m_contactNormalA; // for rigid body btVector3 m_contactNormalB; // for reduced deformable body btVector3 m_contactTangent; // tangential direction of the relative velocity - btVector3 m_relPosA; // relative position of the contact point for A + btVector3 m_relPosA; // relative position of the contact point for A (rigid) btVector3 m_relPosB; // relative position of the contact point for B btMatrix3x3 m_impulseFactor; // total impulse matrix btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration btVector3 m_bufferVelocityB; + btVector3 m_linearComponent; // linear components for the solver body + btVector3 m_angularComponent; // angular components for the solver body btReducedDeformableRigidContactConstraint(btReducedSoftBody* rsb, const btSoftBody::DeformableRigidContact& c, @@ -63,8 +67,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac btReducedDeformableRigidContactConstraint() {} virtual ~btReducedDeformableRigidContactConstraint() {} - void setupSolverBody(btRigidBody* rigid_body); - + void setSolverBody(btSolverBody& solver_body); + virtual void warmStarting() {} virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); @@ -73,6 +77,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later + virtual btVector3 getVa() const; + virtual btVector3 getDeltaVa() const = 0; virtual btVector3 getDeltaVb() const = 0; }; @@ -95,6 +101,9 @@ class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformable // get the velocity of the deformable node in contact virtual btVector3 getVb() const; + // get the velocity change of the rigid body + virtual btVector3 getDeltaVa() const; + // get velocity change of the node in contat virtual btVector3 getDeltaVb() const; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index e571e8517..36599e9ae 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -3,6 +3,7 @@ btReducedSoftBodySolver::btReducedSoftBodySolver() { + m_reducedSolver = true; m_dampingAlpha = 0; m_dampingBeta = 0; m_gravity = btVector3(0, 0, 0); @@ -240,6 +241,29 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob } } +void btReducedSoftBodySolver::pairConstraintWithSolverBody(btSolverBody& solverBody) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // node vs rigid contact + for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k) + { + btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][k]; + constraint.setSolverBody(solverBody); + } + + // face vs rigid contact + // for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + // { + // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + // residualSquare = btMax(residualSquare, localResidualSquare); + // } + } +} + btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; @@ -306,6 +330,7 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de void btReducedSoftBodySolver::deformableBodyInternalWriteBack() { + // reduced deformable update for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 5d96ae625..842fb904d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -44,6 +44,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver // set up contact constraints virtual void setConstraints(const btContactSolverInfo& infoGlobal); + // pair rigid contact constraint with solver body + virtual void pairConstraintWithSolverBody(btSolverBody& solverBody); + // solve all constraints (fixed and contact) virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 68fbbe76f..bff254612 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); + m_reducedSolver = false; } btDeformableBodySolver::~btDeformableBodySolver() diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 9d0c684b9..cf1e1ac24 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -46,6 +46,7 @@ protected: int m_maxNewtonIterations; // max number of newton iterations btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance bool m_lineSearch; // If true, use newton's method with line search under implicit scheme + bool m_reducedSolver; // flag for reduced soft body solver public: // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; @@ -199,6 +200,14 @@ public: m_objective->m_projection.setLagrangeMultiplier(); } + virtual bool isReducedSolver() + { + return m_reducedSolver; + } + + // pair rigid contact constraint with solver body + virtual void pairConstraintWithSolverBody(btSolverBody& solverBody) {} + virtual void deformableBodyInternalWriteBack() {} // unused functions diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 68a6b9e90..db5b2f50c 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -21,6 +21,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { { + // pair deformable body with solver body + pairDeformableAndSolverBody(bodies, numBodies, deformableBodies, numDeformableBodies, infoGlobal); + ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); @@ -89,6 +92,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < numBodies; i++) { int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); @@ -105,6 +114,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject* void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; @@ -116,6 +131,32 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } + +void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + if (!m_deformableSolver->isReducedSolver()) + { + return; + } + + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + m_deformableSolver->pairConstraintWithSolverBody(solverBody); + } + } + + for (int i = 0; i < numDeformableBodies; ++i) + { + // + } +} + void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 94aabce83..e95f51273 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -43,6 +43,9 @@ protected: // write the velocity of the underlying rigid body to the the the solver body void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + // let each deformable body knows which solver body is in constact + void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 1e26e7881..5e7d43921 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1708,7 +1708,6 @@ struct btSoftColliders { btMatrix3x3 rigid_impulse_factor = ImpulseMatrixNonInverse(1, n.m_effectiveMass_inv, imb, iwi, ra); c.m_c0 = psb->getImpulseFactor(n.index) + rigid_impulse_factor; //impulse factor K (not the inverse) - // c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin(); } else {