Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

fixed the incorrect solverBody pairing issue of the reduced deformable solver. still have the problem that the reduced deformable sometimes doesn't show up

Browse Source
This commit is contained in:
Jingyu Chen
2022-02-13 21:29:39 -08:00
parent f8abdd1fe1
commit c159305910
10 changed files with 80 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@@ -55,14 +55,14 @@ public:
void resetCamera()
{
// float dist = 6;
// float pitch = -10;
// float yaw = 90;
// float targetPos[3] = {0, 2, 0};
float dist = 10;
float pitch = -30;
float yaw = 125;
float dist = 6;
float pitch = -10;
float yaw = 90;
float targetPos[3] = {0, 2, 0};
// float dist = 10;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {0, 2, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@@ -81,12 +81,12 @@ public:
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
// rb1->setLinearVelocity(btVector3(0, 0, 4));
rb1->setLinearVelocity(btVector3(0, -4, 0));
}
void createReducedDeformableObject(const btVector3& origin, const btQuaternion& rotation)
{
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
@@ -168,27 +168,18 @@ void FreeFall::initPhysics()
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
btVector3 gravity = btVector3(0, 0, 0);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
// 3x3 torus
createReducedDeformableObject(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createReducedDeformableObject(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
// Ctor_RbUpStack(btVector3(0, 10, 0));
// Ctor_RbUpStack(btVector3(0, 10, 4));
// Ctor_RbUpStack(btVector3(0, 10, 4));
// Ctor_RbUpStack(btVector3(-4, 10, 0));
// Ctor_RbUpStack(btVector3(4, 10, 0));
// 2 reduced deformable cubes
createReducedDeformableObject(btVector3(0, 1, -2), btQuaternion());
// createReducedDeformableObject(btVector3(0, 1, 2), btQuaternion());
// 2 rigid cubes
Ctor_RbUpStack(btVector3(0, 3, -2));
Ctor_RbUpStack(btVector3(0, 3, 2));
// create a static rigid box as the ground
{
@@ -200,7 +191,7 @@ void FreeFall::initPhysics()
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
groundTransform.setOrigin(btVector3(0, -2, 0));
// groundTransform.setOrigin(btVector3(0, 0, 6));
// groundTransform.setOrigin(btVector3(0, -50, 0));
{
@@ -216,9 +207,9 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);

14
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@@ -317,22 +317,22 @@ void ReducedMotorGrasp::initPhysics()
// create volumetric reduced deformable body
{
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.001);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->scale(btVector3(0.075, 0.075, 0.075));
rsb->setTotalMass(1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 0.1, 0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->scale(btVector3(0.075, 0.075, 0.075)); // roughly about 10cm x 10cm x 10cm
rsb->setTotalMass(1); // about 200 gram
// rsb->setRigidVelocity(btVector3(0, 1, 0));

2
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp
View File

@@ -8,7 +8,7 @@
btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = false; //! only use rigid frame to debug
m_rigidOnly = true; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;

44
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
View File

@@ -1,5 +1,5 @@
#include "btReducedDeformableBodySolver.h"
#include "../btDeformableMultiBodyDynamicsWorld.h"
#include "btReducedDeformableBody.h"
btReducedDeformableBodySolver::btReducedDeformableBodySolver()
{
@@ -233,45 +233,9 @@ void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& in
m_nodeRigidConstraints[i].push_back(constraint);
rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
}
// std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
// std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].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
// for (int j = 0; j < rsb->m_faceRigidContacts.size(); ++j)
// {
// const btSoftBody::DeformableFaceRigidContact& contact = rsb->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 btReducedDeformableBodySolver::pairConstraintWithSolverBody(btSolverBody& solverBody)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(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);
// }
}
}
@@ -328,7 +292,7 @@ btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObjec
// handle contact constraint
// node vs rigid contact
// std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
{
btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];

6
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h
View File

@@ -1,8 +1,7 @@
#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
#include "../btDeformableBodySolver.h"
#include "btReducedDeformableBody.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "btReducedDeformableContactConstraint.h"
class btReducedDeformableBody;
@@ -45,9 +44,6 @@ class btReducedDeformableBodySolver : 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);

3
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp
View File

@@ -78,10 +78,11 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
}
void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body)
void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
{
if (!m_collideMultibody)
{
m_solverBodyId = bodyId;
m_solverBody = &solver_body;
m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);

3
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
View File

@@ -45,6 +45,7 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
bool m_collideMultibody; // flag for collision with multibody
int m_nodeQueryIndex;
int m_solverBodyId; // for debugging
btReducedDeformableBody* m_rsb;
btSolverBody* m_solverBody;
@@ -92,7 +93,7 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
btReducedDeformableRigidContactConstraint() {}
virtual ~btReducedDeformableRigidContactConstraint() {}
void setSolverBody(btSolverBody& solver_body);
void setSolverBody(const int bodyId, btSolverBody& solver_body);
virtual void warmStarting() {}

3
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -212,9 +212,6 @@ public:
return m_reducedSolver;
}
// pair rigid contact constraint with solver body
virtual void pairConstraintWithSolverBody(btSolverBody& solverBody) {}
virtual void deformableBodyInternalWriteBack() {}
// unused functions

52
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -14,6 +14,7 @@
*/
#include "btDeformableMultiBodyConstraintSolver.h"
#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include <iostream>
#include <thread>
// override the iterations method to include deformable/multibody contact
@@ -21,7 +22,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
{
{
// pair deformable body with solver body
pairDeformableAndSolverBody(bodies, numBodies, infoGlobal);
pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
@@ -41,6 +42,10 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);
std::cout << "------------Iteration " << iteration << "------------\n";
std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
@@ -57,6 +62,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
m_deformableSolver->deformableBodyInternalWriteBack();
std::cout << "[===================Next Step===================]\n";
break;
}
}
@@ -124,23 +130,49 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
if (!m_deformableSolver->isReducedSolver())
{
return;
}
for (int i = 0; i < numBodies; i++)
btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
for (int i = 0; i < numDeformableBodies; ++i)
{
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
{
btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
m_deformableSolver->pairConstraintWithSolverBody(solverBody);
}
if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
{
btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
// object index in the solver body pool
int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
if (body && body->getInvMass())
{
std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
constraint.setSolverBody(bodyId, solverBody);
}
}
}
// for (int j = 0; j < numBodies; j++)
// {
// int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
// btRigidBody* body = btRigidBody::upcast(bodies[j]);
// if (body && body->getInvMass())
// {
// btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
// m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
// }
// }
}
}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@@ -44,7 +44,7 @@ protected:
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, const btContactSolverInfo& infoGlobal);
void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, 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);