AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2386 from xhan0619/newton

Browse Source 
Newton solver
This commit is contained in:
erwincoumans 2019-09-06 10:32:59 -07:00 committed by GitHub
commit bcc7ea31ff
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
34 changed files with 1081 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
data/single_tet.vtk Normal file
View File

@ -0,0 +1,15 @@
# vtk DataFile Version 2.0
ball_, Created by Gmsh
ASCII
DATASET UNSTRUCTURED_GRID
POINTS 4 double
0 0 0
1 0 0
0 1 0
0 0 1
CELLS 1 1
4 0 1 2 3
CELL_TYPES 1
10

3
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@ -1,6 +1,5 @@
#ifndef GUI_HELPER_INTERFACE_H
#define GUI_HELPER_INTERFACE_H
class btRigidBody;
class btVector3;
class btCollisionObject;
@ -118,6 +117,8 @@ struct GUIHelperInterface
//empty name stops dumping video
virtual void dumpFramesToVideo(const char* mp4FileName){};
virtual void drawDebugDrawerLines(){}
virtual void clearLines(){}
};
///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode')

11
examples/DeformableDemo/DeformableMultibody.cpp
View File

@ -49,7 +49,7 @@ static bool g_floatingBase = true;
static float friction = 1.;
class DeformableMultibody : public CommonMultiBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> forces;
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableMultibody(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
@ -229,11 +229,11 @@ void DeformableMultibody::initPhysics()
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
forces.push_back(mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
forces.push_back(gravity_force);
m_forces.push_back(gravity_force);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@ -257,11 +257,12 @@ void DeformableMultibody::exitPhysics()
delete obj;
}
// delete forces
for (int j = 0; j < forces.size(); j++)
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = forces[j];
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{

15
examples/DeformableDemo/DeformableRigid.cpp
View File

@ -44,7 +44,7 @@
///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
class DeformableRigid : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> forces;
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableRigid(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
@ -234,17 +234,17 @@ void DeformableRigid::initPhysics()
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 1;
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
forces.push_back(mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
forces.push_back(gravity_force);
m_forces.push_back(gravity_force);
// add a few rigid bodies
Ctor_RbUpStack(1);
}
@ -269,11 +269,12 @@ void DeformableRigid::exitPhysics()
delete obj;
}
// delete forces
for (int j = 0; j < forces.size(); j++)
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = forces[j];
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{

40
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -59,7 +59,7 @@ static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
class GraspDeformable : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> forces;
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
GraspDeformable(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
@ -128,7 +128,7 @@ public:
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 250.f;
m_dynamicsWorld->stepSimulation(deltaTime, 100, internalTimeStep);
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
void createGrip()
@ -325,7 +325,8 @@ void GraspDeformable::initPhysics()
{
char relative_path[1024];
// b3FileUtils::findFile("banana.vtk", relative_path, 1024);
b3FileUtils::findFile("ball.vtk", relative_path, 1024);
b3FileUtils::findFile("ball.vtk", relative_path, 1024);
// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
// b3FileUtils::findFile("tube.vtk", relative_path, 1024);
// b3FileUtils::findFile("torus.vtk", relative_path, 1024);
// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
@ -343,31 +344,26 @@ void GraspDeformable::initPhysics()
psb->scale(btVector3(.25, .25, .25));
// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
// psb->scale(btVector3(1, 1, 1)); // for ditto
// psb->translate(btVector3(0, 0, 2)); for boot
psb->getCollisionShape()->setMargin(0.01);
psb->translate(btVector3(.25, 0, 0.4));
psb->getCollisionShape()->setMargin(0.02);
psb->setTotalMass(.1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->getWorldInfo()->m_maxDisplacement = .1f;
// nonlinear damping
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true));
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6));
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.5,0.04, true);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
forces.push_back(mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
forces.push_back(gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
forces.push_back(neohookean);
m_forces.push_back(neohookean);
}
// // create a piece of cloth
@ -415,8 +411,8 @@ void GraspDeformable::initPhysics()
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -.1;
slider.m_maxVal = .1;
slider.m_minVal = -.3;
slider.m_maxVal = .3;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
@ -440,11 +436,13 @@ void GraspDeformable::exitPhysics()
delete obj;
}
// delete forces
for (int j = 0; j < forces.size(); j++)
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = forces[j];
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
@ -468,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
float linkMass = 1.f;
float baseMass = 100.f;
float linkMass = 100.f;
int numLinks = 2;
if (baseMass)

13
examples/DeformableDemo/Pinch.cpp
View File

@ -56,7 +56,7 @@ struct TetraBunny
class Pinch : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> forces;
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
Pinch(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
@ -339,15 +339,15 @@ void Pinch::initPhysics()
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
forces.push_back(mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
forces.push_back(gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
forces.push_back(neohookean);
m_forces.push_back(neohookean);
// add a grippers
createGrip();
}
@ -372,11 +372,12 @@ void Pinch::exitPhysics()
delete obj;
}
// delete forces
for (int j = 0; j < forces.size(); j++)
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = forces[j];
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{

15
examples/DeformableDemo/VolumetricDeformable.cpp
View File

@ -51,7 +51,7 @@ struct TetraCube
class VolumetricDeformable : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> forces;
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
VolumetricDeformable(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
@ -238,15 +238,15 @@ void VolumetricDeformable::initPhysics()
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
forces.push_back(mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
forces.push_back(gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
forces.push_back(neohookean);
m_forces.push_back(neohookean);
}
// add a few rigid bodies
@ -273,12 +273,13 @@ void VolumetricDeformable::exitPhysics()
delete obj;
}
// delete forces
for (int j = 0; j < forces.size(); j++)
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = forces[j];
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{

6
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
View File

@ -45,6 +45,10 @@ static bool UrdfFindMeshFile(
{
*out_type = UrdfGeometry::FILE_CDF;
}
else if (ext == ".vtk")
{
*out_type = UrdfGeometry::FILE_VTK;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
@ -115,4 +119,4 @@ static bool UrdfFindMeshFile(
}
}
#endif //URDF_FIND_MESH_FILE_H
#endif //URDF_FIND_MESH_FILE_H

1
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -82,6 +82,7 @@ struct UrdfGeometry
FILE_OBJ = 3,
FILE_CDF = 4,
MEMORY_VERTICES = 5,
FILE_VTK = 6,
};
int m_meshFileType;

66
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -338,6 +338,71 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_corotatedMu = corotatedMu;
command->m_loadSoftBodyArguments.m_corotatedLambda = corotatedLambda;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_COROTATED_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu;
command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_GRAVITY_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_collisionHardness = collisionHardness;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_COLLISION_HARDNESS;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_frictionCoeff = frictionCoefficient;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_BENDING_SPRINGS;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@ -1373,7 +1438,6 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle ph
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = replaceHeightfieldIndex;
cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float));
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -631,6 +631,13 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda);
B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);

180
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -101,11 +101,15 @@
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#include "../SoftDemo/BunnyMesh.h"
#else
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif
#define SKIP_DEFORMABE_BODY 1
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
@ -1619,12 +1623,22 @@ struct PhysicsServerCommandProcessorInternalData
btHashedOverlappingPairCache* m_pairCache;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
#if defined (SKIP_DEFORMABLE_BODY) || defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
btMultiBodyConstraintSolver* m_solver;
#else
btDeformableMultiBodyConstraintSolver* m_solver;
#endif
btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld;
btDeformableBodySolver* m_deformablebodySolver;
#else
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
btSoftBodySolver* m_softbodySolver;
#endif
#else
btMultiBodyDynamicsWorld* m_dynamicsWorld;
#endif
@ -2607,10 +2621,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
#ifdef SKIP_DEFORMABLE_BODY
m_data->m_solver = new btMultiBodyConstraintSolver;
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver();
m_data->m_solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
#else
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
#else
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
@ -2846,6 +2869,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
delete m_data->m_remoteDebugDrawer;
m_data->m_remoteDebugDrawer = 0;
#if !defined (SKIP_DEFORMABLE_BODY) && !defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
delete m_data->m_deformablebodySolver;
m_data->m_deformablebodySolver = 0;
#endif
delete m_data->m_solver;
m_data->m_solver = 0;
@ -4728,8 +4756,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
break;
}
case GEOM_MESH:
case GEOM_MESH:
{
btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
@ -4909,6 +4936,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
break;
}
default:
{
}
@ -8014,26 +8042,109 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
int out_type;
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
if (!shapes.empty())
{
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices;
btAlignedObjectArray<int> indices;
for (int i = 0; i < attribute.vertices.size(); i++)
{
vertices.push_back(attribute.vertices[i]);
}
for (int i = 0; i < shape.mesh.indices.size(); i++)
{
indices.push_back(shape.mesh.indices[i].vertex_index);
}
int numTris = shape.mesh.indices.size() / 3;
if (numTris > 0)
{
btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
btSoftBody* psb = NULL;
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (out_type == UrdfGeometry::FILE_OBJ)
{
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
if (!shapes.empty())
{
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices;
btAlignedObjectArray<int> indices;
for (int i = 0; i < attribute.vertices.size(); i++)
{
vertices.push_back(attribute.vertices[i]);
}
for (int i = 0; i < shape.mesh.indices.size(); i++)
{
indices.push_back(shape.mesh.indices[i].vertex_index);
}
int numTris = shape.mesh.indices.size() / 3;
if (numTris > 0)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
}
}
#ifndef SKIP_DEFORMABLE_BODY
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
}
#endif
}
else if (out_type == UrdfGeometry::FILE_VTK)
{
#ifndef SKIP_DEFORMABLE_BODY
psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str());
btScalar corotated_mu, corotated_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
}
btScalar neohookean_mu, neohookean_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
{
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
}
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
}
#endif
}
if (psb != NULL)
{
#ifndef SKIP_DEFORMABLE_BODY
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
{
m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
}
btScalar collision_hardness = 1;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_COLLISION_HARDNESS)
{
collision_hardness = loadSoftBodyArgs.m_collisionHardness;
}
psb->m_cfg.kKHR = collision_hardness;
psb->m_cfg.kCHR = collision_hardness;
btScalar friction_coeff = 0;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
{
friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
}
psb->m_cfg.kDF = friction_coeff;
bool use_bending_spring = true;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
{
use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
}
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
if (use_bending_spring)
{
psb->generateBendingConstraints(2,pm);
}
// turn on the collision flag for deformable
// collision
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->setCollisionFlags(0);
psb->setTotalMass(mass);
#else
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_kLST = 0.5;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
@ -8043,11 +8154,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
//turn on softbody vs softbody collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints();
psb->setTotalMass(mass, true);
#endif
psb->scale(btVector3(scale, scale, scale));
psb->rotate(initialOrn);
psb->translate(initialPos);
psb->setTotalMass(mass, true);
psb->getCollisionShape()->setMargin(collisionMargin);
psb->getCollisionShape()->setUserPointer(psb);
m_data->m_dynamicsWorld->addSoftBody(psb);
@ -8108,10 +8220,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
}
}
#endif
return hasStatus;
}
@ -9134,6 +9244,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
}
};
#ifdef SKIP_DEFORMABLE_BODY
if (newSolver)
{
delete oldSolver;
@ -9142,6 +9253,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
m_data->m_solver = newSolver;
printf("switched solver\n");
}
#endif
}
}
@ -12020,7 +12132,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_SAVE_STATE");
bool hasStatus = true;
bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_SAVE_STATE_FAILED;
@ -12430,7 +12542,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_SOFT_BODY:
{
hasStatus = processLoadSoftBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
hasStatus = processLoadSoftBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_SENSOR:
@ -12716,6 +12828,20 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
}
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags());
}
}
m_data->m_guiHelper->drawDebugDrawerLines();
m_data->m_guiHelper->clearLines();
#endif
#endif
}
}

20
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -546,7 +546,7 @@ MultithreadedDebugDrawer : public btIDebugDraw
btHashMap<ColorWidth, int> m_hashedLines;
public:
void drawDebugDrawerLines()
virtual void drawDebugDrawerLines()
{
if (m_hashedLines.size())
{
@ -628,7 +628,7 @@ public:
return m_debugMode;
}
virtual void clearLines()
virtual void clearLines() override
{
m_hashedLines.clear();
m_sortedIndices.clear();
@ -650,13 +650,21 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
public:
MultithreadedDebugDrawer* m_debugDraw;
void drawDebugDrawerLines()
virtual void drawDebugDrawerLines()
{
if (m_debugDraw)
{
m_debugDraw->drawDebugDrawerLines();
}
}
virtual void clearLines()
{
if (m_debugDraw)
{
m_debugDraw->clearLines();
}
}
GUIHelperInterface* m_childGuiHelper;
btHashMap<btHashPtr, int> m_cachedTextureIds;
@ -847,10 +855,8 @@ public:
delete m_debugDraw;
m_debugDraw = 0;
}
m_debugDraw = new MultithreadedDebugDrawer(this);
rbWorld->setDebugDrawer(m_debugDraw);
m_debugDraw = new MultithreadedDebugDrawer(this);
rbWorld->setDebugDrawer(m_debugDraw);
//m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
}

26
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -489,11 +489,18 @@ enum EnumSimParamUpdateFlags
enum EnumLoadSoftBodyUpdateFlags
{
LOAD_SOFT_BODY_FILE_NAME = 1,
LOAD_SOFT_BODY_UPDATE_SCALE = 2,
LOAD_SOFT_BODY_UPDATE_MASS = 4,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8,
LOAD_SOFT_BODY_INITIAL_POSITION = 16,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 32
LOAD_SOFT_BODY_UPDATE_SCALE = 1<<1,
LOAD_SOFT_BODY_UPDATE_MASS = 1<<2,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3,
LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
};
enum EnumSimParamInternalSimFlags
@ -512,6 +519,15 @@ struct LoadSoftBodyArgs
double m_collisionMargin;
double m_initialPosition[3];
double m_initialOrientation[4];
double m_springElasticStiffness;
double m_springDampingStiffness;
double m_corotatedMu;
double m_corotatedLambda;
bool m_useBendingSprings;
double m_collisionHardness;
double m_frictionCoeff;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
};
struct b3LoadSoftBodyResultArgs

5
setup.py
View File

@ -247,6 +247,11 @@ sources = ["examples/pybullet/pybullet.c"]\
+["src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp"]\
+["src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp"]\
+["src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp"]\
+["src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp"]\
+["src/BulletSoftBody/btDeformableBodySolver.cpp"]\
+["src/BulletSoftBody/btDeformableContactProjection.cpp"]\
+["src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"]\
+["src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"]\
+["src/BulletInverseDynamics/IDMath.cpp"]\
+["src/BulletInverseDynamics/MultiBodyTree.cpp"]\
+["src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp"]\

2
src/BulletSoftBody/CMakeLists.txt
View File

@ -16,10 +16,10 @@ SET(BulletSoftBody_SRCS
btSoftMultiBodyDynamicsWorld.cpp
btSoftSoftCollisionAlgorithm.cpp
btDefaultSoftBodySolver.cpp
btDeformableMultiBodyConstraintSolver.cpp
btDeformableBackwardEulerObjective.cpp
btDeformableBodySolver.cpp
btDeformableMultiBodyConstraintSolver.cpp
btDeformableContactProjection.cpp
btDeformableMultiBodyDynamicsWorld.cpp

10
src/BulletSoftBody/btCGProjection.h
View File

@ -49,6 +49,16 @@ struct DeformableContactConstraint
m_static.push_back(false);
m_can_be_dynamic.push_back(true);
}
void replace(const btSoftBody::RContact& rcontact)
{
m_contact.clear();
m_total_normal_dv.clear();
m_total_tangent_dv.clear();
m_static.clear();
m_can_be_dynamic.clear();
append(rcontact);
}
~DeformableContactConstraint()
{

2
src/BulletSoftBody/btConjugateGradient.h
View File

@ -104,7 +104,9 @@ public:
TVStack c;
c.resize(a.size());
for (int i = 0; i < a.size(); ++i)
{
c[i] = a[i] - b[i];
}
return c;
}

26
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -14,14 +14,16 @@
*/
#include "btDeformableBackwardEulerObjective.h"
#include "btPreconditioner.h"
#include "LinearMath/btQuickprof.h"
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: m_softBodies(softBodies)
, projection(m_softBodies, m_dt)
, m_backupVelocity(backup_v)
, m_implicit(false)
{
m_preconditioner = new DefaultPreconditioner();
m_preconditioner = new MassPreconditioner(m_softBodies);
}
btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
@ -72,13 +74,17 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
for (int i = 0; i < m_lf.size(); ++i)
{
// add damping matrix
m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
if (m_implicit)
{
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
}
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
{
// only the velocity of the constrained nodes needs to be updated during CG solve
// only the velocity of the constrained nodes needs to be updated during contact solve
for (int i = 0; i < projection.m_constraints.size(); ++i)
{
int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
@ -105,14 +111,22 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
}
}
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual)
{
BT_PROFILE("computeResidual");
// add implicit force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledImplicitForce(dt, residual);
if (m_implicit)
{
m_lf[i]->addScaledForces(dt, residual);
}
else
{
m_lf[i]->addScaledDampingForce(dt, residual);
}
}
projection.project(residual);
}
btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@ -122,7 +136,7 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared+SIMD_EPSILON);
return std::sqrt(norm_squared);
}
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)

9
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -37,6 +37,8 @@ public:
btDeformableContactProjection projection;
const TVStack& m_backupVelocity;
btAlignedObjectArray<btSoftBody::Node* > m_nodes;
bool m_implicit;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btDeformableBackwardEulerObjective();
@ -44,7 +46,7 @@ public:
void initialize(){}
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
void computeResidual(btScalar dt, TVStack& residual) const;
void computeResidual(btScalar dt, TVStack& residual);
// add explicit force to the velocity
void applyExplicitForce(TVStack& force);
@ -117,6 +119,11 @@ public:
{
return &m_nodes;
}
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
};
#endif /* btBackwardEulerObjective_h */

125
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -20,7 +20,9 @@
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(50)
, m_cg(20)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@ -33,17 +35,70 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveConstraints");
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
computeStep(m_dv, m_residual);
updateVelocity();
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
computeStep(m_dv, m_residual);
updateVelocity();
}
else
{
for (int i = 0; i < m_maxNewtonIterations; ++i)
{
updateState();
// add the inertia term in the residual
int counter = 0;
for (int k = 0; k < m_softBodySet.size(); ++k)
{
btSoftBody* psb = m_softBodySet[k];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im > 0)
{
m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter];
}
++counter;
}
}
m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
{
break;
}
m_objective->applyDynamicFriction(m_residual);
computeStep(m_ddv, m_residual);
updateDv();
for (int j = 0; j < m_numNodes; ++j)
{
m_ddv[j].setZero();
m_residual[j].setZero();
}
}
}
}
void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
void btDeformableBodySolver::updateState()
{
btScalar tolerance = std::numeric_limits<float>::epsilon() * 16 * m_objective->computeNorm(residual);
m_cg.solve(*m_objective, dv, residual, tolerance);
updateVelocity();
updateTempPosition();
}
void btDeformableBodySolver::updateDv()
{
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i] += m_ddv[i];
}
}
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
//btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
m_cg.solve(*m_objective, ddv, residual, tolerance);
}
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@ -54,6 +109,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
if (nodeUpdated)
{
m_dv.resize(m_numNodes, btVector3(0,0,0));
m_ddv.resize(m_numNodes, btVector3(0,0,0));
m_residual.resize(m_numNodes, btVector3(0,0,0));
m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
}
@ -62,9 +118,11 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i].setZero();
m_ddv[i].setZero();
m_residual[i].setZero();
}
m_dt = dt;
m_objective->reinitialize(nodeUpdated, dt);
}
@ -103,6 +161,21 @@ void btDeformableBodySolver::updateVelocity()
}
}
void btDeformableBodySolver::updateTempPosition()
{
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_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
++counter;
}
psb->updateDeformation();
}
}
void btDeformableBodySolver::backupVelocity()
{
int counter = 0;
@ -116,6 +189,29 @@ void btDeformableBodySolver::backupVelocity()
}
}
void btDeformableBodySolver::backupVn()
{
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)
{
// Here:
// dv = 0 for nodes not in constraints
// dv = v_{n+1} - v_{n+1}^* for nodes in constraints
if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL)
{
m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
}
// Now:
// dv = 0 for nodes not in constraints
// dv = v_{n+1} - v_n for nodes in constraints
m_backupVelocity[counter++] = psb->m_nodes[j].m_vn;
}
}
}
void btDeformableBodySolver::revertVelocity()
{
int counter = 0;
@ -173,8 +269,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
n.m_q = n.m_x;
n.m_x += n.m_v * dt;
n.m_q = n.m_x + n.m_v * dt;
}
/* Bounds */
psb->updateBounds();
@ -184,7 +279,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg);
vol = btDbvtVolume::FromCR(n.m_q, psb->m_sst.radmrg);
psb->m_ndbvt.update(n.m_leaf,
vol,
n.m_v * psb->m_sst.velmrg,
@ -209,3 +304,9 @@ void btDeformableBodySolver::updateSoftBodies()
}
}
}
void btDeformableBodySolver::setImplicit(bool implicit)
{
m_implicit = implicit;
m_objective->setImplicit(implicit);
}

15
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver
protected:
int m_numNodes;
TVStack m_dv;
TVStack m_ddv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
@ -41,7 +42,9 @@ protected:
btScalar m_dt;
btScalar m_contact_iterations;
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
bool m_implicit;
int m_maxNewtonIterations;
btScalar m_newtonTolerance;
public:
btDeformableBackwardEulerObjective* m_objective;
@ -72,6 +75,7 @@ public:
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
void backupVelocity();
void backupVn();
void revertVelocity();
void updateVelocity();
@ -94,7 +98,14 @@ public:
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
virtual bool checkInitialized(){return true;}
void setImplicit(bool implicit);
void updateState();
void updateDv();
void updateTempPosition();
};
#endif /* btDeformableBodySolver_h */

15
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -20,6 +20,7 @@
btScalar btDeformableContactProjection::update()
{
btScalar residualSquare = 0;
btScalar max_impulse = 0;
// loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index)
{
@ -128,6 +129,7 @@ btScalar btDeformableContactProjection::update()
}
}
impulse = impulse_normal + impulse_tangent;
max_impulse = btMax(impulse.length2(), max_impulse);
// dn is the normal component of velocity diffrerence. Approximates the residual.
residualSquare = btMax(residualSquare, dn*dn);
@ -231,7 +233,18 @@ void btDeformableContactProjection::setConstraints()
else
{
DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
constraints.append(c);
bool single_contact = true;
if (single_contact)
{
if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset)
{
constraints.replace(c);
}
}
else
{
constraints.append(c);
}
}
}
}

11
src/BulletSoftBody/btDeformableCorotatedForce.h
View File

@ -39,8 +39,9 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@ -92,7 +93,7 @@ public:
{
// btMatrix3x3 JFinvT = F.adjoint();
btScalar J = F.determinant();
P = F.adjoint() * (m_lambda * (J-1));
P = F.adjoint().transpose() * (m_lambda * (J-1));
if (m_mu > SIMD_EPSILON)
{
btMatrix3x3 R,S;
@ -105,7 +106,11 @@ public:
}
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}

14
src/BulletSoftBody/btDeformableGravityForce.h
View File

@ -28,8 +28,9 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledGravityForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@ -37,9 +38,16 @@ public:
addScaledGravityForce(scale, force);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void addScaledGravityForce(btScalar scale, TVStack& force)

279
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@ -18,6 +18,7 @@
#include "btSoftBody.h"
#include <LinearMath/btHashMap.h>
#include <iostream>
enum btDeformableLagrangianForceType
{
@ -40,12 +41,21 @@ public:
virtual ~btDeformableLagrangianForce(){}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
// add all forces
virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
// add damping df
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
// add elastic df
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
// add all forces that are explicit in explicit solve
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
// add all damping forces
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
virtual btDeformableLagrangianForceType getForceType() = 0;
virtual void reinitialize(bool nodeUpdated)
@ -71,5 +81,270 @@ public:
{
m_nodes = nodes;
}
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
{
btVector3 c1 = dx[id1] - dx[id0];
btVector3 c2 = dx[id2] - dx[id0];
btVector3 c3 = dx[id3] - dx[id0];
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
return dF;
}
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
{
btVector3 c1 = n1->m_v - n0->m_v;
btVector3 c2 = n2->m_v - n0->m_v;
btVector3 c3 = n3->m_v - n0->m_v;
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
return dF;
}
virtual void testDerivative()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack dphi_dx;
dphi_dx.resize(dx.size());
for (int i =0; i < dphi_dx.size();++i)
{
dphi_dx[i].setZero();
}
addScaledForces(-1, dphi_dx);
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get dphi/dx * dx
double dphi = 0;
for (int i = 0; i < dx.size(); ++i)
{
dphi += dphi_dx[i].dot(dx[i]);
}
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f1 = totalElasticEnergy();
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f2 = totalElasticEnergy();
//restore m_q
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = f1-f2-2*dphi;
errors.push_back(error);
std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
}
}
virtual void testHessian()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack df;
df.resize(dx.size());
TVStack f1;
f1.resize(dx.size());
TVStack f2;
f2.resize(dx.size());
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get df
for (int i =0; i < df.size();++i)
{
df[i].setZero();
f1[i].setZero();
f2[i].setZero();
}
//set df
addScaledElasticForceDifferential(-1, dx, df);
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f1
addScaledForces(-1, f1);
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f2
addScaledForces(-1, f2);
//restore m_q
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = 0;
for (int i = 0; i < df.size();++i)
{
btVector3 error_vector = f1[i]-f2[i]-2*df[i];
error += error_vector.length2();
}
error = btSqrt(error);
errors.push_back(error);
std::cout << "Iteration = " << it << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
}
}
virtual double totalElasticEnergy()
{
return 0;
}
double randomDouble(double low, double high)
{
return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
}
};
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

66
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@ -31,9 +31,10 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@ -63,7 +64,7 @@ public:
{
if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
btVector3 dir = (node2->m_q - node1->m_q).normalized();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
}
@ -90,7 +91,6 @@ public:
size_t id2 = node2->index;
// elastic force
// explicit elastic force
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
@ -100,12 +100,12 @@ public:
}
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
btSoftBody* psb = m_softBodies[i];
btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j)
{
@ -120,7 +120,7 @@ public:
{
if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
btVector3 dir = (node2->m_q - node1->m_q).normalized();
local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
}
}
@ -129,6 +129,60 @@ public:
}
}
}
virtual double totalElasticEnergy()
{
double energy = 0;
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 btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
// elastic force
btVector3 dir = (node2->m_q - node1->m_q);
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r );
}
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
// implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
btScalar scaled_k = m_elasticStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btScalar r = link.m_rl;
btVector3 dir = (node1->m_q - node2->m_q);
btScalar dir_norm = dir.norm();
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 dx_diff = dx[id1] - dx[id2];
btVector3 scaled_df = btVector3(0,0,0);
if (dir_norm > SIMD_EPSILON)
{
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
}
df[id1] += scaled_df;
df[id2] -= scaled_df;
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{

22
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -166,7 +166,9 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_q + timeStep * node.m_v;
node.m_x = node.m_x + timeStep * node.m_v;
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
}
m_deformableBodySolver->revertVelocity();
@ -180,6 +182,16 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints();
solveMultiBodyRelatedConstraints();
if (m_implicit)
{
// at this point dv = v_{n+1} - v_{n+1}^*
// modify dv such that dv = v_{n+1} - v_n
// modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton
m_deformableBodySolver->backupVn();
}
// At this point, dv should be golden for nodes in contact
m_deformableBodySolver->solveDeformableConstraints(timeStep);
}
@ -199,7 +211,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
// build islands
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
// write the constraint information of each island to the callback, and also setup the constraints in solver
}
}
@ -273,7 +284,6 @@ void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints()
}
}
// todo : may not be necessary
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
@ -304,7 +314,13 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
m_deformableBodySolver->setImplicit(m_implicit);
m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
// if (m_implicit)
// {
// // todo: backup v_n velocity somewhere else
// m_deformableBodySolver->backupVelocity();
// }
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;

7
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -47,6 +47,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
btSoftBodyWorldInfo m_sbi;
btScalar m_internalTime;
int m_contact_iterations;
bool m_implicit;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
@ -81,6 +82,7 @@ public:
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = true;
}
void setSolverCallback(btSolverCallback cb)
@ -153,6 +155,11 @@ public:
void solveMultiBodyRelatedConstraints();
void sortConstraints();
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
};
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

192
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@ -23,18 +23,24 @@ class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btScalar m_mu_damp, m_lambda_damp;
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
{
btScalar damping = 0.005;
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.005): m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@ -44,6 +50,65 @@ public:
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size())
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDampingDifferential(tetra.m_F, dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// damping force differential
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual double totalElasticEnergy()
{
double energy = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
energy += tetra.m_element_measure * elasticEnergyDensity(tetra);
}
}
return energy;
}
double elasticEnergyDensity(const btSoftBody::Tetra& t)
{
double density = 0;
btMatrix3x3 F = t.m_F;
btMatrix3x3 C = F.transpose()*F;
double J = F.determinant();
double trace = C[0].getX() + C[1].getY() + C[2].getZ();
density += m_mu * 0.5 * (trace - 3.);
density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda);
density -= m_mu * 0.5 * log(trace+1);
return density;
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
@ -72,7 +137,6 @@ public:
size_t id3 = node3->index;
// elastic force
// explicit elastic force
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * force_on_node0;
force[id1] -= scale1 * force_on_node123.getColumn(0);
@ -82,23 +146,104 @@ public:
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
int numNodes = getNumNodes();
btAssert(numNodes <= df.size())
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDampingDifferential(tetra.m_F, dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// damping force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
int numNodes = getNumNodes();
btAssert(numNodes <= df.size())
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(tetra.m_F, dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
{
btMatrix3x3 C = F.transpose()*F;
btScalar J = F.determinant();
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint() * (m_lambda * (J - 1) - 0.75 * m_mu);
P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& G, btMatrix3x3& P)
void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP)
{
btScalar J = F.determinant();
addScaledCofactorMatrixDifferential(F, G, m_lambda*(J-1), P);
P += F.adjoint() * m_lambda * DotProduct(F.adjoint(), G);
btMatrix3x3 C = F.transpose()*F;
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP);
dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
}
void firstPiolaDampingDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP)
{
btScalar J = F.determinant();
btMatrix3x3 C = F.transpose()*F;
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
dP = dF * m_mu_damp * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu_damp) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
addScaledCofactorMatrixDifferential(F, dF, m_lambda_damp*(J-1.) - 0.75*m_mu_damp, dP);
dP += F.adjoint().transpose() * m_lambda_damp * DotProduct(F.adjoint().transpose(), dF);
}
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
@ -106,25 +251,22 @@ public:
btScalar ans = 0;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ans += A[i][j] * B[i][j];
}
ans += A[i].dot(B[i]);
}
return ans;
}
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
{
M[0][0] = scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
M[1][0] = scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
M[2][0] = scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
M[0][1] = scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
M[1][1] = scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
M[2][1] = scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
M[0][2] = scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
M[1][2] = scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] = scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
}
virtual btDeformableLagrangianForceType getForceType()

2
src/BulletSoftBody/btPreconditioner.h
View File

@ -70,7 +70,9 @@ public:
btAssert(b.size() == x.size());
btAssert(m_inv_mass.size() == x.size());
for (int i = 0; i < b.size(); ++i)
{
b[i] = x[i] * m_inv_mass[i];
}
}
};

14
src/BulletSoftBody/btSoftBody.cpp
View File

@ -2819,9 +2819,9 @@ void btSoftBody::initializeDmInverse()
for (int i = 0; i < m_tetras.size(); ++i)
{
Tetra &t = m_tetras[i];
btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
@ -2834,12 +2834,10 @@ void btSoftBody::updateDeformation()
{
for (int i = 0; i < m_tetras.size(); ++i)
{
// updateDeformation is called before predictMotion where m_q is sync'd.
// So m_x is the current position of the node.
btSoftBody::Tetra& t = m_tetras[i];
btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());

3
src/BulletSoftBody/btSoftBody.h
View File

@ -251,8 +251,9 @@ public:
struct Node : Feature
{
btVector3 m_x; // Position
btVector3 m_q; // Previous step position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal
btScalar m_im; // 1/mass

12
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -998,7 +998,7 @@ struct btSoftColliders
if (!n.m_battach)
{
// check for collision at x_{n+1}^*
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true))
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
{
const btScalar ima = n.m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
@ -1006,7 +1006,7 @@ struct btSoftColliders
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ false);
psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
btSoftBody::sCti& cti = c.m_cti;
c.m_node = &n;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
@ -1019,7 +1019,7 @@ struct btSoftColliders
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_q - wtr.getOrigin();
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
c.m_c1 = ra;
@ -1035,9 +1035,9 @@ struct btSoftColliders
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];