set up deformable world and solver (does not support contact or friction yet)

This commit is contained in:
Xuchen Han
2019-07-04 12:49:45 -07:00
parent 3ac4959e95
commit 32836b0694
15 changed files with 1237 additions and 2 deletions

View File

@@ -0,0 +1,269 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 1.
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3
#include "DeformableDemo.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableDemo shows the use of rolling friction.
///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
class DeformableDemo : public CommonRigidBodyBase
{
public:
DeformableDemo(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableDemo()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
// float dist = 30;
// float pitch = -14;
// float yaw = 0;
// float targetPos[3] = {0, 0, 0};
float dist = 45;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0,0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableRigidDynamicsWorld* 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 DeformableDemo::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
// m_collisionConfiguration = new btDefaultCollisionConfiguration();
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)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver);
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a few basic rigid bodies
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(25.), btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -30, 0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03));
//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(.5);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
//
// {
// ///create a few basic rigid bodies
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.)));
//
// m_collisionShapes.push_back(groundShape);
//
// btTransform groundTransform;
// groundTransform.setIdentity();
// groundTransform.setOrigin(btVector3(0, 0, -54));
// //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 body to the dynamics world
// m_dynamicsWorld->addRigidBody(body);
// }
//
// {
// // add a simple deformable body
// const btVector3 s(3,2,1); // side length
// const btVector3 p(0,30,0); // origin;
// const btVector3 h = s * 0.5;
// const btVector3 c[] = {p + h * btVector3(-1, -1, -1),
// p + h * btVector3(+1, -1, -1),
// p + h * btVector3(-1, +1, -1),
// p + h * btVector3(+1, +1, -1),
// p + h * btVector3(-1, -1, +1),
// p + h * btVector3(+1, -1, +1),
// p + h * btVector3(-1, +1, +1),
// p + h * btVector3(+1, +1, +1)};
// btSoftBody* psb = btSoftBodyHelpers::CreateFromConvexHull(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), c, 8);
// psb->generateBendingConstraints(2);
// psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
// psb->setTotalMass(150);
// getDeformableDynamicsWorld()->addSoftBody(psb);
// }
{
const btScalar s = 8;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
10, 10,
// 31,31,
1 + 2 + 4 + 8, true);
// 0, true);
psb->getCollisionShape()->setMargin(0.5);
// btSoftBody::Material* pm = psb->appendMaterial();
// pm->m_kLST = 0.4 * 1000;
// pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->setDampingCoefficient(0.01);
getDeformableDynamicsWorld()->addSoftBody(psb);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableDemo::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 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* DeformableCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableDemo(options.m_guiHelper);
}

View File

@@ -0,0 +1,20 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 _DEFORMABLE_DEMO_H
#define _DEFORMABLE_DEMO_H
class CommonExampleInterface* DeformableCreateFunc(struct CommonExampleOptions& options);
#endif //_DEFORMABLE_DEMO_H

View File

@@ -47,6 +47,7 @@
#include "../FractureDemo/FractureDemo.h"
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../DeformableDemo/DeformableDemo.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsClientExample.h"
@@ -118,6 +119,8 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
ExampleEntry(0, "Deformable", "Deformable test", DeformableCreateFunc),
ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
AllConstraintCreateFunc),

BIN
src/.DS_Store vendored Normal file

Binary file not shown.

View File

@@ -34,7 +34,8 @@ enum btDynamicsWorldType
BT_CONTINUOUS_DYNAMICS_WORLD = 3,
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
BT_GPU_DYNAMICS_WORLD = 5,
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
BT_DEFORMABLE_RIGID_DYNAMICS_WORLD = 7
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

View File

@@ -0,0 +1,163 @@
//
// btBackwardEulerObjective.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/1/19.
//
#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
#define BT_BACKWARD_EULER_OBJECTIVE_H
#include <functional>
#include "btConjugateGradient.h"
#include "btLagrangianForce.h"
#include "btMassSpring.h"
struct DirichletDofProjection
{
using TVStack = btAlignedObjectArray<btVector3>;
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
DirichletDofProjection(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{}
void operator()(TVStack& x)
{
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
x[counter].setZero();
}
++counter;
}
}
}
};
class btBackwardEulerObjective
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
struct EmptyProjection
{
void operator()(TVStack& x)
{}
};
struct DefaultPreconditioner
{
void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i];
}
};
btScalar m_dt;
btConjugateGradient<btBackwardEulerObjective> cg;
btAlignedObjectArray<btLagrangianForce*> m_lf;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
std::function<void(TVStack&)> project;
std::function<void(const TVStack&, TVStack&)> precondition;
btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies)
: cg(20)
, m_softBodies(softBodies)
, project(EmptyProjection())
, precondition(DefaultPreconditioner())
{
// this should really be specified in initialization instead of here
btMassSpring* mass_spring = new btMassSpring(m_softBodies);
m_lf.push_back(mass_spring);
}
virtual ~btBackwardEulerObjective() {}
void initialize(){}
void computeResidual(btScalar dt, TVStack& residual) const
{
// gravity is treated explicitly in predictUnconstraintMotion
// add force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledForce(dt, residual);
}
}
btScalar computeNorm(const TVStack& residual) const
{
btScalar norm_squared = 0;
for (int i = 0; i < residual.size(); ++i)
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared);
}
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
{
m_dt = dt;
// TODO:figure out what the tolerance should be
btScalar tolerance = std::numeric_limits<float>::epsilon()*16 * computeNorm(residual);
cg.solve(*this, dv, residual, tolerance);
}
void multiply(const TVStack& x, TVStack& b) const
{
for (int i = 0; i < b.size(); ++i)
b[i].setZero();
// add in the mass term
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
const auto& node = psb->m_nodes[j];
b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
++counter;
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
// damping force is implicit and elastic force is explicit
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
}
}
void reinitialize(bool nodeUpdated)
{
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->reinitialize(nodeUpdated);
}
if(nodeUpdated)
{
DirichletDofProjection dirichlet(m_softBodies);
setProjection(dirichlet);
}
}
template <class Func>
void setProjection(Func project_func)
{
project = project_func;
}
template <class Func>
void setPreconditioner(Func preconditioner_func)
{
precondition = preconditioner_func;
}
};
#endif /* btBackwardEulerObjective_h */

View File

@@ -0,0 +1,208 @@
//
// btConjugateGradient.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/1/19.
//
#ifndef BT_CONJUGATE_GRADIENT_H
#define BT_CONJUGATE_GRADIENT_H
#include <iostream>
template <class TM>
class btConjugateGradient
{
using TVStack = btAlignedObjectArray<btVector3>;
TVStack r,p,z,temp;
int max_iterations;
btScalar tolerance;
public:
btConjugateGradient(const int max_it_in)
: max_iterations(max_it_in)
, tolerance(std::numeric_limits<float>::epsilon() * 16)
{
}
virtual ~btConjugateGradient(){}
// // return the number of iterations taken
// int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
// {
// btAssert(x.size() == b.size());
// reinitialize(b);
//
// // r = M * (b - A * x) --with assigned dof zeroed out
// A.multiply(x, temp);
// temp = sub(b, temp);
// A.project(temp);
// A.precondition(temp, r);
//
// btScalar r_dot_r = squaredNorm(r), r_dot_r_new;
// btScalar r_norm = std::sqrt(r_dot_r);
// if (r_norm < tolerance) {
// std::cout << "Iteration = 0" << std::endl;
// std::cout << "Two norm of the residual = " << r_norm << std::endl;
// return 0;
// }
//
// p = r;
// // q = M * A * q;
// A.multiply(p, temp);
// A.precondition(temp, q);
//
// // alpha = |r|^2 / (p^T * A * p)
// btScalar alpha = r_dot_r / dot(p, q), beta;
//
// for (int k = 1; k < max_iterations; k++) {
//// x += alpha * p;
//// r -= alpha * q;
// multAndAddTo(alpha, p, x);
// multAndAddTo(-alpha, q, r);
//
// // zero out the dofs of r
// A.project(r);
//
// r_dot_r_new = squaredNorm(r);
// r_norm = std::sqrt(r_dot_r_new);
//
// if (r_norm < tolerance) {
// std::cout << "ConjugateGradient iterations " << k << std::endl;
// return k;
//
// beta = r_dot_r_new / r_dot_r;
// r_dot_r = r_dot_r_new;
//// p = r + beta * p;
// p = multAndAdd(beta, p, r);
//
// // q = M * A * q;
// A.multiply(p, temp);
// A.precondition(temp, q);
//
// alpha = r_dot_r / dot(p, q);
// }
//
// setZero(q);
// setZero(r);
// }
// std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
// return max_iterations;
// }
// return the number of iterations taken
int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
{
btAssert(x.size() == b.size());
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp);
r = sub(b, temp);
A.project(r);
btScalar r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_norm << std::endl;
return 0;
}
// z = M^(-1) * r
A.precondition(r, z);
// p = z;
p = z;
// temp = A*p
A.multiply(p, temp);
btScalar r_dot_z = dot(z,r), r_dot_z_new;
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z / dot(p, temp), beta;
for (int k = 1; k < max_iterations; k++) {
// x += alpha * p;
// r -= alpha * temp;
multAndAddTo(alpha, p, x);
multAndAddTo(-alpha, temp, r);
// zero out the dofs of r
A.project(r);
r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
std::cout << "ConjugateGradient iterations " << k << std::endl;
return k;
}
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z_new = dot(r,z);
beta = r_dot_z_new/ r_dot_z;
r_dot_z = r_dot_z_new;
// p = z + beta * p;
p = multAndAdd(beta, p, z);
// temp = A * p;
A.multiply(p, temp);
// alpha = r^T * z / (p^T * A * p)
alpha = r_dot_z / dot(p, temp);
}
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
return max_iterations;
}
void reinitialize(const TVStack& b)
{
r.resize(b.size());
p.resize(b.size());
z.resize(b.size());
temp.resize(b.size());
}
TVStack sub(const TVStack& a, const TVStack& b)
{
// c = a-b
btAssert(a.size() == b.size())
TVStack c;
c.resize(a.size());
for (int i = 0; i < a.size(); ++i)
c[i] = a[i] - b[i];
return c;
}
btScalar squaredNorm(const TVStack& a)
{
return dot(a,a);
}
btScalar dot(const TVStack& a, const TVStack& b)
{
btScalar ans(0);
for (int i = 0; i < a.size(); ++i)
ans += a[i].dot(b[i]);
return ans;
}
void setZero(TVStack& a)
{
for (int i = 0; i < a.size(); ++i)
a[i].setZero();
}
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
// result += s*a
btAssert(a.size() == result.size())
for (int i = 0; i < a.size(); ++i)
result[i] += s * a[i];
}
TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
{
// result = a*s + b
TVStack result;
result.resize(a.size());
for (int i = 0; i < a.size(); ++i)
result[i] = s * a[i] + b[i];
return result;
}
};
#endif /* btConjugateGradient_h */

View File

@@ -0,0 +1,269 @@
//
// btDeformableBodySolver.h
// BulletSoftBody
//
// Created by Chuyuan Fu on 7/1/19.
//
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
#define BT_DEFORMABLE_BODY_SOLVERS_H
#include "btSoftBodySolvers.h"
#include "btBackwardEulerObjective.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct btCollisionObjectWrapper;
class btDeformableBodySolver : public btSoftBodySolver
{
using TVStack = btAlignedObjectArray<btVector3>;
protected:
/** Variable to define whether we need to update solver constants on the next iteration */
bool m_updateSolverConstants;
int m_numNodes;
TVStack m_dv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
btBackwardEulerObjective m_objective;
int m_solveIterations;
int m_impulseIterations;
public:
btDeformableBodySolver()
: m_numNodes(0)
, m_objective(m_softBodySet)
, m_solveIterations(1)
, m_impulseIterations(1)
{
}
virtual ~btDeformableBodySolver()
{
}
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
virtual bool checkInitialized()
{
return true;
}
virtual void updateSoftBodies()
{
for (int i = 0; i < m_softBodySet.size(); i++)
{
btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
if (psb->isActive())
{
psb->integrateMotion(); // normal is updated here
}
}
}
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false)
{
m_softBodySet.copyFromArray(softBodies);
}
virtual void copyBackToSoftBodies(bool bMove = true) {}
virtual void solveConstraints(float solverdt)
{
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
for (int i = 0; i < m_solveIterations; ++i)
{
// get the velocity after contact solve
// TODO: perform contact solve here
for (int j = 0; j < m_impulseIterations; ++j)
{
for (int s = 0; s < m_softBodySet.size(); ++s)
VSolve_RContacts(m_softBodySet[s], 0, solverdt);
}
// advect with v_n+1 ** to update position based states
// where v_n+1 ** is the velocity after contact response
// only need to advect x here if elastic force is implicit
// prepareSolve(solverdt);
m_objective.computeResidual(solverdt, m_residual);
m_objective.computeStep(m_dv, m_residual, solverdt);
updateVelocity();
}
advect(solverdt);
}
void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i].setZero();
m_residual[i].setZero();
}
m_objective.reinitialize(nodeUpdated);
}
void prepareSolve(btScalar dt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
node.m_x = node.m_q + dt * node.m_v * psb->m_dampingCoefficient;
}
}
}
void advect(btScalar dt)
{
size_t counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
node.m_x += dt * m_dv[counter++];
}
}
}
void updateVelocity()
{
// serial implementation
int counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v += m_dv[counter++];
}
}
}
bool updateNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
numNodes += m_softBodySet[i]->m_nodes.size();
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
return true;
}
return false;
}
virtual void predictMotion(float solverdt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody *psb = m_softBodySet[i];
if (psb->isActive())
{
psb->predictMotion(solverdt);
}
}
}
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
{
softBody->defaultCollisionHandler(collisionObjectWrap);
}
virtual void processCollision(btSoftBody *, btSoftBody *) {
// TODO
}
void VSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar dt)
{
const btScalar mrg = psb->getCollisionShape()->getMargin();
btMultiBodyJacobianData jacobianData;
for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
{
const btSoftBody::RContact& c = psb->m_rcontacts[i];
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
btScalar vel = 0.0;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * dt;
}
}
const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn <= SIMD_EPSILON)
{
const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
const btVector3 fv = vr - (cti.m_normal * dn);
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
c.m_node->m_v -= impulse * c.m_c2 / dt;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c.m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{
double multiplier = 0.5;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
}
}
}
}
}
}
};
#endif /* btDeformableBodySolver_h */

View File

@@ -0,0 +1,39 @@
//
// btDeformableRigidDynamicsWorld.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 7/1/19.
//
#include <stdio.h>
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableBodySolver.h"
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
// Let the solver grab the soft bodies and if necessary optimize for it
m_deformableBodySolver->optimize(getSoftDynamicsWorld()->getSoftBodyArray());
if (!m_deformableBodySolver->checkInitialized())
{
btAssert("Solver initialization failed\n");
}
btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
///update soft bodies
m_deformableBodySolver->updateSoftBodies();
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
{
m_deformableBodySolver->solveConstraints(timeStep);
}

View File

@@ -0,0 +1,89 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
#include "btSoftRigidDynamicsWorld.h"
#include "btLagrangianForce.h"
#include "btMassSpring.h"
#include "btDeformableBodySolver.h"
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver;
class btLagrangianForce;
class btDeformableRigidDynamicsWorld : public btSoftRigidDynamicsWorld
{
using TVStack = btAlignedObjectArray<btVector3>;
///Solver classes that encapsulate multiple deformable bodies for solving
btDeformableBodySolver* m_deformableBodySolver;
protected:
virtual void internalSingleStepSimulation(btScalar timeStep);
void solveDeformableBodiesConstraints(btScalar timeStep);
public:
btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btSoftRigidDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration, 0),
m_deformableBodySolver(deformableBodySolver)
{
}
virtual ~btDeformableRigidDynamicsWorld()
{
}
virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld()
{
return (btSoftRigidDynamicsWorld*)(this);
}
virtual const btSoftRigidDynamicsWorld* getSoftDynamicsWorld() const
{
return (const btSoftRigidDynamicsWorld*)(this);
}
virtual btDynamicsWorldType getWorldType() const
{
return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD;
}
virtual void predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(float(timeStep));
}
// virtual void internalStepSingleStepSimulation(btScalar timeStep);
// virtual void solveDeformableBodiesConstraints(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter)
{
getSoftDynamicsWorld()->getSoftBodyArray().push_back(body);
// Set the soft body solver that will deal with this body
// to be the world's solver
body->setSoftBodySolver(m_deformableBodySolver);
btCollisionWorld::addCollisionObject(body,
collisionFilterGroup,
collisionFilterMask);
}
};
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

View File

@@ -0,0 +1,53 @@
//
// btLagrangianForce.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/1/19.
//
#ifndef BT_LAGRANGIAN_FORCE_H
#define BT_LAGRANGIAN_FORCE_H
#include "btSoftBody.h"
#include <unordered_map>
class btLagrangianForce
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
btLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{
}
virtual ~btLagrangianForce(){}
virtual void addScaledForce(btScalar scale, TVStack& force) = 0;
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
updateId();
}
void updateId()
{
size_t index = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
m_indices[&(psb->m_nodes[j])] = index++;
}
}
}
};
#endif /* btLagrangianForce_h */

View File

@@ -0,0 +1,113 @@
//
// btMassSpring.h
// BulletSoftBody
//
// Created by Chuyuan Fu on 7/1/19.
//
#ifndef BT_MASS_SPRING_H
#define BT_MASS_SPRING_H
#include "btLagrangianForce.h"
class btMassSpring : public btLagrangianForce
{
public:
using TVStack = btLagrangianForce::TVStack;
btMassSpring(const btAlignedObjectArray<btSoftBody *>& softBodies) : btLagrangianForce(softBodies)
{
}
virtual void addScaledForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST; // this is probly wrong, TODO: figure out how to get stiffness
btScalar r = link.m_rl;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
// elastic force
btVector3 dir = (node2->m_x - node1->m_x);
btVector3 dir_normalized = dir.normalized();
btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
force[id1] += scaled_force;
force[id2] -= scaled_force;
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
scaled_force = scale * v_diff * k_damp;
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
int numNodes = getNumNodes();
btAssert(numNodes == dx.size());
btAssert(numNodes == df.size());
// implicit elastic force
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
btVector3 local_scaled_df = scale * kLST * (dx[id2] - dx[id1]);
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicity damping force
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
int getNumNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
numNodes += m_softBodies[i]->m_nodes.size();
}
return numNodes;
}
};
#endif /* btMassSpring_h */

View File

@@ -110,6 +110,7 @@ void btSoftBody::initDefaults()
m_windVelocity = btVector3(0, 0, 0);
m_restLengthScale = btScalar(1.0);
m_dampingCoefficient = 1;
}
//

View File

@@ -704,6 +704,7 @@ public:
btDbvt m_fdbvt; // Faces tree
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
@@ -736,6 +737,11 @@ public:
return m_worldInfo;
}
void setDampingCoefficient(btScalar damping_coeff)
{
m_dampingCoefficient = damping_coeff;
}
///@todo: avoid internal softbody shape hack and move collision code to collision library
virtual void setCollisionShape(btCollisionShape* collisionShape)
{

View File

@@ -35,7 +35,8 @@ public:
CL_SOLVER,
CL_SIMD_SOLVER,
DX_SOLVER,
DX_SIMD_SOLVER
DX_SIMD_SOLVER,
DEFORMABLE_SOLVER
};
protected: