AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

add option for deformable rigid split impulse

Browse Source
This commit is contained in:
Xuchen Han 2019-11-05 18:07:32 -08:00
parent fb85b2e05f
commit 13314360a8
15 changed files with 571 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

259
examples/DeformableDemo/SplitImpulse.cpp Normal file
View File

@ -0,0 +1,259 @@
/*
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 "SplitImpulse.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#define USE_SPLIT_IMPULSE 1
///The SplitImpulse shows the effect of split impulse in deformable rigid contact.
class SplitImpulse : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
SplitImpulse(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~SplitImpulse()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
void Ctor_RbUpStack(int count)
{
float mass = 0.2;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(1, 1, 1)),
};
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, 0.7, 0));
createRigidBody(mass, startTransform, shape[0]);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void SplitImpulse::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -50, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -32, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
#ifdef USE_SPLIT_IMPULSE
getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.03;
#else
getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.0;
#endif
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
// 3,3,
20,20,
1 + 2 + 4 + 8, true);
// 0, true);
psb->getCollisionShape()->setMargin(0.15);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
// add a few rigid bodies
Ctor_RbUpStack(1);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void SplitImpulse::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options)
{
return new SplitImpulse(options.m_guiHelper);
}

19
examples/DeformableDemo/SplitImpulse.h Normal file
View File

@ -0,0 +1,19 @@
/*
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.
*/
#ifndef _SPLIT_IMPULSE_H
#define _SPLIT_IMPULSE_H
class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options);
#endif //_SPLIT_IMPULSE_H

2
examples/ExampleBrowser/CMakeLists.txt
View File

@ -369,6 +369,8 @@ SET(BulletExampleBrowser_SRCS
../DeformableDemo/DeformableMultibody.h
../DeformableDemo/DeformableRigid.cpp
../DeformableDemo/DeformableRigid.h
../DeformableDemo/SplitImpulse.cpp
../DeformableDemo/SplitImpulse.h
../DeformableDemo/VolumetricDeformable.cpp
../DeformableDemo/VolumetricDeformable.h
../DeformableDemo/DeformableClothAnchor.cpp

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -45,6 +45,7 @@
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../DeformableDemo/DeformableRigid.h"
#include "../DeformableDemo/SplitImpulse.h"
#include "../DeformableDemo/ClothFriction.h"
#include "../DeformableDemo/Pinch.h"
#include "../DeformableDemo/DeformableSelfCollision.h"
@ -197,6 +198,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc),
ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc),
ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
ExampleEntry(1, "Split Impulse Contact", "Split impulse test", SplitImpulseCreateFunc),
ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),

42
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@ -332,6 +332,48 @@ public:
}
}
}
void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
{
applyCentralPushImpulse(impulse);
if (m_angularFactor)
{
applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
}
}
}
btVector3 getPushVelocity()
{
return m_pushVelocity;
}
btVector3 getTurnVelocity()
{
return m_turnVelocity;
}
void setPushVelocity(const btVector3& v)
{
m_pushVelocity = v;
}
void setTurnVelocity(const btVector3& v)
{
m_turnVelocity = v;
}
void applyCentralPushImpulse(const btVector3& impulse)
{
m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
}
void applyTorqueTurnImpulse(const btVector3& torque)
{
m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
}
void clearForces()
{

14
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -36,7 +36,7 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveConstraints");
BT_PROFILE("solveDeformableConstraints");
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
@ -241,6 +241,16 @@ btScalar btDeformableBodySolver::solveContactConstraints()
return maxSquaredResidual;
}
btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveSplitImpulse");
return m_objective->m_projection.solveSplitImpulse(infoGlobal);
}
void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
m_objective->m_projection.splitImpulseSetup(infoGlobal);
}
void btDeformableBodySolver::updateVelocity()
{
@ -321,7 +331,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
}
else
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
++counter;
}
}

6
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -66,6 +66,12 @@ public:
// solve the contact between deformable and rigid as well as among deformables
btScalar solveContactConstraints();
// solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// set up the position error in split impulse
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
// resize/clear data structures
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);

60
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -140,11 +140,14 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
// penetration is non-positive. The magnitude of penetration is the depth of penetration.
m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
@ -285,6 +288,36 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
return residualSquare;
}
btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
const btScalar dn = m_penetration;
if (dn != 0)
{
const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
// one iteration of the position impulse corrects all the position error at this timestep
m_penetration -= dn;
// apply impulse to deformable nodes involved and change their position
applySplitImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their position
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// todo xuchenhan@
}
return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
}
return 0;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
: m_node(contact.m_node)
@ -316,6 +349,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_vsplit -= dv;
};
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
: m_face(contact.m_face)
@ -386,6 +426,26 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
v2 += dv2;
}
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_vsplit;
btVector3& v1 = face->m_n[1]->m_vsplit;
btVector3& v2 = face->m_n[2]->m_vsplit;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
: m_node(contact.m_node)

47
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -50,6 +50,9 @@ public:
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
virtual btScalar solveConstraint() = 0;
// solve the position error by applying an inelastic impulse that changes only the position (not velocity)
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
// get the velocity of the object A in the contact
virtual btVector3 getVa() const = 0;
@ -61,6 +64,12 @@ public:
// apply impulse to the soft body node and/or face involved
virtual void applyImpulse(const btVector3& impulse) = 0;
// apply position based impulse to the soft body node and/or face involved
virtual void applySplitImpulse(const btVector3& impulse) = 0;
// scale the penetration depth by erp
virtual void setPenetrationScale(btScalar scale) = 0;
};
//
@ -90,6 +99,11 @@ public:
return 0;
}
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
return 0;
}
virtual btVector3 getVa() const
{
return btVector3(0,0,0);
@ -106,6 +120,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse){}
virtual void applySplitImpulse(const btVector3& impulse){}
virtual void setPenetrationScale(btScalar scale){}
};
//
@ -122,6 +138,11 @@ public:
{
}
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo xuchenhan@
return 0;
}
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the velocity of the deformable node in contact
@ -131,6 +152,11 @@ public:
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
};
virtual void setPenetrationScale(btScalar scale){}
};
@ -141,6 +167,7 @@ class btDeformableRigidContactConstraint : public btDeformableContactConstraint
public:
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
btScalar m_penetration;
const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(){}
@ -154,6 +181,13 @@ public:
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)
{
m_penetration *= scale;
}
};
//
@ -185,6 +219,7 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@ -214,6 +249,7 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@ -235,6 +271,12 @@ public:
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo: xuchenhan@
return 0;
}
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
@ -251,5 +293,10 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
}
virtual void setPenetrationScale(btScalar scale){}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

51
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -51,6 +51,57 @@ btScalar btDeformableContactProjection::update()
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_erp);
}
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
constraint->setPenetrationScale(infoGlobal.m_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;
}
void btDeformableContactProjection::setConstraints()
{

7
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -60,9 +60,12 @@ public:
// add friction force to the rhs of the linear solve
virtual void applyDynamicFriction(TVStack& f);
// update the constraints
// update and solve the constraints
virtual btScalar update();
// solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints();
@ -70,5 +73,7 @@ public:
virtual void setProjection();
virtual void reinitialize(bool nodeUpdated);
virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
};
#endif /* btDeformableContactProjection_h */

37
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -104,3 +104,40 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
}
}
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
m_deformableSolver->splitImpulseSetup(infoGlobal);
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j = 0; j < numPoolConstraints; j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
// solve the position correction between deformable and rigid/multibody
btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}
}
}
}
}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@ -44,6 +44,8 @@ protected:
// write the velocity of the underlying rigid body to the the the solver body
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();

25
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -118,9 +118,32 @@ void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
}
}
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
// correct the position of rigid bodies with temporary velocity generated from split impulse
btContactSolverInfo infoGlobal;
btVector3 zero(0,0,0);
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btVector3 pushVelocity = rb->getPushVelocity();
btVector3 turnVelocity = rb->getTurnVelocity();
if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
{
btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
rb->setWorldTransform(newTransform);
rb->setPushVelocity(zero);
rb->setTurnVelocity(zero);
}
}
}
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
positionCorrection(timeStep);
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
for (int i = 0; i < m_softBodies.size(); ++i)
{
@ -142,6 +165,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_v -= node.m_vsplit;
node.m_vsplit.setZero();
node.m_q = node.m_x;
node.m_vn = node.m_v;
}

1
src/BulletSoftBody/btSoftBody.h
View File

@ -257,6 +257,7 @@ public:
btVector3 m_x; // Position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal