Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2550 from xhan0619/master

Browse Source 
Group deformable constraint solves by islands
This commit is contained in:
erwincoumans
2019-12-20 16:26:32 -08:00
committed by GitHub
parent d6bde9271b 6a8973d2f4
commit 830f0a9565
15 changed files with 806 additions and 544 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
ExampleEntry(1, "Gripper Grasp", "Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc, eGRIPPER_GRASP),
ExampleEntry(1, "Two Point Grasp", "Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
ExampleEntry(1, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
ExampleEntry(1, "One Motor Gripper Grasp", "Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),

131
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -294,6 +294,102 @@ public:
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
if (results.m_uniqueObjectIds.size() == 1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d", numJoints);
for (int i = 0; i < numJoints; i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo);
b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
}
for (int i = 0; i < 8; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
}
}
}
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, 0, -0.2);
args.m_startOrientation.setEulerZYX(0, 0, 0);
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
revoluteJoint1.m_parentFrame[2] = 0.02;
revoluteJoint1.m_parentFrame[3] = 0;
revoluteJoint1.m_parentFrame[4] = 0;
revoluteJoint1.m_parentFrame[5] = 0;
revoluteJoint1.m_parentFrame[6] = 1.0;
revoluteJoint1.m_childFrame[0] = 0;
revoluteJoint1.m_childFrame[1] = 0;
revoluteJoint1.m_childFrame[2] = 0;
revoluteJoint1.m_childFrame[3] = 0;
revoluteJoint1.m_childFrame[4] = 0;
revoluteJoint1.m_childFrame[5] = 0;
revoluteJoint1.m_childFrame[6] = 1.0;
revoluteJoint1.m_jointAxis[0] = 1.0;
revoluteJoint1.m_jointAxis[1] = 0.0;
revoluteJoint1.m_jointAxis[2] = 0.0;
revoluteJoint1.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint2;
revoluteJoint2.m_parentFrame[0] = 0.055;
revoluteJoint2.m_parentFrame[1] = 0;
revoluteJoint2.m_parentFrame[2] = 0.02;
revoluteJoint2.m_parentFrame[3] = 0;
revoluteJoint2.m_parentFrame[4] = 0;
revoluteJoint2.m_parentFrame[5] = 0;
revoluteJoint2.m_parentFrame[6] = 1.0;
revoluteJoint2.m_childFrame[0] = 0;
revoluteJoint2.m_childFrame[1] = 0;
revoluteJoint2.m_childFrame[2] = 0;
revoluteJoint2.m_childFrame[3] = 0;
revoluteJoint2.m_childFrame[4] = 0;
revoluteJoint2.m_childFrame[5] = 0;
revoluteJoint2.m_childFrame[6] = 1.0;
revoluteJoint2.m_jointAxis[0] = 1.0;
revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
{
SliderParams slider("Vertical velocity", &sGripperVerticalVelocity);
slider.m_minVal = -2;
slider.m_maxVal = 2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -1;
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
@@ -326,10 +422,19 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
args.m_springElasticStiffness = .1;
args.m_springDampingStiffness = .0004;
args.m_springBendingStiffness = 1;
args.m_frictionCoeff = 1;
args.m_useSelfCollision = false;
// args.m_useFaceContact = true;
args.m_useBendingSprings = true;
args.m_startPosition.setValue(0, 0, 0);
args.m_startOrientation.setValue(0, 0, 1, 1);
m_robotSim.loadDeformableBody("flat_napkin.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -371,7 +476,8 @@ public:
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}
m_robotSim.setNumSimulationSubSteps(8);
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{
@@ -462,6 +568,21 @@ public:
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
}
}
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
int fingerJointIndices[2] = {0, 1};
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
double maxTorqueValues[2] = {250.0, 50.0};
for (int i = 0; i < 2; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
}
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
eONE_MOTOR_GRASP = 4,
eGRASP_SOFT_BODY = 8,
eSOFTBODY_MULTIBODY_COUPLING = 16,
eGRASP_DEFORMABLE_CLOTH = 32,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

42
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}
void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
b3InitResetSimulationSetFlags(command, flag);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
{
if (!isConnected())
@@ -1151,6 +1164,35 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
b3LoadSoftBodySetScale(command, args.m_scale);
b3LoadSoftBodySetMass(command, args.m_mass);
b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
if (args.m_NeoHookeanMu > 0)
{
b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
}
if (args.m_springElasticStiffness > 0)
{
b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
}
b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
{
mouseEventsData->m_numMouseEvents = 0;

62
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -89,6 +89,64 @@ struct b3RobotSimulatorLoadSoftBodyArgs
}
};
struct b3RobotSimulatorLoadDeformableBodyArgs
{
btVector3 m_startPosition;
btQuaternion m_startOrientation;
double m_scale;
double m_mass;
double m_collisionMargin;
double m_springElasticStiffness;
double m_springDampingStiffness;
double m_springBendingStiffness;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
double m_NeoHookeanDamping;
bool m_useSelfCollision;
bool m_useFaceContact;
bool m_useBendingSprings;
double m_frictionCoeff;
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin),
m_springElasticStiffness(-1),
m_springDampingStiffness(-1),
m_springBendingStiffness(-1),
m_NeoHookeanMu(-1),
m_NeoHookeanDamping(-1),
m_useSelfCollision(false),
m_useFaceContact(false),
m_useBendingSprings(false),
m_frictionCoeff(0)
{
}
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
{
b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
}
b3RobotSimulatorLoadDeformableBodyArgs()
{
b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
}
b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin)
: m_startPosition(btVector3(0, 0, 0)),
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin)
{
}
};
struct b3RobotSimulatorLoadFileResults
{
btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -534,6 +592,8 @@ public:
void syncBodies();
void resetSimulation();
void resetSimulation(int flag);
btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
btVector3 getEulerFromQuaternion(const btQuaternion &quat);
@@ -685,6 +745,8 @@ public:
int getConstraintUniqueId(int serialIndex);
void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();

6
src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
View File

@@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btCollisionObject*> m_softBodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
@@ -194,6 +195,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
{
m_bodies.push_back(bodies[i]);
}
else
{
m_softBodies.push_back(bodies[i]);
}
}
for (i = 0; i < numManifolds; i++)
m_manifolds.push_back(manifolds[i]);
@@ -231,6 +236,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);

46
src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h Normal file
View File

@@ -0,0 +1,46 @@
//
// DeformableBodyInplaceSolverIslandCallback.h
// BulletSoftBody
//
// Created by Xuchen Han on 12/16/19.
//
#ifndef DeformableBodyInplaceSolverIslandCallback_h
#define DeformableBodyInplaceSolverIslandCallback_h
struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
{
btDeformableMultiBodyConstraintSolver* m_deformableSolver;
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
{
}
virtual void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
{
m_deformableSolver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
#endif /* DeformableBodyInplaceSolverIslandCallback_h */

4
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints()
m_objective->setConstraints();
}
btScalar btDeformableBodySolver::solveContactConstraints()
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
{
BT_PROFILE("solveContactConstraints");
btScalar maxSquaredResidual = m_objective->m_projection.update();
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
return maxSquaredResidual;
}

2
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -65,7 +65,7 @@ public:
virtual void solveDeformableConstraints(btScalar solverdt);
// solve the contact between deformable and rigid as well as among deformables
btScalar solveContactConstraints();
btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
// solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

945
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -17,527 +17,492 @@
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
btScalar btDeformableContactProjection::update()
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
{
btScalar residualSquare = 0;
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
btScalar localResidualSquare = constraints[i].solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// anchor constraints
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
{
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
return residualSquare;
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();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
{
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
}
return residualSquare;
}
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
constraints[i].setPenetrationScale(infoGlobal.m_deformable_erp);
}
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
constraint->setPenetrationScale(infoGlobal.m_deformable_erp);
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
// node constraints
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
}
// face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
}
}
}
btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// anchor constraints
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
{
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
return residualSquare;
btScalar residualSquare = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
// node constraints
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// anchor constraints
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
{
BT_PROFILE("setConstraints");
// set Dirichlet constraint
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
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]);
m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint);
}
}
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
BT_PROFILE("setConstraints");
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
// 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;
}
if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
{
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
m_nodeAnchorConstraints.insert(anchor.m_node->index, 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);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList;
constraintsList.push_back(constraint);
m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
constraintsList.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 = new btDeformableFaceRigidContactConstraint(contact);
btVector3 va = constraint->getVa();
btVector3 vb = constraint->getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
m_allFaceConstraints.push_back(constraint);
// add face constraints to each of the nodes
for (int k = 0; k < 3; ++k)
{
btSoftBody::Node* node = contact.m_face->m_n[k];
// static node does not need to own face/rigid constraint
if (node->m_im != 0)
{
if (m_faceRigidConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_faceRigidConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
else
{
delete constraint;
}
}
// set Deformable Face vs. Deformable Node constraint
for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
{
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact);
btVector3 va = constraint->getVa();
btVector3 vb = constraint->getVb();
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, contact.m_normal);
if (dn > -SIMD_EPSILON)
{
btSoftBody::Node* node = contact.m_node;
btSoftBody::Face* face = contact.m_face;
m_allFaceConstraints.push_back(constraint);
if (node->m_im != 0)
{
if (m_deformableConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
constraintsList.push_back(constraint);
}
}
// add face constraints to each of the nodes
for (int k = 0; k < 3; ++k)
{
btSoftBody::Node* node = face->m_n[k];
// static node does not need to own face/rigid constraint
if (node->m_im != 0)
{
if (m_deformableConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
else
{
delete constraint;
}
}
}
// 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]);
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);
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);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
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);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
m_faceRigidConstraints[i].push_back(constraint);
}
}
// set Deformable Face vs. Deformable Node constraint
for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
{
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
btDeformableFaceNodeContactConstraint constraint(contact);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, contact.m_normal);
if (dn > -SIMD_EPSILON)
{
m_deformableConstraints[i].push_back(constraint);
}
}
}
}
void btDeformableContactProjection::project(TVStack& x)
{
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.norm() < SIMD_EPSILON)
{
x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1;
}
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;
}
}
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;
x[i] -= x[i].dot(dir1) * dir1;
}
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;
}
}
}
void btDeformableContactProjection::setProjection()
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
int index = psb->m_nodes[j].index;
bool hasConstraint = false;
bool existStaticConstraint = false;
btVector3 averagedNormal(0,0,0);
btAlignedObjectArray<btVector3> normals;
if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
{
existStaticConstraint = true;
hasConstraint = true;
}
// accumulate normals from Deformable Node vs. Rigid constraints
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k].m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k].m_normal;
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// accumulate normals from Deformable Face vs. Rigid constraints
if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k]->m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k]->m_normal;
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// accumulate normals from Deformable Node vs. Deformable Face constraints
if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k]->m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k]->m_normal;
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// build projections
if (!hasConstraint)
{
continue;
}
btAlignedObjectArray<btVector3> projections;
if (existStaticConstraint)
{
projections.push_back(btVector3(1,0,0));
projections.push_back(btVector3(0,1,0));
projections.push_back(btVector3(0,0,1));
}
else
{
bool averageExists = (averagedNormal.length2() > SIMD_EPSILON);
averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0);
if (averageExists)
{
projections.push_back(averagedNormal);
}
for (int k = 0; k < normals.size(); ++k)
{
const btVector3& local_normal = normals[k];
// add another projection direction if it deviates from the average by more than about 15 degrees
if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25)
{
projections.push_back(local_normal);
}
}
}
m_projectionsDict.insert(index, projections);
}
}
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;
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;
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;
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;
for (int k = 0; k < 3; ++k)
{
const 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 k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
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);
}
}
}
}
for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
{
const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
for (int k = 0; k < 3; ++k)
{
const btSoftBody::Node* node = face->m_n[k];
int index = node->index;
if (m_deformableConstraints[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_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[i][j].m_normal);
}
}
}
const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
int index = node->index;
if (m_deformableConstraints[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_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[i][j].m_normal);
}
}
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
// loop over constraints
for (int i = 0; i < f.size(); ++i)
{
if (m_projectionsDict.find(i) != NULL)
{
// doesn't need to add friction force for fully constrained vertices
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i];
if (projectionDirs.size() >= 3)
{
continue;
}
}
// add friction contribution from Face vs. Node
if (m_nodeRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
btSoftBody::Node* node = constraint.getContact()->m_node;
// it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
f[i] += constraint.getDv(node)* (1./node->m_im);
}
}
// add friction contribution from Face vs. Rigid
if (m_faceRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
btSoftBody::Face* face = constraint->getContact()->m_face;
// it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
for (int k = 0; k < 3; ++k)
{
if (face->m_n[k]->index == i)
{
if (face->m_n[k]->m_im != 0)
{
f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
}
break;
}
}
}
}
if (m_deformableConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j];
btSoftBody::Face* face = constraint->getContact()->m_face;
btSoftBody::Node* node = constraint->getContact()->m_node;
// it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
if (node->index == i)
{
if (node->m_im != 0)
{
f[i] += constraint->getDv(node)*(1./node->m_im);
}
}
else
{
for (int k = 0; k < 3; ++k)
{
if (face->m_n[k]->index == i)
{
if (face->m_n[k]->m_im != 0)
{
f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
}
break;
}
}
}
}
}
}
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)
{
m_staticConstraints.clear();
m_nodeAnchorConstraints.clear();
m_nodeRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_deformableConstraints.clear();
m_projectionsDict.clear();
for (int i = 0; i < m_allFaceConstraints.size(); ++i)
{
delete m_allFaceConstraints[i];
}
m_allFaceConstraints.clear();
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();
}
m_projectionsDict.clear();
}

35
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -28,22 +28,33 @@ public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
// map from node index to static constraint
btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// map from node index to node rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// map from node index to node anchor constraint
btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// // map from node index to static constraint
// btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// // map from node index to node rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// // map from node index to deformable constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// // map from node index to node anchor constraint
// btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
// map from node index to deformable constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
// map from node index to node anchor constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
@@ -61,7 +72,7 @@ public:
virtual void applyDynamicFriction(TVStack& f);
// update and solve the constraints
virtual btScalar update();
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
// solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

10
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -17,12 +17,12 @@
#include "btDeformableMultiBodyConstraintSolver.h"
#include <iostream>
// override the iterations method to include deformable/multibody contact
btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
@@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints();
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
// update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity
@@ -58,7 +58,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
return 0.f;
}
void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
{
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
@@ -67,7 +67,7 @@ void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObjec
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// overriden
solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlyFinish(bodies, numBodies, info);

5
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@@ -36,7 +36,7 @@ btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
protected:
// override the iterations method to include deformable/multibody contact
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
// write the velocity of the the solver body to the underlying rigid body
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
@@ -46,6 +46,7 @@ protected:
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, 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);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -54,7 +55,7 @@ public:
m_deformableSolver = deformableSolver;
}
virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
};
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

33
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -36,9 +36,36 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
#include <stdio.h>
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "DeformableBodyInplaceSolverIslandCallback.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
{
m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true;
m_drawFaceTree = false;
m_drawClusterTree = false;
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
m_sbi.water_density = 0;
m_sbi.water_offset = 0;
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = false;
m_lineSearch = false;
m_selfCollision = true;
m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
}
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
@@ -261,7 +288,7 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
// setup the solver callback
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
// build islands
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
@@ -290,10 +317,10 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
{
// process constraints on each island
m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
// process deferred
m_solverMultiBodyIslandCallback->processConstraints();
m_solverDeformableBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
// write joint feedback

27
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -29,6 +29,7 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver;
class btDeformableLagrangianForce;
struct MultiBodyInplaceSolverIslandCallback;
struct DeformableBodyInplaceSolverIslandCallback;
class btDeformableMultiBodyConstraintSolver;
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
@@ -49,6 +50,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
bool m_implicit;
bool m_lineSearch;
bool m_selfCollision;
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
@@ -67,30 +69,7 @@ protected:
void clearGravity();
public:
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
{
m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true;
m_drawFaceTree = false;
m_drawClusterTree = false;
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
m_sbi.water_density = 0;
m_sbi.water_offset = 0;
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = false;
m_lineSearch = false;
m_selfCollision = true;
}
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));