summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp639
1 files changed, 0 insertions, 639 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
deleted file mode 100644
index 7f67260ce6..0000000000
--- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
+++ /dev/null
@@ -1,639 +0,0 @@
-/*
- Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
-
- Bullet Continuous Collision Detection and Physics Library
- Copyright (c) 2019 Google Inc. http://bulletphysics.org
- This software is provided 'as-is', without any express or implied warranty.
- In no event will the authors be held liable for any damages arising from the use of this software.
- Permission is granted to anyone to use this software for any purpose,
- including commercial applications, and to alter it and redistribute it freely,
- subject to the following restrictions:
- 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
- 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
- 3. This notice may not be removed or altered from any source distribution.
- */
-
-#include "btDeformableContactProjection.h"
-#include "btDeformableMultiBodyDynamicsWorld.h"
-#include <algorithm>
-#include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
-{
- btScalar residualSquare = 0;
- for (int i = 0; i < numDeformableBodies; ++i)
- {
- for (int j = 0; j < m_softBodies.size(); ++j)
- {
- btCollisionObject* psb = m_softBodies[j];
- if (psb != deformableBodies[i])
- {
- continue;
- }
- for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
- {
- btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
- btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
- {
- btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
- btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
- {
- btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
- btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
- {
- btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
- btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- }
- }
- return residualSquare;
-}
-
-btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
-{
- btScalar residualSquare = 0;
- for (int i = 0; i < numDeformableBodies; ++i)
- {
- for (int j = 0; j < m_softBodies.size(); ++j)
- {
- btCollisionObject* psb = m_softBodies[j];
- if (psb != deformableBodies[i])
- {
- continue;
- }
- for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
- {
- btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
- btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
- {
- btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
- btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
- residualSquare = btMax(residualSquare, localResidualSquare);
- }
- }
- }
- return residualSquare;
-}
-
-void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
-{
- BT_PROFILE("setConstraints");
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- btSoftBody* psb = m_softBodies[i];
- if (!psb->isActive())
- {
- continue;
- }
-
- // set Dirichlet constraint
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- if (psb->m_nodes[j].m_im == 0)
- {
- btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
- m_staticConstraints[i].push_back(static_constraint);
- }
- }
-
- // set up deformable anchors
- for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
- {
- btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
- // skip fixed points
- if (anchor.m_node->m_im == 0)
- {
- continue;
- }
- anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
- btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
- m_nodeAnchorConstraints[i].push_back(constraint);
- }
-
- // set Deformable Node vs. Rigid constraint
- for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
- {
- const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
- // skip fixed points
- if (contact.m_node->m_im == 0)
- {
- continue;
- }
- btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
- m_nodeRigidConstraints[i].push_back(constraint);
- }
-
- // set Deformable Face vs. Rigid constraint
- for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
- {
- const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
- // skip fixed faces
- if (contact.m_c2 == 0)
- {
- continue;
- }
- btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
- m_faceRigidConstraints[i].push_back(constraint);
- }
- }
-}
-
-void btDeformableContactProjection::project(TVStack& x)
-{
-#ifndef USE_MGS
- const int dim = 3;
- for (int index = 0; index < m_projectionsDict.size(); ++index)
- {
- btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
- size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
- if (projectionDirs.size() >= dim)
- {
- // static node
- x[i].setZero();
- continue;
- }
- else if (projectionDirs.size() == 2)
- {
- btVector3 dir0 = projectionDirs[0];
- btVector3 dir1 = projectionDirs[1];
- btVector3 free_dir = btCross(dir0, dir1);
- if (free_dir.safeNorm() < SIMD_EPSILON)
- {
- x[i] -= x[i].dot(dir0) * dir0;
- }
- else
- {
- free_dir.normalize();
- x[i] = x[i].dot(free_dir) * free_dir;
- }
- }
- else
- {
- btAssert(projectionDirs.size() == 1);
- btVector3 dir0 = projectionDirs[0];
- x[i] -= x[i].dot(dir0) * dir0;
- }
- }
-#else
- btReducedVector p(x.size());
- for (int i = 0; i < m_projections.size(); ++i)
- {
- p += (m_projections[i].dot(x) * m_projections[i]);
- }
- for (int i = 0; i < p.m_indices.size(); ++i)
- {
- x[p.m_indices[i]] -= p.m_vecs[i];
- }
-#endif
-}
-
-void btDeformableContactProjection::setProjection()
-{
-#ifndef USE_MGS
- BT_PROFILE("btDeformableContactProjection::setProjection");
- btAlignedObjectArray<btVector3> units;
- units.push_back(btVector3(1, 0, 0));
- units.push_back(btVector3(0, 1, 0));
- units.push_back(btVector3(0, 0, 1));
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- btSoftBody* psb = m_softBodies[i];
- if (!psb->isActive())
- {
- continue;
- }
- for (int j = 0; j < m_staticConstraints[i].size(); ++j)
- {
- int index = m_staticConstraints[i][j].m_node->index;
- m_staticConstraints[i][j].m_node->m_constrained = true;
- if (m_projectionsDict.find(index) == NULL)
- {
- m_projectionsDict.insert(index, units);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- for (int k = 0; k < 3; ++k)
- {
- projections.push_back(units[k]);
- }
- }
- }
- for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
- {
- int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
- m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
- if (m_projectionsDict.find(index) == NULL)
- {
- m_projectionsDict.insert(index, units);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- for (int k = 0; k < 3; ++k)
- {
- projections.push_back(units[k]);
- }
- }
- }
- for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
- {
- int index = m_nodeRigidConstraints[i][j].m_node->index;
- m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
- if (m_nodeRigidConstraints[i][j].m_binding)
- {
- if (m_nodeRigidConstraints[i][j].m_static)
- {
- if (m_projectionsDict.find(index) == NULL)
- {
- m_projectionsDict.insert(index, units);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- for (int k = 0; k < 3; ++k)
- {
- projections.push_back(units[k]);
- }
- }
- }
- else
- {
- if (m_projectionsDict.find(index) == NULL)
- {
- btAlignedObjectArray<btVector3> projections;
- projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
- m_projectionsDict.insert(index, projections);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
- }
- }
- }
- }
- for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
- {
- const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
- if (m_faceRigidConstraints[i][j].m_binding)
- {
- for (int k = 0; k < 3; ++k)
- {
- face->m_n[k]->m_constrained = true;
- }
- }
- for (int k = 0; k < 3; ++k)
- {
- btSoftBody::Node* node = face->m_n[k];
- int index = node->index;
- if (m_faceRigidConstraints[i][j].m_static)
- {
- if (m_projectionsDict.find(index) == NULL)
- {
- m_projectionsDict.insert(index, units);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- for (int l = 0; l < 3; ++l)
- {
- projections.push_back(units[l]);
- }
- }
- }
- else
- {
- if (m_projectionsDict.find(index) == NULL)
- {
- btAlignedObjectArray<btVector3> projections;
- projections.push_back(m_faceRigidConstraints[i][j].m_normal);
- m_projectionsDict.insert(index, projections);
- }
- else
- {
- btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
- projections.push_back(m_faceRigidConstraints[i][j].m_normal);
- }
- }
- }
- }
- }
-#else
- int dof = 0;
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- dof += m_softBodies[i]->m_nodes.size();
- }
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- btSoftBody* psb = m_softBodies[i];
- if (!psb->isActive())
- {
- continue;
- }
- for (int j = 0; j < m_staticConstraints[i].size(); ++j)
- {
- int index = m_staticConstraints[i][j].m_node->index;
- m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
- btAlignedObjectArray<int> indices;
- btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
- indices.push_back(index);
- vecs1.push_back(btVector3(1, 0, 0));
- vecs2.push_back(btVector3(0, 1, 0));
- vecs3.push_back(btVector3(0, 0, 1));
- m_projections.push_back(btReducedVector(dof, indices, vecs1));
- m_projections.push_back(btReducedVector(dof, indices, vecs2));
- m_projections.push_back(btReducedVector(dof, indices, vecs3));
- }
-
- for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
- {
- int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
- m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
- btAlignedObjectArray<int> indices;
- btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
- indices.push_back(index);
- vecs1.push_back(btVector3(1, 0, 0));
- vecs2.push_back(btVector3(0, 1, 0));
- vecs3.push_back(btVector3(0, 0, 1));
- m_projections.push_back(btReducedVector(dof, indices, vecs1));
- m_projections.push_back(btReducedVector(dof, indices, vecs2));
- m_projections.push_back(btReducedVector(dof, indices, vecs3));
- }
- for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
- {
- int index = m_nodeRigidConstraints[i][j].m_node->index;
- m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
- btAlignedObjectArray<int> indices;
- indices.push_back(index);
- btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
- if (m_nodeRigidConstraints[i][j].m_static)
- {
- vecs1.push_back(btVector3(1, 0, 0));
- vecs2.push_back(btVector3(0, 1, 0));
- vecs3.push_back(btVector3(0, 0, 1));
- m_projections.push_back(btReducedVector(dof, indices, vecs1));
- m_projections.push_back(btReducedVector(dof, indices, vecs2));
- m_projections.push_back(btReducedVector(dof, indices, vecs3));
- }
- else
- {
- vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
- m_projections.push_back(btReducedVector(dof, indices, vecs1));
- }
- }
- for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
- {
- const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
- btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
- btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
- for (int k = 0; k < 3; ++k)
- {
- face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
- }
- if (m_faceRigidConstraints[i][j].m_static)
- {
- for (int l = 0; l < 3; ++l)
- {
- btReducedVector rv(dof);
- for (int k = 0; k < 3; ++k)
- {
- rv.m_indices.push_back(face->m_n[k]->index);
- btVector3 v(0, 0, 0);
- v[l] = bary[k];
- rv.m_vecs.push_back(v);
- rv.sort();
- }
- m_projections.push_back(rv);
- }
- }
- else
- {
- btReducedVector rv(dof);
- for (int k = 0; k < 3; ++k)
- {
- rv.m_indices.push_back(face->m_n[k]->index);
- rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
- rv.sort();
- }
- m_projections.push_back(rv);
- }
- }
- }
- btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
- mgs.solve();
- m_projections = mgs.m_out;
-#endif
-}
-
-void btDeformableContactProjection::checkConstraints(const TVStack& x)
-{
- for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
- {
- btVector3 d(0, 0, 0);
- const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
- for (int j = 0; j < lm.m_num_constraints; ++j)
- {
- for (int k = 0; k < lm.m_num_nodes; ++k)
- {
- d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
- }
- }
- // printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
- // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
- }
-}
-
-void btDeformableContactProjection::setLagrangeMultiplier()
-{
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- btSoftBody* psb = m_softBodies[i];
- if (!psb->isActive())
- {
- continue;
- }
- for (int j = 0; j < m_staticConstraints[i].size(); ++j)
- {
- int index = m_staticConstraints[i][j].m_node->index;
- m_staticConstraints[i][j].m_node->m_constrained = true;
- LagrangeMultiplier lm;
- lm.m_num_nodes = 1;
- lm.m_indices[0] = index;
- lm.m_weights[0] = 1.0;
- lm.m_num_constraints = 3;
- lm.m_dirs[0] = btVector3(1, 0, 0);
- lm.m_dirs[1] = btVector3(0, 1, 0);
- lm.m_dirs[2] = btVector3(0, 0, 1);
- m_lagrangeMultipliers.push_back(lm);
- }
- for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
- {
- int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
- m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
- LagrangeMultiplier lm;
- lm.m_num_nodes = 1;
- lm.m_indices[0] = index;
- lm.m_weights[0] = 1.0;
- lm.m_num_constraints = 3;
- lm.m_dirs[0] = btVector3(1, 0, 0);
- lm.m_dirs[1] = btVector3(0, 1, 0);
- lm.m_dirs[2] = btVector3(0, 0, 1);
- m_lagrangeMultipliers.push_back(lm);
- }
-
- for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
- {
- if (!m_nodeRigidConstraints[i][j].m_binding)
- {
- continue;
- }
- int index = m_nodeRigidConstraints[i][j].m_node->index;
- m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
- LagrangeMultiplier lm;
- lm.m_num_nodes = 1;
- lm.m_indices[0] = index;
- lm.m_weights[0] = 1.0;
- if (m_nodeRigidConstraints[i][j].m_static)
- {
- lm.m_num_constraints = 3;
- lm.m_dirs[0] = btVector3(1, 0, 0);
- lm.m_dirs[1] = btVector3(0, 1, 0);
- lm.m_dirs[2] = btVector3(0, 0, 1);
- }
- else
- {
- lm.m_num_constraints = 1;
- lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
- }
- m_lagrangeMultipliers.push_back(lm);
- }
-
- for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
- {
- if (!m_faceRigidConstraints[i][j].m_binding)
- {
- continue;
- }
- btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-
- btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
- LagrangeMultiplier lm;
- lm.m_num_nodes = 3;
-
- for (int k = 0; k < 3; ++k)
- {
- face->m_n[k]->m_constrained = true;
- lm.m_indices[k] = face->m_n[k]->index;
- lm.m_weights[k] = bary[k];
- }
- if (m_faceRigidConstraints[i][j].m_static)
- {
- face->m_pcontact[3] = 1;
- lm.m_num_constraints = 3;
- lm.m_dirs[0] = btVector3(1, 0, 0);
- lm.m_dirs[1] = btVector3(0, 1, 0);
- lm.m_dirs[2] = btVector3(0, 0, 1);
- }
- else
- {
- face->m_pcontact[3] = 0;
- lm.m_num_constraints = 1;
- lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
- }
- m_lagrangeMultipliers.push_back(lm);
- }
- }
-}
-
-//
-void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
-{
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
- {
- const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
- const btSoftBody::Node* node = constraint.m_node;
- if (node->m_im != 0)
- {
- int index = node->index;
- f[index] += constraint.getDv(node) * (1. / node->m_im);
- }
- }
- for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
- {
- const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
- const btSoftBody::Face* face = constraint.getContact()->m_face;
- for (int k = 0; k < 3; ++k)
- {
- const btSoftBody::Node* node = face->m_n[k];
- if (node->m_im != 0)
- {
- int index = node->index;
- f[index] += constraint.getDv(node) * (1. / node->m_im);
- }
- }
- }
- for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
- {
- const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
- const btSoftBody::Face* face = constraint.getContact()->m_face;
- const btSoftBody::Node* node = constraint.getContact()->m_node;
- if (node->m_im != 0)
- {
- int index = node->index;
- f[index] += constraint.getDv(node) * (1. / node->m_im);
- }
- for (int k = 0; k < 3; ++k)
- {
- const btSoftBody::Node* node = face->m_n[k];
- if (node->m_im != 0)
- {
- int index = node->index;
- f[index] += constraint.getDv(node) * (1. / node->m_im);
- }
- }
- }
- }
-}
-
-void btDeformableContactProjection::reinitialize(bool nodeUpdated)
-{
- int N = m_softBodies.size();
- if (nodeUpdated)
- {
- m_staticConstraints.resize(N);
- m_nodeAnchorConstraints.resize(N);
- m_nodeRigidConstraints.resize(N);
- m_faceRigidConstraints.resize(N);
- m_deformableConstraints.resize(N);
- }
- for (int i = 0; i < N; ++i)
- {
- m_staticConstraints[i].clear();
- m_nodeAnchorConstraints[i].clear();
- m_nodeRigidConstraints[i].clear();
- m_faceRigidConstraints[i].clear();
- m_deformableConstraints[i].clear();
- }
-#ifndef USE_MGS
- m_projectionsDict.clear();
-#else
- m_projections.clear();
-#endif
- m_lagrangeMultipliers.clear();
-}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 06:35:35 +0000