mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
add option for deformable rigid split impulse
This commit is contained in:
Xuchen Han
parent
fb85b2e05f
commit
13314360a8
259
examples/DeformableDemo/SplitImpulse.cpp
Normal file
259
examples/DeformableDemo/SplitImpulse.cpp
Normal 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
19
examples/DeformableDemo/SplitImpulse.h
Normal 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
|
||||
@ -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
|
||||
|
||||
@ -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),
|
||||
|
||||
@ -333,6 +333,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()
|
||||
{
|
||||
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -67,6 +67,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);
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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,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()
|
||||
{
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user