diff options
Diffstat (limited to 'src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp')
| -rw-r--r-- | src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp | 106 |
1 files changed, 88 insertions, 18 deletions
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 030cbaf90..bc7b31273 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t beforeSolverCallbacks(timeStep); - // ///solve contact constraints and then deformable bodies momemtum equation + ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -298,7 +298,83 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) BT_PROFILE("integrateTransforms"); positionCorrection(timeStep); btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - m_deformableBodySolver->applyTransforms(timeStep); + 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(); + } } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) @@ -315,9 +391,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->setProjection(); + m_deformableBodySolver->m_objective->m_projection.setProjection(); else - m_deformableBodySolver->setLagrangeMultiplier(); + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); // for explicit scheme, m_backupVelocity = v_{n+1}^* // for implicit scheme, m_backupVelocity = v_n @@ -443,12 +519,6 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time m_deformableBodySolver->predictMotion(timeStep); } -void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity) -{ - btDiscreteDynamicsWorld::setGravity(gravity); - m_deformableBodySolver->setGravity(gravity); -} - void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; @@ -463,14 +533,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; - m_deformableBodySolver->setStrainLimiting(true); - m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner); + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; } else { m_deformableBodySolver->m_useProjection = false; - m_deformableBodySolver->setStrainLimiting(false); - m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner); + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; } } @@ -612,7 +682,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; bool added = false; for (int i = 0; i < forces.size(); ++i) { @@ -626,14 +696,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL if (!added) { force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->getIndices()); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); forces.push_back(force); } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; int removed_index = -1; for (int i = 0; i < forces.size(); ++i) { @@ -651,7 +721,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; for (int i = 0; i < forces.size(); ++i) { forces[i]->removeSoftBody(psb); |