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 "../FractureDemo/FractureDemo.h"
|
||||||
#include "../DynamicControlDemo/MotorDemo.h"
|
#include "../DynamicControlDemo/MotorDemo.h"
|
||||||
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
|
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
|
||||||
|
#include "../DeformableDemo/DeformableDemo.h"
|
||||||
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
|
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
|
||||||
#include "../SharedMemory/PhysicsServerExample.h"
|
#include "../SharedMemory/PhysicsServerExample.h"
|
||||||
#include "../SharedMemory/PhysicsClientExample.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(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.",
|
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),
|
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_CONTINUOUS_DYNAMICS_WORLD = 3,
|
||||||
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
|
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
|
||||||
BT_GPU_DYNAMICS_WORLD = 5,
|
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.
|
///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_windVelocity = btVector3(0, 0, 0);
|
||||||
m_restLengthScale = btScalar(1.0);
|
m_restLengthScale = btScalar(1.0);
|
||||||
|
m_dampingCoefficient = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
|
|||||||
@@ -704,6 +704,7 @@ public:
|
|||||||
btDbvt m_fdbvt; // Faces tree
|
btDbvt m_fdbvt; // Faces tree
|
||||||
btDbvt m_cdbvt; // Clusters tree
|
btDbvt m_cdbvt; // Clusters tree
|
||||||
tClusterArray m_clusters; // Clusters
|
tClusterArray m_clusters; // Clusters
|
||||||
|
btScalar m_dampingCoefficient; // Damping Coefficient
|
||||||
|
|
||||||
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
|
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
|
||||||
|
|
||||||
@@ -736,6 +737,11 @@ public:
|
|||||||
return m_worldInfo;
|
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
|
///@todo: avoid internal softbody shape hack and move collision code to collision library
|
||||||
virtual void setCollisionShape(btCollisionShape* collisionShape)
|
virtual void setCollisionShape(btCollisionShape* collisionShape)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -35,7 +35,8 @@ public:
|
|||||||
CL_SOLVER,
|
CL_SOLVER,
|
||||||
CL_SIMD_SOLVER,
|
CL_SIMD_SOLVER,
|
||||||
DX_SOLVER,
|
DX_SOLVER,
|
||||||
DX_SIMD_SOLVER
|
DX_SIMD_SOLVER,
|
||||||
|
DEFORMABLE_SOLVER
|
||||||
};
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
Reference in New Issue
Block a user