Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Revert "Reduced Deformable Model""

Browse Source
This commit is contained in:
erwincoumans
2022-03-07 16:35:07 -08:00
committed by GitHub
parent aee1ab63fe
commit 4fbecfeddc
77 changed files with 30447 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

316
examples/ReducedDeformableDemo/ConservationTest.cpp Normal file
View File

@@ -0,0 +1,316 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ConservationTest.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include <random>
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
class ConservationTest : public CommonDeformableBodyBase
{
btScalar sim_time;
bool first_step;
// get deformed shape
void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar scale = 1)
{
// for (int i = 0; i < rsb->m_nodes.size(); ++i)
// for (int k = 0; k < 3; ++k)
// rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale;
// rsb->m_reducedDofs[mode_n] = scale;
// rsb->m_reducedDofsBuffer[mode_n] = scale;
srand(1);
for (int r = 0; r < rsb->m_nReduced; r++)
{
rsb->m_reducedDofs[r] = (btScalar(rand()) / btScalar(RAND_MAX) - 0.5);
rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r];
}
rsb->mapToFullPosition(rsb->getRigidTransform());
// std::cout << "-----------\n";
// std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
// std::cout << "-----------\n";
}
public:
ConservationTest(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
first_step = true;
}
virtual ~ConservationTest()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void checkMomentum(btReducedDeformableBody* rsb)
{
btVector3 x_com(0, 0, 0);
btVector3 total_linear(0, 0, 0);
btVector3 total_angular(0, 0, 0);
{
std::ofstream myfile("center_of_mass.txt", std::ios_base::app);
for (int i = 0; i < rsb->m_nFull; ++i)
{
x_com += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_x;
}
x_com /= rsb->getTotalMass();
myfile << sim_time << "\t" << x_com[0] << "\t" << x_com[1] << "\t" << x_com[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("linear_momentum.txt", std::ios_base::app);
for (int i = 0; i < rsb->m_nFull; ++i)
{
total_linear += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_v;
}
myfile << sim_time << "\t" << total_linear[0] << "\t" << total_linear[1] << "\t" << total_linear[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("angular_momentum.txt", std::ios_base::app);
// btVector3 ri(0, 0, 0);
// for (int i = 0; i < rsb->m_nFull; ++i)
// {
// ri = rsb->m_nodes[i].m_x - x_com;
// total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v);
// }
total_angular = rsb->computeTotalAngularMomentum();
myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n";
myfile.close();
}
{
btVector3 angular_rigid(0, 0, 0);
std::ofstream myfile("angular_momentum_rigid.txt", std::ios_base::app);
btVector3 ri(0, 0, 0);
for (int i = 0; i < rsb->m_nFull; ++i)
{
ri = rsb->m_nodes[i].m_x - x_com;
btMatrix3x3 ri_star = Cross(ri);
angular_rigid += rsb->m_nodalMass[i] * (ri_star.transpose() * ri_star * rsb->getAngularVelocity());
}
myfile << sim_time << "\t" << angular_rigid[0] << "\t" << angular_rigid[1] << "\t" << angular_rigid[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("reduced_velocity.txt", std::ios_base::app);
myfile << sim_time << "\t" << rsb->m_reducedVelocity[0] << "\t" << rsb->m_reducedDofs[0] << "\n";
myfile.close();
}
}
void stepSimulation(float deltaTime)
{
// add initial deformation
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
if (first_step /* && !rsb->m_bUpdateRtCst*/)
{
getDeformedShape(rsb, 0, 1);
first_step = false;
}
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
sim_time += internalTimeStep;
checkMomentum(rsb);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
btVector3 origin = rsb->getRigidTransform().getOrigin();
btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin;
btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin;
btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin;
deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0));
deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0));
deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void ConservationTest::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, 0, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transformTo(init_transform);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ConservationTest::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options)
{
return new ConservationTest(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ConservationTest.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_CONSERVATION_TEST_H
#define _REDUCED_CONSERVATION_TEST_H
class CommonExampleInterface* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_CONSERVATION_TEST_H

276
examples/ReducedDeformableDemo/FreeFall.cpp Normal file
View File

@@ -0,0 +1,276 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "FreeFall.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 40;
class FreeFall : public CommonDeformableBodyBase
{
public:
FreeFall(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~FreeFall()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 8;
float pitch = -10;
float yaw = 90;
float targetPos[3] = {0, 2, 0};
// float dist = 10;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {0, 2, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack(const btVector3& origin)
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 12, 0));
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(origin);
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
rb1->setLinearVelocity(btVector3(0, 0, 0));
}
void createReducedDeformableObject(const btVector3& origin, const btQuaternion& rotation)
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
// rsb->scale(btVector3(1, 1, 0.5));
rsb->setTotalMass(10);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
rsb->transformTo(init_transform);
rsb->setStiffnessScale(25);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
// int flag = 0;
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
// btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag);
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
// for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
// }
// for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
// }
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void FreeFall::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
btVector3 gravity = btVector3(0, -9.8, 0);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
// 2 reduced deformable cubes
createReducedDeformableObject(btVector3(0, 4, -2), btQuaternion(0, 0, 0));
createReducedDeformableObject(btVector3(0, 4, 2), btQuaternion(0, 0, 0));
// 2 rigid cubes
Ctor_RbUpStack(btVector3(0, 10, -2));
Ctor_RbUpStack(btVector3(0, 10, 2));
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, -2, 0));
// groundTransform.setOrigin(btVector3(0, 0, 6));
// groundTransform.setOrigin(btVector3(0, -50, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
}
void FreeFall::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options)
{
return new FreeFall(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/FreeFall.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_FREE_FALL_H
#define _REDUCED_FREE_FALL_H
class CommonExampleInterface* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_FREE_FALL_H

288
examples/ReducedDeformableDemo/FrictionSlope.cpp Normal file
View File

@@ -0,0 +1,288 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "FrictionSlope.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 20;
class FrictionSlope : public CommonDeformableBodyBase
{
public:
FrictionSlope(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{}
virtual ~FrictionSlope()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 20;
float pitch = -20;
float yaw = 90;
float targetPos[3] = {0, 0, 0.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 1;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
// btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
}
void createGround()
{
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
btScalar mass(1e6);
btRigidBody* ground = createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// ground->setFriction(1);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
// btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag);
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
// for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
// }
// for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
// }
}
}
}
};
namespace FrictionSlopeHelper
{
void groundMotion(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
btRigidBody* ground = rbs[0];
btAssert(ground->getMass() > 1e5);
btScalar start_time = 2;
btScalar end_time = 8;
btScalar start_angle = 0;
btScalar end_angle = SIMD_PI / 6;
btScalar current_angle = 0;
btScalar turn_speed = (end_angle - start_angle) / (end_time - start_time);
if (time >= start_time)
{
current_angle = (time - start_time) * turn_speed;
if (time > end_time)
{
current_angle = end_angle;
turn_speed = 0;
}
}
else
{
current_angle = start_angle;
turn_speed = 0;
}
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), current_angle));
ground->setCenterOfMassTransform(groundTransform);
ground->setLinearVelocity(btVector3(0, 0, 0));
ground->setAngularVelocity(btVector3(0, 0, 0));
}
};
void FrictionSlope::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, -10, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(50);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
createGround();
// add a few rigid bodies
// Ctor_RbUpStack();
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void FrictionSlope::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* FrictionSlopeCreateFunc(struct CommonExampleOptions& options)
{
return new FrictionSlope(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/FrictionSlope.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _FRICTION_SLOPE_H
#define _FRICTION_SLOPE_H
class CommonExampleInterface* FrictionSlopeCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_FREE_FALL_H

227
examples/ReducedDeformableDemo/ModeVisualizer.cpp Normal file
View File

@@ -0,0 +1,227 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ModeVisualizer.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static int num_modes = 20;
static btScalar visualize_mode = 0;
static btScalar frequency_scale = 1;
class ModeVisualizer : public CommonDeformableBodyBase
{
btScalar sim_time;
// get deformed shape
void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar time_term = 1)
{
for (int i = 0; i < rsb->m_nodes.size(); ++i)
for (int k = 0; k < 3; ++k)
rsb->m_nodes[i].m_x[k] = rsb->m_x0[i][k] + rsb->m_modes[mode_n][3 * i + k] * time_term;
}
btVector3 computeMassWeightedColumnSum(btReducedDeformableBody* rsb, const int mode_n)
{
btVector3 sum(0, 0, 0);
for (int i = 0; i < rsb->m_nodes.size(); ++i)
{
for (int k = 0; k < 3; ++k)
{
sum[k] += rsb->m_nodalMass[i] * rsb->m_modes[mode_n][3 * i + k];
}
}
return sum;
}
public:
ModeVisualizer(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
}
virtual ~ModeVisualizer()
{
}
void initPhysics();
void exitPhysics();
// disable pick force. non-interactive example.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
sim_time += deltaTime;
int n_mode = floor(visualize_mode);
btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale);
getDeformedShape(rsb, n_mode, scale);
// btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
// std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
// << mass_weighted_column_sum[1] << "\t"
// << mass_weighted_column_sum[2] << "\n";
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* rsb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void ModeVisualizer::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric soft body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 2, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
getDeformableDynamicsWorld()->setImplicit(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Visualize Mode", &visualize_mode);
slider.m_minVal = 0;
slider.m_maxVal = num_modes - 1;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Frequency Reduction", &frequency_scale);
slider.m_minVal = 1;
slider.m_maxVal = 1e3;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ModeVisualizer::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options)
{
return new ModeVisualizer(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ModeVisualizer.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_MODE_VISUALIZER_H
#define _REDUCED_MODE_VISUALIZER_H
class CommonExampleInterface* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_MODE_VISUALIZER_H

349
examples/ReducedDeformableDemo/ReducedBenchmark.cpp Normal file
View File

@@ -0,0 +1,349 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ReducedBenchmark.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 20;
static bool run_reduced = true;
class ReducedBenchmark : public CommonDeformableBodyBase
{
btVector3 m_gravity;
public:
ReducedBenchmark(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedBenchmark()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
// float dist = 6;
// float pitch = -10;
// float yaw = 90;
// float targetPos[3] = {0, 2, 0};
float dist = 10;
float pitch = -30;
float yaw = 125;
float targetPos[3] = {0, 2, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack(const btVector3& origin)
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 12, 0));
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(origin);
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
// rb1->setLinearVelocity(btVector3(0, 0, 4));
}
void createDeform(const btVector3& origin, const btQuaternion& rotation)
{
if (run_reduced)
{
std::string file_path("../../../data/reduced_torus/");
std::string vtk_file("torus_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
// rsb->scale(btVector3(1, 1, 0.5));
rsb->setTotalMass(10);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
rsb->transformTo(init_transform);
rsb->setStiffnessScale(5);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
std::cout << "Running reduced deformable\n";
}
else // create full deformable cube
{
std::string filepath("../../../data/reduced_torus/");
std::string filename = filepath + "torus_mesh.vtk";
btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
psb->transform(init_transform);
psb->getCollisionShape()->setMargin(0.015);
psb->setTotalMass(10);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = .5;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
getDeformableDynamicsWorld()->addSoftBody(psb);
btSoftBodyHelpers::generateBoundaryFaces(psb);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(m_gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btScalar E = 10000;
btScalar nu = 0.3;
btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
btScalar mu = E / (2 * (1 + nu));
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.01);
// neohookean->setPoissonRatio(0.3);
// neohookean->setYoungsModulus(25);
neohookean->setDamping(0.01);
psb->m_cfg.drag = 0.001;
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
std::cout << "Running full deformable\n";
}
// btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
// getDeformableDynamicsWorld()->addSoftBody(rsb);
// rsb->getCollisionShape()->setMargin(0.01);
// // rsb->scale(btVector3(1, 1, 0.5));
// rsb->setTotalMass(10);
// btTransform init_transform;
// init_transform.setIdentity();
// init_transform.setOrigin(origin);
// init_transform.setRotation(rotation);
// rsb->transformTo(init_transform);
// rsb->setStiffnessScale(5);
// rsb->setDamping(damping_alpha, damping_beta);
// // rsb->scale(btVector3(0.5, 0.5, 0.5));
// rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
// rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
// rsb->m_cfg.kDF = 0;
// rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
// rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
// rsb->m_sleepingThreshold = 0;
// btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void ReducedBenchmark::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
if (run_reduced)
{
btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
}
else
{
btDeformableBodySolver* solver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
}
// m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
btVector3 gravity = btVector3(0, -10, 0);
m_gravity = gravity;
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// 3x3 torus
createDeform(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(20), btScalar(2), btScalar(20)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
// groundTransform.setOrigin(btVector3(0, 0, 6));
// groundTransform.setOrigin(btVector3(0, -50, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
}
void ReducedBenchmark::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedBenchmark(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedBenchmark.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_BENCHMARK_H
#define _REDUCED_BENCHMARK_H
class CommonExampleInterface* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_BENCHMARK_H

522
examples/ReducedDeformableDemo/ReducedCollide.cpp Normal file
View File

@@ -0,0 +1,522 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ReducedCollide.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 4;
static int num_modes = 20;
class ReducedCollide : public CommonDeformableBodyBase
{
public:
ReducedCollide(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedCollide()
{
}
void initPhysics();
void exitPhysics();
btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
// float dist = 20;
// float pitch = -10;
float dist = 10;
float pitch = -5;
float yaw = 90;
float targetPos[3] = {0, 0, 0};
// float dist = 5;
// float pitch = -35;
// float yaw = 50;
// float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, COLLIDING_VELOCITY, 0));
// body->setFriction(1);
}
void rigidBar()
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,10,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, 0, 0));
// body->setFriction(0);
}
void createGround()
{
// float mass = 55;
float mass = 0;
btCollisionShape* shape = new btBoxShape(btVector3(10, 2, 10));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, 0, 0));
// body->setFriction(1);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
{
int index = rsb->m_contactNodesList[p];
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
}
}
}
};
void ReducedCollide::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, 0, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;
m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->setTotalMass(15);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
rsb->transformTo(init_transform);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
b3Printf("total mass: %e", rsb->getTotalMass());
}
// rigidBar();
// add a few rigid bodies
Ctor_RbUpStack();
// create ground
// createGround();
// create multibody
// {
// bool damping = false;
// bool gyro = true;
// int numLinks = 0;
// bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
// bool multibodyOnly = true;
// bool canSleep = false;
// bool selfCollide = true;
// bool multibodyConstraint = false;
// btVector3 linkHalfExtents(0.05, 0.37, 0.1);
// btVector3 baseHalfExtents(1, 1, 1);
// // btVector3 baseHalfExtents(2.5, 0.5, 2.5);
// // btVector3 baseHalfExtents(0.05, 0.37, 0.1);
// bool g_floatingBase = true;
// // btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0, 4, 0), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
// btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 4.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
// //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
// mbC->setCanSleep(canSleep);
// mbC->setHasSelfCollision(selfCollide);
// mbC->setUseGyroTerm(gyro);
// //
// if (!damping)
// {
// mbC->setLinearDamping(0.f);
// mbC->setAngularDamping(0.f);
// }
// else
// {
// mbC->setLinearDamping(0.1f);
// mbC->setAngularDamping(0.9f);
// }
// //
// //////////////////////////////////////////////
// // if (numLinks > 0)
// // {
// // btScalar q0 = 45.f * SIMD_PI / 180.f;
// // if (!spherical)
// // {
// // mbC->setJointPosMultiDof(0, &q0);
// // }
// // else
// // {
// // btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
// // quat0.normalize();
// // mbC->setJointPosMultiDof(0, quat0);
// // }
// // }
// ///
// addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
// }
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
// {
// SliderParams slider("Young's Modulus", &E);
// slider.m_minVal = 0;
// slider.m_maxVal = 2000;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Poisson Ratio", &nu);
// slider.m_minVal = 0.05;
// slider.m_maxVal = 0.49;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Mass Damping", &damping_alpha);
// slider.m_minVal = 0;
// slider.m_maxVal = 1;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Stiffness Damping", &damping_beta);
// slider.m_minVal = 0;
// slider.m_maxVal = 0.1;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
}
void ReducedCollide::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
btMultiBody* ReducedCollide::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 10;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// btQuaternion baseOriQuat(btVector3(0, 0, 1), -SIMD_PI / 6.0);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
pMultiBody->setBaseVel(vel);
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void ReducedCollide::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(1);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(1);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedCollide(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedCollide.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_COLLIDE_H
#define _REDUCED_COLLIDE_H
class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_COLLIDE_H

457
examples/ReducedDeformableDemo/ReducedGrasp.cpp Normal file
View File

@@ -0,0 +1,457 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ReducedGrasp.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static int num_modes = 20;
class ReducedGrasp : public CommonDeformableBodyBase
{
public:
ReducedGrasp(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedGrasp()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 10;
float pitch = -10;
float yaw = 90;
// float dist = 25;
// float pitch = -30;
// float yaw = 100;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
// float internalTimeStep = 1. / 60.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 0.25));
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,-4));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
}
void Ctor_RbUpStack()
{
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,9.5,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
// btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
// {
// btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
// btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
// }
// for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.1, btVector3(0, 1, 0));
// }
btSoftBody* psb = static_cast<btSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
static void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
};
void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
if (rbs.size()<2)
return;
btRigidBody* rb0 = rbs[0];
// btScalar pressTime = 0.9;
// btScalar pressTime = 0.96;
btScalar pressTime = 1.26;
btScalar liftTime = 2.5;
btScalar shiftTime = 6;
btScalar holdTime = 7;
btScalar dropTime = 10;
// btScalar holdTime = 500;
// btScalar dropTime = 1000;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0,1,0); // inner face has z=2
btVector3 initialTranslationRight = btVector3(0,1,-4); // inner face has z=-2
btVector3 pinchVelocityLeft = btVector3(0,0,-1);
btVector3 pinchVelocityRight = btVector3(0,0,1);
btVector3 liftVelocity = btVector3(0,4,0);
btVector3 shiftVelocity = btVector3(0,0,2);
btVector3 holdVelocity = btVector3(0,0,0);
btVector3 openVelocityLeft = btVector3(0,0,4);
btVector3 openVelocityRight = btVector3(0,0,-4);
if (time < pressTime)
{
velocity = pinchVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * time;
}
// else
// {
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb0->setCenterOfMassTransform(rbTransform);
rb0->setAngularVelocity(btVector3(0,0,0));
rb0->setLinearVelocity(velocity);
btRigidBody* rb1 = rbs[1];
if (time < pressTime)
{
velocity = pinchVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * time;
}
// else
// {
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationRight + pinchVelocityRight * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb1->setCenterOfMassTransform(rbTransform);
rb1->setAngularVelocity(btVector3(0,0,0));
rb1->setLinearVelocity(velocity);
rb0->setFriction(20);
rb1->setFriction(20);
}
void ReducedGrasp::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
// btDeformableBodySolver* solver = new btDeformableBodySolver();
btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->setSolverCallback(GripperDynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.015);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 1, -2));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
// create full deformable cube
{
// std::string filepath("../../../examples/SoftDemo/cube/");
// std::string filename = filepath + "mesh.vtk";
// btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
// // psb->scale(btVector3(2, 2, 2));
// psb->translate(btVector3(0, 1, -2));
// psb->getCollisionShape()->setMargin(0.05);
// psb->setTotalMass(28.6);
// psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
// psb->m_cfg.kCHR = 1; // collision hardness with rigid body
// psb->m_cfg.kDF = .5;
// psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
// getDeformableDynamicsWorld()->addSoftBody(psb);
// btSoftBodyHelpers::generateBoundaryFaces(psb);
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
// getDeformableDynamicsWorld()->addForce(psb, gravity_force);
// m_forces.push_back(gravity_force);
// btScalar E = 10000;
// btScalar nu = 0.3;
// btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
// btScalar mu = E / (2 * (1 + nu));
// btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.02);
// // neohookean->setPoissonRatio(0.3);
// // neohookean->setYoungsModulus(25);
// neohookean->setDamping(0.01);
// psb->m_cfg.drag = 0.001;
// getDeformableDynamicsWorld()->addForce(psb, neohookean);
// m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// grippers
createGrip();
// rigid block
// Ctor_RbUpStack();
// {
// float mass = 10;
// btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
// btTransform startTransform;
// startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0,4,0));
// btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
// rb1->setLinearVelocity(btVector3(0, 0, 0));
// }
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -25, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ReducedGrasp::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedGrasp(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedGrasp.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_GRASP_H
#define _REDUCED_GRASP_H
class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_GRASP_H

558
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp Normal file
View File

@@ -0,0 +1,558 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ReducedMotorGrasp.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
///The ReducedMotorGrasp shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static int num_modes = 20;
static float friction = 1.;
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
struct TetraBunny
{
#include "../SoftDemo/bunny.inl"
};
static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
class ReducedMotorGrasp : public CommonDeformableBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
ReducedMotorGrasp(struct GUIHelperInterface* helper)
:CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedMotorGrasp()
{
}
void initPhysics();
void exitPhysics();
void Ctor_RbUpStack()
{
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(2, 0.25, 0.5));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,0.25,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
}
void resetCamera()
{
// float dist = 0.3;
// float pitch = -45;
// float yaw = 100;
// float targetPos[3] = {0, -0.1, 0};
float dist = 0.4;
float pitch = -25;
float yaw = 90;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld,const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating);
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
btMultiBody* createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating);
void stepSimulation(float deltaTime)
{
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
int num_multiBody = getDeformableDynamicsWorld()->getNumMultibodies();
for (int i = 0; i < num_multiBody; ++i)
{
btMultiBody* mb = getDeformableDynamicsWorld()->btMultiBodyDynamicsWorld::getMultiBody(i);
mb->setBaseVel(btVector3(0,sGripperVerticalVelocity, 0));
int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link = 0; link < mb->getNumLinks(); link++)
{
if (supportsJointMotor(mb, link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(10);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(10);
}
motor->setMaxAppliedImpulse(25);
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
// float internalTimeStep = 1. / 60.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 2;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(3, 3, 0.5)),
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(10, 0, 0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape[i % nshapes]);
}
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
// for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
// {
// int index = rsb->m_contactNodesList[p];
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
// }
}
}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual void removePickingConstraint(){}
};
void ReducedMotorGrasp::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
// btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -9.81, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
// getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_maxPickingForce = 0.001;
// build a gripper
{
bool damping = true;
bool gyro = false;
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
// btVector3 linkHalfExtents(0.02, 0.018, .003);
// btVector3 baseHalfExtents(0.02, 0.002, .002);
btVector3 linkHalfExtents(0.03, 0.04, 0.006);
btVector3 baseHalfExtents(0.02, 0.015, 0.015);
btVector3 basePosition(0, 0.3, 0);
// btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), baseHalfExtents, linkHalfExtents, false);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basePosition, baseHalfExtents, linkHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
double maxMotorImpulse = 1;
if (supportsJointMotor(mbC, mbLinkIndex))
{
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mbC, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1);
mbC->getLink(mbLinkIndex).m_userPtr = motor;
getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
motor->finalizeMultiDof();
}
}
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
btScalar q0 = 0.f * SIMD_PI / 180.f;
if (numLinks > 0)
mbC->setJointPosMultiDof(0, &q0);
addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
}
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
groundShape->setMargin(0.001);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -5.1, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.001);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->scale(btVector3(0.075, 0.075, 0.075));
rsb->setTotalMass(1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 0.1, 0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
// rsb->setRigidVelocity(btVector3(0, 1, 0));
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
// Ctor_RbUpStack();
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
// slider.m_minVal = -.02;
// slider.m_maxVal = .02;
slider.m_minVal = -.2;
slider.m_maxVal = .2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -1;
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ReducedMotorGrasp::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 55;
float linkMass = 55;
int numLinks = 2;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btAlignedObjectArray<btVector3> parentComToCurrentCom;
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, -baseHalfExtents[2] * 2.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*2.f, 0); //cur body's COM to cur body's PIV offset
btAlignedObjectArray<btVector3> parentComToCurrentPivot;
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[1] - currentPivotToCurrentCom));//par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot[i], currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void ReducedMotorGrasp::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedMotorGrasp(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedMotorGrasp.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_MOTOR_GRASP
#define _REDUCED_MOTOR_GRASP
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_MOTOR_GRASP

264
examples/ReducedDeformableDemo/Springboard.cpp Normal file
View File

@@ -0,0 +1,264 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "Springboard.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static int num_modes = 20;
class Springboard : public CommonDeformableBodyBase
{
btScalar sim_time;
bool first_step;
public:
Springboard(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
first_step = true;
}
virtual ~Springboard()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 2;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,8,1));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
{
sim_time += internalTimeStep;
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(getDeformableDynamicsWorld()->getSoftBodyArray()[0]);
// std::ofstream myfile("fixed_node.txt", std::ios_base::app);
// myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t"
// << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t"
// << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n";
// myfile.close();
}
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
// std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n";
}
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void Springboard::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, -10, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(200);
rsb->setDamping(damping_alpha, damping_beta);
// set fixed nodes
rsb->setFixedNodes(0);
rsb->setFixedNodes(1);
rsb->setFixedNodes(2);
rsb->setFixedNodes(3);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// add a few rigid bodies
Ctor_RbUpStack();
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Springboard::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options)
{
return new Springboard(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/Springboard.h Normal file
View File

@@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _REDUCED_SPRINGBOARD_H
#define _REDUCED_SPRINGBOARD_H
class CommonExampleInterface* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_SPRINGBOARD_H