set up deformable world and solver (does not support contact or friction yet)
This commit is contained in:
269
examples/DeformableDemo/DeformableDemo.cpp
Normal file
269
examples/DeformableDemo/DeformableDemo.cpp
Normal 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);
|
||||
}
|
||||
20
examples/DeformableDemo/DeformableDemo.h
Normal file
20
examples/DeformableDemo/DeformableDemo.h
Normal 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
|
||||
@@ -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
BIN
src/.DS_Store
vendored
Normal file
Binary file not shown.
@@ -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.
|
||||
|
||||
163
src/BulletSoftBody/btBackwardEulerObjective.h
Normal file
163
src/BulletSoftBody/btBackwardEulerObjective.h
Normal 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 */
|
||||
208
src/BulletSoftBody/btConjugateGradient.h
Normal file
208
src/BulletSoftBody/btConjugateGradient.h
Normal 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 */
|
||||
269
src/BulletSoftBody/btDeformableBodySolver.h
Normal file
269
src/BulletSoftBody/btDeformableBodySolver.h
Normal 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 */
|
||||
39
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
Normal file
39
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
Normal 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);
|
||||
}
|
||||
|
||||
89
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
Normal file
89
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
Normal 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
|
||||
53
src/BulletSoftBody/btLagrangianForce.h
Normal file
53
src/BulletSoftBody/btLagrangianForce.h
Normal 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 */
|
||||
113
src/BulletSoftBody/btMassSpring.h
Normal file
113
src/BulletSoftBody/btMassSpring.h
Normal 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 */
|
||||
@@ -110,6 +110,7 @@ void btSoftBody::initDefaults()
|
||||
|
||||
m_windVelocity = btVector3(0, 0, 0);
|
||||
m_restLengthScale = btScalar(1.0);
|
||||
m_dampingCoefficient = 1;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -35,7 +35,8 @@ public:
|
||||
CL_SOLVER,
|
||||
CL_SIMD_SOLVER,
|
||||
DX_SOLVER,
|
||||
DX_SIMD_SOLVER
|
||||
DX_SIMD_SOLVER,
|
||||
DEFORMABLE_SOLVER
|
||||
};
|
||||
|
||||
protected:
|
||||
|
||||
Reference in New Issue
Block a user