From c2601663e4359347a44ec93e19e0ad7d437fdae0 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Wed, 1 Sep 2021 16:36:52 -0400 Subject: [PATCH] debugging moving rigid vs reduced deformable contact --- examples/ReducedDeformableDemo/FreeFall.cpp | 27 ++++-- .../ReducedDeformableDemo/ReducedCollide.cpp | 7 +- .../btReducedDeformableContactConstraint.cpp | 88 ++++++++++++++++--- .../btReducedSoftBodySolver.cpp | 2 +- .../btDeformableMultiBodyConstraintSolver.cpp | 2 +- 5 files changed, 100 insertions(+), 26 deletions(-) diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index b138e88bb..c0eb74d14 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -66,22 +66,33 @@ public: void Ctor_RbUpStack() { float mass = 10; - btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); + btCollisionShape* shape = new btBoxShape(btVector3(3, 3, 3)); btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0, 12, 0)); - btRigidBody* rb0 = createRigidBody(mass, startTransform, shape); - rb0->setLinearVelocity(btVector3(0, 0, 0)); + // startTransform.setOrigin(btVector3(0, 12, 0)); + // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape); + // rb0->setLinearVelocity(btVector3(0, 0, 0)); - // startTransform.setOrigin(btVector3(0,8,0)); - // btRigidBody* rb1 = createRigidBody(mass, startTransform, shape); - // rb1->setLinearVelocity(btVector3(0, 0, 0)); + startTransform.setOrigin(btVector3(0,6,0)); + // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0)); + btRigidBody* rb1 = createRigidBody(mass, startTransform, shape); + rb1->setLinearVelocity(btVector3(0, 0, 0)); } void stepSimulation(float deltaTime) { float internalTimeStep = 1. / 60.f; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + + // btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + // std::cout << "rigid_array size=" << deformableWorld->getNonStaticRigidBodies().size() << '\n'; + // for (int i = 0; i < deformableWorld->getNonStaticRigidBodies().size(); ++i) + // { + // btRigidBody* rb = deformableWorld->getNonStaticRigidBodies()[i]; + // std::cout << "end of timestep, rigid_vel: " << rb->getLinearVelocity()[0] << '\t' + // << rb->getLinearVelocity()[1] << '\t' + // << rb->getLinearVelocity()[2] << '\n'; + // } } virtual void renderScene() @@ -151,7 +162,7 @@ void FreeFall::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.5, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(10); rsb->setDamping(damping_alpha, damping_beta); diff --git a/examples/ReducedDeformableDemo/ReducedCollide.cpp b/examples/ReducedDeformableDemo/ReducedCollide.cpp index 3b76db3bc..039c80a09 100644 --- a/examples/ReducedDeformableDemo/ReducedCollide.cpp +++ b/examples/ReducedDeformableDemo/ReducedCollide.cpp @@ -31,7 +31,7 @@ // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.0; -static btScalar COLLIDING_VELOCITY = 15; +static btScalar COLLIDING_VELOCITY = 4; static int start_mode = 6; static int num_modes = 1; @@ -66,10 +66,11 @@ public: void Ctor_RbUpStack() { float mass = 10; - btCollisionShape* shape = new btBoxShape(btVector3(3, 3, 3)); + btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0,-3,0)); + startTransform.setOrigin(btVector3(0,-2,0)); + // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0)); btRigidBody* rb = createRigidBody(mass, startTransform, shape); rb->setActivationState(DISABLE_DEACTIVATION); rb->setLinearVelocity(btVector3(0, +COLLIDING_VELOCITY, 0)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index 81bf9b092..e7ea8fab3 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -78,7 +78,7 @@ void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solv { m_solverBody = &solver_body; m_linearComponentNormal = m_contactNormalA * m_solverBody->internalGetInvMass(); - btVector3 torqueAxis = m_relPosA.cross(m_contactNormalA); + btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA); m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis; } @@ -96,26 +96,47 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont { btVector3 Va = getVa(); // btVector3 deltaVa = Va - m_bufferVelocityA; + if (!m_collideStatic) + { + std::cout << "moving collision!!!\n"; + // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t' + // << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t' + // << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n'; + } 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'; + + if (!m_collideStatic) + { + std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; + std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; + } // 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"; - std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; + + if (!m_collideStatic) + { + std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; + std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; + } // get the normal impulse to be applied btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor; + if (!m_collideStatic) + { + std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n"; + std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n'; + } + { // cumulative impulse that has been applied btScalar sum = m_appliedNormalImpulse + deltaImpulse; // if the cumulative impulse is pushing the object into the rigid body, set it zero if (sum < 0) { - std::cout <<"set zeroed!!!\n"; + if (!m_collideStatic) std::cout <<"set zeroed!!!\n"; deltaImpulse = -m_appliedNormalImpulse; m_appliedNormalImpulse = 0; } @@ -124,8 +145,12 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont m_appliedNormalImpulse = sum; } } - std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; - std::cout << "deltaImpulse: " << deltaImpulse << '\n'; + + if (!m_collideStatic) + { + std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; + std::cout << "deltaImpulse: " << deltaImpulse << '\n'; + } // residual is the nodal normal velocity change in current iteration btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual @@ -187,8 +212,30 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont const btSoftBody::sCti& cti = m_contact->m_cti; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { + // if (!m_collideStatic) + // { + // std::cout << "rigid impulse applied!!\n"; + // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + // } + m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -deltaImpulse); - // m_solverBody->internalApplyImpulse(m_linearComponent, m_angularComponent, -deltaImpulse_tangent); + + if (!m_collideStatic) + { + std::cout << "after\n"; + std::cout << "rigid impulse applied!!\n"; + std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + } } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -255,6 +302,17 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting() // get the initial estimate of impulse magnitude to be applied // m_rhs = -velocity_error * m_normalImpulseFactorInv; m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor; + + // warm starting + // applyImpulse(m_rhs * m_contactNormalA); + // if (!m_collideStatic) + // { + // const btSoftBody::sCti& cti = m_contact->m_cti; + // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + // { + // m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs); + // } + // } } btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const @@ -289,13 +347,17 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody: void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { - std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); - std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; - btVector3 v_after = getDeltaVb() + m_node->m_v; - std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; + if (!m_collideStatic) + { + std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; + std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; + btVector3 v_after = getDeltaVb() + m_node->m_v; + std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; + } + // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; } // ================= face vs rigid constraints =================== diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 36599e9ae..701f9981b 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -222,7 +222,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob m_nodeRigidConstraints[i].push_back(constraint); rsb->m_contactNodesList.push_back(contact.m_node->index); } - std::cout << "contact list size: " << rsb->m_contactNodesList.size() << "\n"; + std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n"; std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; // set Deformable Face vs. Rigid constraint diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index ca3ba13f7..bfc749987 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -67,7 +67,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b } } std::cout << "======next step=========\n"; - // std::this_thread::sleep_for (std::chrono::milliseconds(100)); + // std::this_thread::sleep_for (std::chrono::milliseconds(300)); } return 0.f; }