Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' into merge_upstream

Browse Source
This commit is contained in:
Bart Moyaers
2020-01-15 13:24:59 +01:00
parent ee9f25405e 6428ed5e5f
commit ef20f9e62c
370 changed files with 496645 additions and 22916 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
examples/BlockSolver/BlockSolverExample.cpp
View File

@@ -1,209 +0,0 @@
#include "BlockSolverExample.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "btBlockSolver.h"
//for URDF import support
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class BlockSolverExample : public CommonMultiBodyBase
{
int m_option;
public:
BlockSolverExample(GUIHelperInterface* helper, int option);
virtual ~BlockSolverExample();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 3;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {0, 0, .1};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void createMultiBodyStack();
btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
btMultiBody* loadRobot(std::string filepath);
};
BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
: CommonMultiBodyBase(helper),
m_option(option)
{
m_guiHelper->setUpAxis(2);
}
BlockSolverExample::~BlockSolverExample()
{
// Do nothing
}
void BlockSolverExample::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
btScalar internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void BlockSolverExample::initPhysics()
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
///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();
btMLCPSolverInterface* mlcp;
if (m_option & BLOCK_SOLVER_SI)
{
btAssert(!m_solver);
m_solver = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
}
if (m_option & BLOCK_SOLVER_MLCP_PGS)
{
btAssert(!m_solver);
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
}
if (m_option & BLOCK_SOLVER_MLCP_DANTZIG)
{
btAssert(!m_solver);
mlcp = new btDantzigSolver();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
}
if (m_option & BLOCK_SOLVER_BLOCK)
{
m_solver = new btBlockSolver();
}
btAssert(m_solver);
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld = world;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6); //todo: what value is good?
if (m_option & BLOCK_SOLVER_SCENE_MB_STACK)
{
createMultiBodyStack();
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void BlockSolverExample::createMultiBodyStack()
{
///create a few basic rigid bodies
bool loadPlaneFromURDF = false;
if (loadPlaneFromURDF)
{
btMultiBody* mb = loadRobot("plane.urdf");
printf("!\n");
}
else
{
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btScalar mass = 0;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, -50));
btMultiBody* body = createMultiBody(mass, tr, groundShape);
}
for (int i = 0; i < 10; i++)
{
btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
btScalar mass = 1;
if (i == 9)
mass = 100;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
btMultiBody* body = createMultiBody(mass, tr, boxShape);
}
if (/* DISABLES CODE */ (0))
{
btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 1.));
mb->setBaseWorldTransform(tr);
}
}
btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape)
{
btVector3 inertia;
collisionShape->calculateLocalInertia(mass, inertia);
bool canSleep = false;
bool isDynamic = mass > 0;
btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic, canSleep);
btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1);
collider->setWorldTransform(trans);
mb->setBaseWorldTransform(trans);
collider->setCollisionShape(collisionShape);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
mb->setBaseCollider(collider);
mb->finalizeMultiDof();
this->m_dynamicsWorld->addMultiBody(mb);
m_dynamicsWorld->forwardKinematics();
return mb;
}
btMultiBody* BlockSolverExample::loadRobot(std::string filepath)
{
btMultiBody* m_multiBody = 0;
BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
bool loadOk = u2b.loadURDF(filepath.c_str()); // lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n", rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
btTransform identityTrans;
identityTrans.setIdentity();
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix());
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
m_multiBody = creation.getBulletMultiBody();
if (m_multiBody)
{
b3Printf("Root link name = %s", u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
}
}
return m_multiBody;
}
CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options)
{
return new BlockSolverExample(options.m_guiHelper, options.m_option);
}

7
examples/BlockSolver/BlockSolverExample.h
View File

@@ -1,7 +0,0 @@
#ifndef BLOCK_SOLVER_EXAMPLE_H
#define BLOCK_SOLVER_EXAMPLE_H
class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCK_SOLVER_EXAMPLE_H

151
examples/BlockSolver/RigidBodyBoxes.cpp
View File

@@ -1,151 +0,0 @@
#include "RigidBodyBoxes.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "BlockSolverExample.h"
#include "btBlockSolver.h"
class RigidBodyBoxes : public CommonRigidBodyBase
{
int m_option;
int m_numIterations;
int m_numBoxes;
btAlignedObjectArray<btRigidBody*> boxes;
static btScalar numSolverIterations;
public:
RigidBodyBoxes(GUIHelperInterface* helper, int option);
virtual ~RigidBodyBoxes();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void resetCubePosition();
virtual void resetCamera()
{
float dist = 3;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {0, 0, .1};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1],
targetPos[2]);
}
void createRigidBodyStack();
};
btScalar RigidBodyBoxes::numSolverIterations = 50;
RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option)
: CommonRigidBodyBase(helper),
m_option(option),
m_numIterations(numSolverIterations),
m_numBoxes(4)
{
m_guiHelper->setUpAxis(2);
}
RigidBodyBoxes::~RigidBodyBoxes()
{
// Do nothing
}
void RigidBodyBoxes::createRigidBodyStack()
{
// create ground
btBoxShape* groundShape =
createBoxShape(btVector3(btScalar(5.), btScalar(5.), btScalar(5.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, -5));
btScalar mass(0.);
btRigidBody* body = createRigidBody(mass, groundTransform, groundShape,
btVector4(0, 0, 1, 1));
// create a few boxes
mass = 1;
for (int i = 0; i < m_numBoxes; i++)
{
btBoxShape* boxShape =
createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
mass *= 4;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
boxes.push_back(createRigidBody(mass, tr, boxShape));
}
}
void RigidBodyBoxes::initPhysics()
{
/// collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
/// 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();
{
SliderParams slider("numSolverIterations", &numSolverIterations);
slider.m_minVal = 5;
slider.m_maxVal = 500;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (m_option & BLOCK_SOLVER_SI)
{
m_solver = new btSequentialImpulseConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
}
if (m_option & BLOCK_SOLVER_BLOCK)
{
m_solver = new btBlockSolver();
b3Printf("Constraint Solver: Block solver");
}
btAssert(m_solver);
m_dynamicsWorld = new btDiscreteDynamicsWorld(
m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
createRigidBodyStack();
m_dynamicsWorld->getSolverInfo().m_numIterations = numSolverIterations;
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void RigidBodyBoxes::resetCubePosition()
{
for (int i = 0; i < m_numBoxes; i++)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
boxes[i]->setWorldTransform(tr);
}
}
void RigidBodyBoxes::stepSimulation(float deltaTime)
{
if ((int)numSolverIterations != m_numIterations)
{
resetCubePosition();
m_numIterations = (int)numSolverIterations;
m_dynamicsWorld->getSolverInfo().m_numIterations = m_numIterations;
b3Printf("New num iterations; %d", m_numIterations);
}
m_dynamicsWorld->stepSimulation(deltaTime);
}
CommonExampleInterface* RigidBodyBoxesCreateFunc(
CommonExampleOptions& options)
{
return new RigidBodyBoxes(options.m_guiHelper, options.m_option);
}

6
examples/BlockSolver/RigidBodyBoxes.h
View File

@@ -1,6 +0,0 @@
#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_
#define BLOCKSOLVER_RIGIDBODYBOXES_H_
class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCKSOLVER_RIGIDBODYBOXES_H_

374
examples/BlockSolver/btBlockSolver.cpp
View File

@@ -1,374 +0,0 @@
#include "btBlockSolver.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "LinearMath/btQuickprof.h"
void setupHelper(btSISolverSingleIterationData& siData,
btCollisionObject** bodies, int numBodies,
const btContactSolverInfo& info,
btTypedConstraint** constraintStart, int constrainNums,
btPersistentManifold** manifoldPtr, int numManifolds);
struct btBlockSolverInternalData
{
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>
m_tmpConstraintSizesPool;
unsigned long m_btSeed2;
int m_fixedBodyId;
int m_maxOverrideNumSolverIterations;
btAlignedObjectArray<int>
m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
btBlockSolverInternalData()
: m_btSeed2(0),
m_fixedBodyId(-1),
m_maxOverrideNumSolverIterations(0),
m_resolveSingleConstraintRowGeneric(
btSequentialImpulseConstraintSolver::
getScalarConstraintRowSolverGeneric()),
m_resolveSingleConstraintRowLowerLimit(
btSequentialImpulseConstraintSolver::
getScalarConstraintRowSolverLowerLimit()),
m_resolveSplitPenetrationImpulse(
btSequentialImpulseConstraintSolver::
getScalarSplitPenetrationImpulseGeneric()) {}
};
btBlockSolver::btBlockSolver()
{
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}
btBlockSolver::~btBlockSolver()
{
delete m_data21;
delete m_data22;
}
btScalar btBlockSolver::solveGroupInternalBlock(
btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr, int numManifolds,
btTypedConstraint** constraints, int numConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
// initialize data for two children solvers
btSISolverSingleIterationData siData1(
m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
m_data21->m_tmpSolverNonContactConstraintPool,
m_data21->m_tmpSolverContactFrictionConstraintPool,
m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
m_data21->m_orderTmpConstraintPool,
m_data21->m_orderNonContactConstraintPool,
m_data21->m_orderFrictionConstraintPool,
m_data21->m_tmpConstraintSizesPool,
m_data21->m_resolveSingleConstraintRowGeneric,
m_data21->m_resolveSingleConstraintRowLowerLimit,
m_data21->m_resolveSplitPenetrationImpulse,
m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
btSISolverSingleIterationData siData2(
m_data22->m_tmpSolverBodyPool, m_data22->m_tmpSolverContactConstraintPool,
m_data22->m_tmpSolverNonContactConstraintPool,
m_data22->m_tmpSolverContactFrictionConstraintPool,
m_data22->m_tmpSolverContactRollingFrictionConstraintPool,
m_data22->m_orderTmpConstraintPool,
m_data22->m_orderNonContactConstraintPool,
m_data22->m_orderFrictionConstraintPool,
m_data22->m_tmpConstraintSizesPool,
m_data22->m_resolveSingleConstraintRowGeneric,
m_data22->m_resolveSingleConstraintRowLowerLimit,
m_data22->m_resolveSplitPenetrationImpulse,
m_data22->m_kinematicBodyUniqueIdToSolverBodyTable, m_data22->m_btSeed2,
m_data22->m_fixedBodyId, m_data22->m_maxOverrideNumSolverIterations);
m_data21->m_fixedBodyId = -1;
m_data22->m_fixedBodyId = -1;
// set up
int halfNumConstraints1 = numConstraints / 2;
int halfNumConstraints2 = numConstraints - halfNumConstraints1;
int halfNumManifolds1 = numConstraints / 2;
int halfNumManifolds2 = numManifolds - halfNumManifolds1;
setupHelper(siData1, bodies, numBodies, info, constraints,
halfNumConstraints1, manifoldPtr, halfNumManifolds1);
setupHelper(siData2, bodies, numBodies, info,
constraints + halfNumConstraints1, halfNumConstraints2,
manifoldPtr + halfNumManifolds1, halfNumManifolds2);
// set up complete
// begin solve
btScalar leastSquaresResidual = 0;
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
/// this is a special step to resolve penetrations (just for contacts)
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData1, bodies, numBodies, manifoldPtr, halfNumManifolds1,
constraints, halfNumConstraints1, info, debugDrawer);
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData2, bodies, numBodies, manifoldPtr + halfNumManifolds1,
halfNumManifolds2, constraints + halfNumConstraints1,
halfNumConstraints2, info, debugDrawer);
int maxIterations =
siData1.m_maxOverrideNumSolverIterations > info.m_numIterations
? siData1.m_maxOverrideNumSolverIterations
: info.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
btScalar res1 =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData1, iteration, constraints, halfNumConstraints1, info);
btScalar res2 =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData2, iteration, constraints + halfNumConstraints1,
halfNumConstraints2, info);
leastSquaresResidual = btMax(res1, res2);
if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
(iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
iteration);
#endif
break;
}
}
}
btScalar res = btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlyFinishInternal(siData1, bodies, numBodies, info);
+btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(
siData2, bodies, numBodies, info);
return res;
}
void setupHelper(btSISolverSingleIterationData& siData,
btCollisionObject** bodies, int numBodies,
const btContactSolverInfo& info,
btTypedConstraint** constraintStart, int constrainNums,
btPersistentManifold** manifoldPtr, int numManifolds)
{
btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
numBodies, info);
btSequentialImpulseConstraintSolver::convertJointsInternal(
siData, constraintStart, constrainNums, info);
int i;
btPersistentManifold* manifold = 0;
for (i = 0; i < numManifolds; i++)
{
manifold = manifoldPtr[i];
btSequentialImpulseConstraintSolver::convertContactInternal(siData,
manifold, info);
int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
int numFrictionPool =
siData.m_tmpSolverContactFrictionConstraintPool.size();
siData.m_orderNonContactConstraintPool.resizeNoInitialize(
numNonContactPool);
if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
else
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
{
int i;
for (i = 0; i < numNonContactPool; i++)
{
siData.m_orderNonContactConstraintPool[i] = i;
}
for (i = 0; i < numConstraintPool; i++)
{
siData.m_orderTmpConstraintPool[i] = i;
}
for (i = 0; i < numFrictionPool; i++)
{
siData.m_orderFrictionConstraintPool[i] = i;
}
}
}
}
btScalar btBlockSolver::solveGroup(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
// if (m_childSolvers.size())
// hard code to use block solver for now
return solveGroupInternalBlock(bodies, numBodies, manifoldPtr, numManifolds,
constraints, numConstraints, info, debugDrawer,
dispatcher);
// else
// return solveGroupInternal(bodies, numBodies, manifoldPtr, numManifolds,
// constraints, numConstraints, info, debugDrawer,
// dispatcher);
}
btScalar btBlockSolver::solveGroupInternal(
btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr, int numManifolds,
btTypedConstraint** constraints, int numConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
btSISolverSingleIterationData siData(
m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
m_data21->m_tmpSolverNonContactConstraintPool,
m_data21->m_tmpSolverContactFrictionConstraintPool,
m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
m_data21->m_orderTmpConstraintPool,
m_data21->m_orderNonContactConstraintPool,
m_data21->m_orderFrictionConstraintPool,
m_data21->m_tmpConstraintSizesPool,
m_data21->m_resolveSingleConstraintRowGeneric,
m_data21->m_resolveSingleConstraintRowLowerLimit,
m_data21->m_resolveSplitPenetrationImpulse,
m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
m_data21->m_fixedBodyId = -1;
// todo: setup sse2/4 constraint row methods
btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
numBodies, info);
btSequentialImpulseConstraintSolver::convertJointsInternal(
siData, constraints, numConstraints, info);
int i;
btPersistentManifold* manifold = 0;
// btCollisionObject* colObj0=0,*colObj1=0;
for (i = 0; i < numManifolds; i++)
{
manifold = manifoldPtr[i];
btSequentialImpulseConstraintSolver::convertContactInternal(siData,
manifold, info);
}
int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
// @todo: use stack allocator for such temporarily memory, same for solver
// bodies/constraints
siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
else
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
{
int i;
for (i = 0; i < numNonContactPool; i++)
{
siData.m_orderNonContactConstraintPool[i] = i;
}
for (i = 0; i < numConstraintPool; i++)
{
siData.m_orderTmpConstraintPool[i] = i;
}
for (i = 0; i < numFrictionPool; i++)
{
siData.m_orderFrictionConstraintPool[i] = i;
}
}
btScalar leastSquaresResidual = 0;
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
/// this is a special step to resolve penetrations (just for contacts)
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData, bodies, numBodies, manifoldPtr, numManifolds, constraints,
numConstraints, info, debugDrawer);
int maxIterations =
siData.m_maxOverrideNumSolverIterations > info.m_numIterations
? siData.m_maxOverrideNumSolverIterations
: info.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
leastSquaresResidual =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData, iteration, constraints, numConstraints, info);
if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
(iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
iteration);
#endif
break;
}
}
}
btScalar res = btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
return res;
}
void btBlockSolver::solveMultiBodyGroup(
btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold,
int numManifolds, btTypedConstraint** constraints, int numConstraints,
btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
btMultiBodyConstraintSolver::solveMultiBodyGroup(
bodies, numBodies, manifold, numManifolds, constraints, numConstraints,
multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer,
dispatcher);
}
void btBlockSolver::reset()
{
// or just set m_data2->m_btSeed2=0?
delete m_data21;
delete m_data22;
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}

77
examples/BlockSolver/btBlockSolver.h
View File

@@ -1,77 +0,0 @@
#ifndef BT_BLOCK_SOLVER_H
#define BT_BLOCK_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "Bullet3Common/b3Logging.h"
enum BlockSolverOptions
{
BLOCK_SOLVER_SI = 1 << 0,
BLOCK_SOLVER_MLCP_PGS = 1 << 1,
BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
BLOCK_SOLVER_BLOCK = 1 << 3,
BLOCK_SOLVER_SCENE_MB_STACK = 1 << 5,
BLOCK_SOLVER_SCENE_CHAIN = 1 << 6,
};
class btBlockSolver : public btMultiBodyConstraintSolver
{
struct btBlockSolverInternalData* m_data21;
struct btBlockSolverInternalData* m_data22;
public
: btBlockSolver();
virtual ~btBlockSolver();
// btRigidBody
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds, btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
class btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
btScalar solveGroupInternal(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
btScalar solveGroupInternalBlock(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
// btMultibody
virtual void solveMultiBodyGroup(
btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifold, int numManifolds,
btTypedConstraint** constraints, int numConstraints,
btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
/// clear internal cached data and reset random seed
virtual void reset();
virtual btConstraintSolverType getSolverType() const
{
return BT_BLOCK_SOLVER;
}
};
#endif //BT_BLOCK_SOLVER_H

2
examples/CMakeLists.txt
View File

@@ -1,4 +1,4 @@
SUBDIRS( HelloWorld BasicDemo )
SUBDIRS( HelloWorld BasicDemo)
IF(BUILD_BULLET3)
SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint )
ENDIF()

6
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -1,6 +1,5 @@
#ifndef GUI_HELPER_INTERFACE_H
#define GUI_HELPER_INTERFACE_H
class btRigidBody;
class btVector3;
class btCollisionObject;
@@ -44,10 +43,11 @@ struct GUIHelperInterface
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) = 0;
virtual void removeAllGraphicsInstances() = 0;
virtual void removeGraphicsInstance(int graphicsUid) {}
virtual void changeInstanceFlags(int instanceUid, int flags) {}
virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {}
virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {}
virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) {}
virtual void updateShape(int shapeIndex, float* vertices) {}
virtual int getShapeIndexFromInstance(int instanceUid) { return -1; }
virtual void replaceTexture(int shapeIndex, int textureUid) {}
virtual void removeTexture(int textureUid) {}
@@ -118,6 +118,8 @@ struct GUIHelperInterface
//empty name stops dumping video
virtual void dumpFramesToVideo(const char* mp4FileName){};
virtual void drawDebugDrawerLines(){}
virtual void clearLines(){}
};
///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode')

1
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -93,6 +93,7 @@ struct CommonGraphicsApp
if (blue)
*blue = m_backgroundColorRGB[2];
}
virtual void setMp4Fps(int fps) {}
virtual void setBackgroundColor(float red, float green, float blue)
{
m_backgroundColorRGB[0] = red;

10
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -9,6 +9,13 @@ enum
B3_GL_POINTS
};
enum
{
B3_INSTANCE_TRANSPARANCY = 1,
B3_INSTANCE_TEXTURE = 2,
B3_INSTANCE_DOUBLE_SIDED = 4,
};
enum
{
B3_DEFAULT_RENDERMODE = 1,
@@ -94,7 +101,8 @@ struct CommonRenderInterface
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex) = 0;
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex) = 0;
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex) = 0;
virtual void writeSingleInstanceFlagsToCPU(int flags, int srcIndex) = 0;
virtual int getTotalNumInstances() const = 0;
virtual void writeTransforms() = 0;

253
examples/DeformableDemo/ClothFriction.cpp Normal file
View File

@@ -0,0 +1,253 @@
/*
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 "ClothFriction.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The ClothFriction shows the use of deformable friction.
class ClothFriction : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
ClothFriction(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~ClothFriction()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 12;
float pitch = -50;
float yaw = 120;
float targetPos[3] = {0, -3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
//btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void ClothFriction::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -32, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1));
//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(3);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
btScalar s = 4;
btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
10,10,
0, true);
psb->getCollisionShape()->setMargin(0.05);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 3;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
h = 2;
s = 2;
btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
5,5,
0, true);
psb2->getCollisionShape()->setMargin(0.05);
psb2->generateBendingConstraints(2);
psb2->setTotalMass(1);
psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
psb2->m_cfg.kDF = 20;
psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
psb->translate(btVector3(0,0,0));
getDeformableDynamicsWorld()->addSoftBody(psb2);
btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true);
getDeformableDynamicsWorld()->addForce(psb2, mass_spring2);
m_forces.push_back(mass_spring2);
btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb2, gravity_force2);
m_forces.push_back(gravity_force2);
}
getDeformableDynamicsWorld()->setImplicit(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ClothFriction::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* ClothFrictionCreateFunc(struct CommonExampleOptions& options)
{
return new ClothFriction(options.m_guiHelper);
}

19
examples/DeformableDemo/ClothFriction.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 CLOTH_FRICTION_H
#define CLOTH_FRICTION_H
class CommonExampleInterface* ClothFrictionCreateFunc(struct CommonExampleOptions& options);
#endif //CLOTH_FRICTION_H

236
examples/DeformableDemo/DeformableClothAnchor.cpp Normal file
View File

@@ -0,0 +1,236 @@
/*
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 "DeformableClothAnchor.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableClothAnchor shows contact between deformable objects and rigid objects.
class DeformableClothAnchor : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableClothAnchor(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableClothAnchor()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 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);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void DeformableClothAnchor::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -50, 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(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 6;
const int r = 9;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s), r, r, 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, h, -(s + 3.5)));
btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3)));
psb->appendDeformableAnchor(0, body);
psb->appendDeformableAnchor(r - 1, body);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableClothAnchor::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableClothAnchor(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableClothAnchor.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 DEFORMABLE_CLOTH_ANCHOR_H
#define DEFORMABLE_CLOTH_ANCHOR_H
class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options);
#endif //DEFORMABLE_CLOTH_ANCHOR_H

264
examples/DeformableDemo/DeformableContact.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 "DeformableContact.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableContact shows the contact between deformable objects
class DeformableContact : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableContact(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableContact()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 12;
float pitch = -50;
float yaw = 120;
float targetPos[3] = {0, -3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
//btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// StddeformableWorld->getDrawFlags());
}
}
}
};
void DeformableContact::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -32, 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(2);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
btScalar s = 4;
btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
20,20,
1 + 2 + 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 0;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
h = 2;
s = 2;
btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
10,10,
0, true);
psb2->getCollisionShape()->setMargin(0.1);
psb2->generateBendingConstraints(2);
psb2->setTotalMass(1);
psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
psb2->m_cfg.kDF = 0.5;
psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
psb->translate(btVector3(3.5,0,0));
getDeformableDynamicsWorld()->addSoftBody(psb2);
btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true);
getDeformableDynamicsWorld()->addForce(psb2, mass_spring2);
m_forces.push_back(mass_spring2);
btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb2, gravity_force2);
m_forces.push_back(gravity_force2);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
int numInstances = m_guiHelper->getRenderInterface()->getTotalNumInstances();
double rgbaColors[3][4] = { { 1, 0, 0, 1 } , { 0, 1, 0, 1 } ,{ 0, 0, 1, 1 } };
for (int i = 0; i < numInstances; i++)
{
m_guiHelper->changeInstanceFlags(i, B3_INSTANCE_DOUBLE_SIDED);
}
}
void DeformableContact::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* DeformableContactCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableContact(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableContact.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 DEFORMABLE_CONTACT_H
#define DEFORMABLE_CONTACT_H
class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options);
#endif //_DEFORMABLE_CONTACT_H

402
examples/DeformableDemo/DeformableMultibody.cpp Normal file
View File

@@ -0,0 +1,402 @@
/*
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 "DeformableMultibody.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../SoftDemo/BunnyMesh.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableMultibody demo deformable bodies self-collision
static bool g_floatingBase = true;
static float friction = 1.;
class DeformableMultibody : public CommonMultiBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableMultibody(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
}
virtual ~DeformableMultibody()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 30;
float pitch = -30;
float yaw = 100;
float targetPos[3] = {0, -10, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
virtual void stepSimulation(float deltaTime);
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);
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonMultiBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void DeformableMultibody::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* deformableBodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol;
sol = new btDeformableMultiBodyConstraintSolver;
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -40, 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);
}
{
bool damping = true;
bool gyro = false;
int numLinks = 4;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(.4, 1, .4);
btVector3 baseHalfExtents(.4, 1, .4);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
if (numLinks > 0)
{
btScalar q0 = 0.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);
}
// create a patch of cloth
{
btScalar h = 0;
const btScalar s = 4;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
20,20,
// 3,3,
1 + 2 + 4 + 8, true);
psb->getCollisionShape()->setMargin(0.25);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->setCollisionFlags(0);
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
}
getDeformableDynamicsWorld()->setImplicit(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableMultibody::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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;
}
void DeformableMultibody::stepSimulation(float deltaTime)
{
// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
}
btMultiBody* DeformableMultibody::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 = .1f;
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);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = .1f;
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();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void DeformableMultibody::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();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
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(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);
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* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableMultibody(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableMultibody.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 _DEFORMABLE_MULTIBODY_H
#define _DEFORMABLE_MULTIBODY_H
class CommonExampleInterface* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options);
#endif //_DEFORMABLE_MULTIBODY_H

291
examples/DeformableDemo/DeformableRigid.cpp Normal file
View File

@@ -0,0 +1,291 @@
/*
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 "DeformableRigid.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableRigid shows contact between deformable objects and rigid objects.
class DeformableRigid : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableRigid(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableRigid()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 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);
}
void Ctor_RbUpStack(int count)
{
float mass = 0.2;
btCompoundShape* cylinderCompound = new btCompoundShape;
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
btTransform localTransform;
localTransform.setIdentity();
cylinderCompound->addChildShape(localTransform, boxShape);
btQuaternion orn(SIMD_HALF_PI, 0, 0);
localTransform.setRotation(orn);
// localTransform.setOrigin(btVector3(1,1,1));
cylinderCompound->addChildShape(localTransform, cylinderShape);
btCollisionShape* shape[] = {
new btBoxShape(btVector3(1, 1, 1)),
// new btSphereShape(0.75),
// cylinderCompound
};
// static const int nshapes = sizeof(shape) / sizeof(shape[0]);
// for (int i = 0; i < count; ++i)
// {
// btTransform startTransform;
// startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 2+ 2 * i, 0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
// createRigidBody(mass, startTransform, shape[i % nshapes]);
// }
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(1, 1.5, 1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(1, 1.5, -1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(-1, 1.5, 1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(-1, 1.5, -1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(0, 3.5, 0));
createRigidBody(mass, startTransform, shape[0]);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void DeformableRigid::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -32, 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(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
bool onGround = false;
const btScalar s = 4;
const btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
// 3,3,
20,20,
1 + 2 + 4 + 8, true);
// 0, true);
if (onGround)
psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
// 20,20,
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = .4;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
// add a few rigid bodies
Ctor_RbUpStack(1);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableRigid::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* DeformableRigidCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableRigid(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableRigid.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 _DEFORMABLE_RIGID_H
#define _DEFORMABLE_RIGID_H
class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options);
#endif //_DEFORMABLE_RIGID_H

234
examples/DeformableDemo/DeformableSelfCollision.cpp Normal file
View File

@@ -0,0 +1,234 @@
/*
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 "DeformableSelfCollision.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableSelfCollision shows deformable self collisions
class DeformableSelfCollision : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableSelfCollision(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableSelfCollision()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 10;
float pitch = -8;
float yaw = 100;
float targetPos[3] = {0, -10, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void DeformableSelfCollision::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -100, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -35, 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(40);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
const btScalar s = 2;
const btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -4*s),
btVector3(+s, h, -4*s),
btVector3(-s, h, +4*s),
btVector3(+s, h, +4*s),
10,40,
0, true, 0.01);
psb->getCollisionShape()->setMargin(0.2);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 0.2;
psb->rotate(btQuaternion(0,SIMD_PI / 2, 0));
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->setSelfCollision(true);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,0.2, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableSelfCollision::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableSelfCollision(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableSelfCollision.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 DEFORMABLE_SELF_COLLISION_H
#define DEFORMABLE_SELF_COLLISION_H
class CommonExampleInterface* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options);
#endif //_DEFORMABLE_SELF_COLLISION_H

545
examples/DeformableDemo/GraspDeformable.cpp Normal file
View File

@@ -0,0 +1,545 @@
/*
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 "GraspDeformable.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.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 GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
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 GraspDeformable : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
GraspDeformable(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~GraspDeformable()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 2;
float pitch = -45;
float yaw = 100;
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(2);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(2);
}
motor->setMaxAppliedImpulse(1);
}
}
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);
}
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()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
};
void GraspDeformable::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -9.81, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// build a gripper
if(1)
{
bool damping = true;
bool gyro = false;
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
btVector3 linkHalfExtents(.1, .2, .04);
btVector3 baseHalfExtents(.1, 0.02, .2);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, .7f,0.f), linkHalfExtents, baseHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
float maxMotorImpulse = 1.f;
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(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -25-.6, 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.1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create a soft block
if (1)
{
char absolute_path[1024];
b3BulletDefaultFileIO fileio;
fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
// fileio.findResourcePath("banana.vtk", absolute_path, 1024);
// fileio.findResourcePath("ball.vtk", absolute_path, 1024);
// fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024);
// fileio.findResourcePath("single_tet.vtk", absolute_path, 1024);
// fileio.findResourcePath("tube.vtk", absolute_path, 1024);
// fileio.findResourcePath("torus.vtk", absolute_path, 1024);
// fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024);
// fileio.findResourcePath("bread.vtk", absolute_path, 1024);
// fileio.findResourcePath("boot.vtk", absolute_path, 1024);
// btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
// TetraCube::getElements(),
// 0,
// TetraCube::getNodes(),
// false, true, true);
btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path);
// psb->scale(btVector3(30, 30, 30)); // for banana
psb->scale(btVector3(.7, .7, .7));
// psb->scale(btVector3(2, 2, 2));
// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
psb->scale(btVector3(.1, .1, .1)); // for ditto
// psb->translate(btVector3(.25, 10, 0.4));
psb->getCollisionShape()->setMargin(0.0005);
psb->setMaxStress(50);
psb->setTotalMass(.01);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 20;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
// create a piece of cloth
if(0)
{
bool onGround = false;
const btScalar s = .1;
const btScalar h = 1;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
20,20,
0, true);
if (onGround)
psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
// 20,20,
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.005);
psb->generateBendingConstraints(2);
psb->setTotalMass(.01);
psb->setSpringStiffness(10);
psb->setDampingCoefficient(0.05);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.05, false));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
slider.m_minVal = -.2;
slider.m_maxVal = .2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -.3;
slider.m_maxVal = .3;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void GraspDeformable::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* GraspDeformable::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 = 100.f;
float linkMass = 100.f;
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] * 8.f, -baseHalfExtents[2] * 4.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 4.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.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 GraspDeformable::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);
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);
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* GraspDeformableCreateFunc(struct CommonExampleOptions& options)
{
return new GraspDeformable(options.m_guiHelper);
}

19
examples/DeformableDemo/GraspDeformable.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 _GRASP_DEFORMABLE_H
#define _GRASP_DEFORMABLE_H
class CommonExampleInterface* GraspDeformableCreateFunc(struct CommonExampleOptions& options);
#endif //_GRASP_DEFORMABLE_H

395
examples/DeformableDemo/MultibodyClothAnchor.cpp Normal file
View File

@@ -0,0 +1,395 @@
/*
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 "MultibodyClothAnchor.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The MultibodyClothAnchor shows contact between deformable objects and rigid objects.
class MultibodyClothAnchor : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
MultibodyClothAnchor(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~MultibodyClothAnchor()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 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);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
}
}
btMultiBody* createMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
void MultibodyClothAnchor::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -20, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -35, 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(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 6;
const int r = 9;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s), r, r, 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
bool damping = true;
bool gyro = false;
int numLinks = 5;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(1.5, .5, .5);
btVector3 baseHalfExtents(1.5, .5, .5);
btMultiBody* mbC = createMultiBody(getDeformableDynamicsWorld(), numLinks, btVector3(s+3.5f, h, -s-0.6f), linkHalfExtents, baseHalfExtents, spherical, true);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
if (numLinks > 0)
{
btScalar q0 = 0.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(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
// quick hack: advance time to populate the variables in multibody
m_dynamicsWorld->stepSimulation(SIMD_EPSILON, 0);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mbC->forwardKinematics(scratch_q, scratch_m);
psb->appendDeformableAnchor(0, mbC->getLink(3).m_collider);
psb->appendDeformableAnchor(r - 1, mbC->getLink(0).m_collider);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void MultibodyClothAnchor::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* MultibodyClothAnchor::createMultiBody(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 = 1.f;
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);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(0, 1, 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(-linkHalfExtents[0] * 2.f, 0, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(-linkHalfExtents[0], 0, 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->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void MultibodyClothAnchor::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()};
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];
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* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options)
{
return new MultibodyClothAnchor(options.m_guiHelper);
}

19
examples/DeformableDemo/MultibodyClothAnchor.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 MULTIBODY_CLOTH_ANCHOR_H
#define MULTIBODY_CLOTH_ANCHOR_H
class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options);
#endif //MULTIBODY_CLOTH_ANCHOR_H

387
examples/DeformableDemo/Pinch.cpp Normal file
View File

@@ -0,0 +1,387 @@
/*
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 "Pinch.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
class Pinch : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
Pinch(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~Pinch()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
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);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
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()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void dynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
if (rbs.size()<2)
return;
btRigidBody* rb0 = rbs[0];
btScalar pressTime = 0.9;
btScalar liftTime = 2.5;
btScalar shiftTime = 3.5;
btScalar holdTime = 4.5*1000;
btScalar dropTime = 5.3*1000;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0.5,3,4);
btVector3 initialTranslationRight = btVector3(0.5,3,-4);
btVector3 pinchVelocityLeft = btVector3(0,0,-2);
btVector3 pinchVelocityRight = btVector3(0,0,2);
btVector3 liftVelocity = btVector3(0,5,0);
btVector3 shiftVelocity = btVector3(0,0,5);
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 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 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 Pinch::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->setSolverCallback(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//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);
}
// create a soft block
{
btScalar verts[24] = {0.f, 0.f, 0.f,
1.f, 0.f, 0.f,
0.f, 1.f, 0.f,
0.f, 0.f, 1.f,
1.f, 1.f, 0.f,
0.f, 1.f, 1.f,
1.f, 0.f, 1.f,
1.f, 1.f, 1.f
};
int triangles[60] = {0, 6, 3,
0,1,6,
7,5,3,
7,3,6,
4,7,6,
4,6,1,
7,2,5,
7,4,2,
0,3,2,
2,3,5,
0,2,4,
0,4,1,
0,6,5,
0,6,4,
3,4,2,
3,4,7,
2,7,3,
2,7,1,
4,5,0,
4,5,6,
};
// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
////
btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
psb->scale(btVector3(2, 2, 2));
psb->translate(btVector3(0, 4, 0));
psb->getCollisionShape()->setMargin(0.1);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btSoftBodyHelpers::generateBoundaryFaces(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
// add a grippers
createGrip();
}
getDeformableDynamicsWorld()->setImplicit(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Pinch::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* PinchCreateFunc(struct CommonExampleOptions& options)
{
return new Pinch(options.m_guiHelper);
}

19
examples/DeformableDemo/Pinch.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 _PINCH_H
#define _PINCH_H
class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options);
#endif //_PINCH_H

400
examples/DeformableDemo/PinchFriction.cpp Normal file
View File

@@ -0,0 +1,400 @@
/*
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 "PinchFriction.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The PinchFriction shows the frictional contacts among volumetric deformable objects
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
class PinchFriction : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
PinchFriction(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~PinchFriction()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
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);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
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()
{
CommonRigidBodyBase::renderScene();
}
};
void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
if (rbs.size()<2)
return;
btRigidBody* rb0 = rbs[0];
btScalar pressTime = 0.45;
btScalar liftTime = 5;
btScalar shiftTime = 1.75;
btScalar holdTime = 7.5;
btScalar dropTime = 8.3;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0.5,3,4);
btVector3 initialTranslationRight = btVector3(0.5,3,-4);
btVector3 PinchFrictionVelocityLeft = btVector3(0,0,-2);
btVector3 PinchFrictionVelocityRight = btVector3(0,0,2);
btVector3 liftVelocity = btVector3(0,2,0);
btVector3 shiftVelocity = btVector3(0,0,0);
btVector3 holdVelocity = btVector3(0,0,0);
btVector3 openVelocityLeft = btVector3(0,0,4);
btVector3 openVelocityRight = btVector3(0,0,-4);
if (time < pressTime)
{
velocity = PinchFrictionVelocityLeft;
translation = initialTranslationLeft + PinchFrictionVelocityLeft * time;
}
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityLeft;
translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationLeft + PinchFrictionVelocityLeft * 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 = PinchFrictionVelocityRight;
translation = initialTranslationRight + PinchFrictionVelocityRight * time;
}
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityRight;
translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationRight + PinchFrictionVelocityRight * 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(200);
rb1->setFriction(200);
}
void PinchFriction::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
getDeformableDynamicsWorld()->setSolverCallback(dynamics2);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//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);
}
// create a soft block
{
btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
psb->scale(btVector3(2, 2, 1));
psb->translate(btVector3(0, 2.1, 2.2));
psb->getCollisionShape()->setMargin(0.05);
psb->setTotalMass(.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 = 2;
btSoftBodyHelpers::generateBoundaryFaces(psb);
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
}
// create a second soft block
{
btSoftBody* psb2 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
psb2->scale(btVector3(2, 2, 1));
psb2->translate(btVector3(0, 2.1, -2.2));
psb2->getCollisionShape()->setMargin(0.05);
psb2->setTotalMass(.6);
psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
psb2->m_cfg.kDF = 2;
psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
btSoftBodyHelpers::generateBoundaryFaces(psb2);
getDeformableDynamicsWorld()->addSoftBody(psb2);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb2, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1);
getDeformableDynamicsWorld()->addForce(psb2, neohookean);
m_forces.push_back(neohookean);
}
// create a third soft block
{
btSoftBody* psb3 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
psb3->scale(btVector3(2, 2, 1));
psb3->translate(btVector3(0, 2.1, 0));
psb3->getCollisionShape()->setMargin(0.05);
psb3->setTotalMass(.6);
psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb3->m_cfg.kCHR = 1; // collision hardness with rigid body
psb3->m_cfg.kDF = 2;
psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
btSoftBodyHelpers::generateBoundaryFaces(psb3);
getDeformableDynamicsWorld()->addSoftBody(psb3);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb3, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1);
getDeformableDynamicsWorld()->addForce(psb3, neohookean);
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
// add a pair of grippers
createGrip();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void PinchFriction::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* PinchFrictionCreateFunc(struct CommonExampleOptions& options)
{
return new PinchFriction(options.m_guiHelper);
}

19
examples/DeformableDemo/PinchFriction.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 _PINCH_FRICTION_H
#define _PINCH_FRICTION_H
class CommonExampleInterface* PinchFrictionCreateFunc(struct CommonExampleOptions& options);
#endif //_PINCH_FRICTION_H

259
examples/DeformableDemo/SplitImpulse.cpp Normal file
View File

@@ -0,0 +1,259 @@
/*
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 "SplitImpulse.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#define USE_SPLIT_IMPULSE 1
///The SplitImpulse shows the effect of split impulse in deformable rigid contact.
class SplitImpulse : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
SplitImpulse(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~SplitImpulse()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 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);
}
void Ctor_RbUpStack(int count)
{
float mass = 0.2;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(1, 1, 1)),
};
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, 0.7, 0));
createRigidBody(mass, startTransform, shape[0]);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void SplitImpulse::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* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -50, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///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, -32, 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(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
#ifdef USE_SPLIT_IMPULSE
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.03;
#else
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.0;
#endif
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
// 3,3,
20,20,
1 + 2 + 4 + 8, true);
// 0, true);
psb->getCollisionShape()->setMargin(0.15);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
// add a few rigid bodies
Ctor_RbUpStack(1);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void SplitImpulse::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* SplitImpulseCreateFunc(struct CommonExampleOptions& options)
{
return new SplitImpulse(options.m_guiHelper);
}

19
examples/DeformableDemo/SplitImpulse.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 _SPLIT_IMPULSE_H
#define _SPLIT_IMPULSE_H
class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options);
#endif //_SPLIT_IMPULSE_H

293
examples/DeformableDemo/VolumetricDeformable.cpp Normal file
View File

@@ -0,0 +1,293 @@
/*
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 "VolumetricDeformable.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects.
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
class VolumetricDeformable : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
VolumetricDeformable(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~VolumetricDeformable()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, 3, 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);
}
void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
{
btCollisionShape* box = new btBoxShape(halfEdge);
m_collisionShapes.push_back(box);
btTransform Transform;
Transform.setIdentity();
Transform.setOrigin(translation);
Transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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)
box->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(Transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, box, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
void Ctor_RbUpStack(int count)
{
float mass = 1;
btCompoundShape* cylinderCompound = new btCompoundShape;
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
btTransform localTransform;
localTransform.setIdentity();
cylinderCompound->addChildShape(localTransform, boxShape);
btQuaternion orn(SIMD_HALF_PI, 0, 0);
localTransform.setRotation(orn);
// localTransform.setOrigin(btVector3(1,1,1));
cylinderCompound->addChildShape(localTransform, cylinderShape);
btCollisionShape* shape[] = {
new btBoxShape(btVector3(1, 1, 1)),
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(i, 10 + 2 * i, i-1));
createRigidBody(mass, startTransform, shape[i % nshapes]);
}
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void VolumetricDeformable::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* deformableBodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
btVector3 gravity = btVector3(0, -100, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(50.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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);
}
createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
createStaticBox(btVector3(5, 5, 1), btVector3(0,0,5));
createStaticBox(btVector3(5, 5, 1), btVector3(0,0,-5));
// create volumetric soft body
{
btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(2, 2, 2));
psb->translate(btVector3(0, 5, 0));
psb->getCollisionShape()->setMargin(0.25);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 0.5;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
btSoftBodyHelpers::generateBoundaryFaces(psb);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
// add a few rigid bodies
Ctor_RbUpStack(4);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void VolumetricDeformable::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete 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* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options)
{
return new VolumetricDeformable(options.m_guiHelper);
}

19
examples/DeformableDemo/VolumetricDeformable.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 _VOLUMETRIC_DEFORMABLE_H
#define _VOLUMETRIC_DEFORMABLE_H
class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options);
#endif //_VOLUMETRIC_DEFORMABLE__H

30
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -220,12 +220,6 @@ SET(BulletExampleBrowser_SRCS
../MultiThreadedDemo/CommonRigidBodyMTBase.h
../Heightfield/HeightfieldExample.cpp
../Heightfield/HeightfieldExample.h
../BlockSolver/btBlockSolver.cpp
../BlockSolver/btBlockSolver.h
../BlockSolver/BlockSolverExample.cpp
../BlockSolver/BlockSolverExample.h
../BlockSolver/RigidBodyBoxes.cpp
../BlockSolver/RigidBodyBoxes.h
../Tutorial/Tutorial.cpp
../Tutorial/Tutorial.h
../Tutorial/Dof6ConstraintTutorial.cpp
@@ -359,6 +353,30 @@ SET(BulletExampleBrowser_SRCS
../MultiBody/MultiBodyConstraintFeedback.cpp
../SoftDemo/SoftDemo.cpp
../SoftDemo/SoftDemo.h
../DeformableDemo/DeformableContact.cpp
../DeformableDemo/DeformableContact.h
../DeformableDemo/GraspDeformable.cpp
../DeformableDemo/GraspDeformable.h
../DeformableDemo/Pinch.cpp
../DeformableDemo/Pinch.h
../DeformableDemo/DeformableSelfCollision.cpp
../DeformableDemo/DeformableSelfCollision.h
../DeformableDemo/PinchFriction.cpp
../DeformableDemo/PinchFriction.h
../DeformableDemo/ClothFriction.cpp
../DeformableDemo/ClothFriction.h
../DeformableDemo/DeformableMultibody.cpp
../DeformableDemo/DeformableMultibody.h
../DeformableDemo/DeformableRigid.cpp
../DeformableDemo/DeformableRigid.h
../DeformableDemo/SplitImpulse.cpp
../DeformableDemo/SplitImpulse.h
../DeformableDemo/VolumetricDeformable.cpp
../DeformableDemo/VolumetricDeformable.h
../DeformableDemo/DeformableClothAnchor.cpp
../DeformableDemo/DeformableClothAnchor.h
../DeformableDemo/MultibodyClothAnchor.cpp
../DeformableDemo/MultibodyClothAnchor.h
../MultiBody/MultiDofDemo.cpp
../MultiBody/MultiDofDemo.h
../RigidBody/RigidBodySoftContact.cpp

40
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -1,8 +1,5 @@
#include "ExampleEntries.h"
#include "../BlockSolver/btBlockSolver.h"
#include "../BlockSolver/BlockSolverExample.h"
#include "../BlockSolver/RigidBodyBoxes.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "EmptyExample.h"
#include "../Heightfield/HeightfieldExample.h"
@@ -47,6 +44,18 @@
#include "../FractureDemo/FractureDemo.h"
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../DeformableDemo/DeformableRigid.h"
#include "../DeformableDemo/SplitImpulse.h"
#include "../DeformableDemo/ClothFriction.h"
#include "../DeformableDemo/Pinch.h"
#include "../DeformableDemo/DeformableSelfCollision.h"
#include "../DeformableDemo/PinchFriction.h"
#include "../DeformableDemo/DeformableMultibody.h"
#include "../DeformableDemo/VolumetricDeformable.h"
#include "../DeformableDemo/GraspDeformable.h"
#include "../DeformableDemo/DeformableContact.h"
#include "../DeformableDemo/DeformableClothAnchor.h"
#include "../DeformableDemo/MultibodyClothAnchor.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsClientExample.h"
@@ -117,7 +126,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Basic Example", "Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
AllConstraintCreateFunc),
@@ -159,13 +168,6 @@ static ExampleEntry gDefaultExamples[] =
//
// ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(0, "BlockSolver"),
ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_SI),
ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
//ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI),
//ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK),
ExampleEntry(0, "Inverse Dynamics"),
ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF),
@@ -190,6 +192,21 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc, TUT_SPHERE_PLANE_BULLET2),
//ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3),
ExampleEntry(0, "Deformabe Body"),
ExampleEntry(1, "Deformable Self Collision", "Deformable Self Collision", DeformableSelfCollisionCreateFunc),
ExampleEntry(1, "Deformable-Deformable Contact", "Deformable contact", DeformableContactCreateFunc),
ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc),
ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc),
ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
ExampleEntry(1, "Split Impulse Contact", "Split impulse test", SplitImpulseCreateFunc),
ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
// ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
#ifdef INCLUDE_CLOTH_DEMOS
ExampleEntry(0, "Soft Body"),
ExampleEntry(1, "Cloth", "Simulate a patch of cloth.", SoftDemoCreateFunc, 0),
@@ -286,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
ExampleEntry(1, "Gripper Grasp", "Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc, eGRIPPER_GRASP),
ExampleEntry(1, "Two Point Grasp", "Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
ExampleEntry(1, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
ExampleEntry(1, "One Motor Gripper Grasp", "Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),

2
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -290,7 +290,7 @@ void GwenUserInterface::init(int width, int height, Gwen::Renderer::Base* render
//tab->SetHeight(300);
tab->SetWidth(240);
tab->SetHeight(1250);
tab->SetHeight(13250);
//tab->Dock(Gwen::Pos::Left);
tab->Dock(Gwen::Pos::Fill);
//tab->SetMargin( Gwen::Margin( 2, 2, 2, 2 ) );

7
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -921,6 +921,13 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
m_internalData->m_app = s_app;
char* gVideoFileName = 0;
args.GetCmdLineArgument("mp4", gVideoFileName);
int gVideoFps = 0;
args.GetCmdLineArgument("mp4fps", gVideoFps);
if (gVideoFps)
{
simpleApp->setMp4Fps(gVideoFps);
}
#ifndef NO_OPENGL3
if (gVideoFileName)
simpleApp->dumpFramesToVideo(gVideoFileName);

18
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -380,6 +380,14 @@ void OpenGLGuiHelper::replaceTexture(int shapeIndex, int textureUid)
m_data->m_glApp->m_renderer->replaceTexture(shapeIndex, textureUid);
};
}
void OpenGLGuiHelper::changeInstanceFlags(int instanceUid, int flags)
{
if (instanceUid >= 0)
{
//careful, flags/instanceUid is swapped
m_data->m_glApp->m_renderer->writeSingleInstanceFlagsToCPU( flags, instanceUid);
}
}
void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4])
{
if (instanceUid >= 0)
@@ -1433,6 +1441,11 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
color.setValue(1, 1, 1, 1);
}
createCollisionObjectGraphicsObject(colObj, color);
if (sb)
{
int graphicsInstanceId = colObj->getUserIndex();
changeInstanceFlags(graphicsInstanceId, B3_INSTANCE_DOUBLE_SIDED);
}
}
}
@@ -1496,3 +1509,8 @@ void OpenGLGuiHelper::computeSoftBodyVertices(btCollisionShape* collisionShape,
}
}
}
void OpenGLGuiHelper::updateShape(int shapeIndex, float* vertices)
{
m_data->m_glApp->m_renderer->updateShape(shapeIndex, vertices);
}

2
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -27,12 +27,14 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
virtual void removeAllGraphicsInstances();
virtual void removeGraphicsInstance(int graphicsUid);
virtual void changeInstanceFlags(int instanceUid, int flags);
virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]);
virtual void changeSpecularColor(int instanceUid, const double specularColor[3]);
virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height);
virtual void removeTexture(int textureUid);
virtual int getShapeIndexFromInstance(int instanceUid);
virtual void replaceTexture(int shapeIndex, int textureUid);
virtual void updateShape(int shapeIndex, float* vertices);
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);

3
examples/ExampleBrowser/premake4.lua
View File

@@ -168,7 +168,6 @@ project "App_BulletExampleBrowser"
"../Evolution/NN3DWalkers.h",
"../Collision/*",
"../RoboticsLearning/*",
"../BlockSolver/*",
"../Collision/Internal/*",
"../Benchmarks/*",
"../MultiThreadedDemo/*",
@@ -182,6 +181,7 @@ project "App_BulletExampleBrowser"
"../RenderingExamples/*",
"../VoronoiFracture/*",
"../SoftDemo/*",
"../DeformableDemo/*",
"../RollingFrictionDemo/*",
"../rbdl/*",
"../FractureDemo/*",
@@ -199,7 +199,6 @@ project "App_BulletExampleBrowser"
"../RigidBody/RigidBodySoftContact.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
"../ThirdPartyLibs/BussIK/*",
"../GyroscopicDemo/GyroscopicSetup.cpp",
"../GyroscopicDemo/GyroscopicSetup.h",
"../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",

26
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -2265,7 +2265,7 @@ int BulletMJCFImporter::getBodyUniqueId() const
return m_data->m_activeBodyUniqueId;
}
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
{
btCompoundShape* compound = new btCompoundShape();
compound->setMargin(collisionMargin);
@@ -2278,25 +2278,26 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::sha
btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(collisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f < faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
}
@@ -2391,10 +2392,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
}
break;
}

7
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -65,13 +65,14 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
btVector3 shift(0, 0, 0);
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
{
B3_PROFILE("tinyobj::LoadObj");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO);
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
}
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
{
B3_PROFILE("Load Texture");
//int textureIndex = -1;
@@ -84,7 +85,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
meshData.m_rgbaColor[2] = shape.material.diffuse[2];
meshData.m_rgbaColor[3] = shape.material.transparency;
meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR;
meshData.m_specularColor[0] = shape.material.specular[0];
meshData.m_specularColor[1] = shape.material.specular[1];
meshData.m_specularColor[2] = shape.material.specular[2];

14
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
View File

@@ -12,6 +12,7 @@ struct CachedObjResult
{
std::string m_msg;
std::vector<tinyobj::shape_t> m_shapes;
tinyobj::attrib_t m_attribute;
};
static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
@@ -31,24 +32,26 @@ void b3EnableFileCaching(int enable)
}
std::string LoadFromCachedOrFromObj(
tinyobj::attrib_t& attribute,
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath,
struct CommonFileIOInterface* fileIO
)
struct CommonFileIOInterface* fileIO)
{
CachedObjResult* resultPtr = gCachedObjResults[filename];
if (resultPtr)
{
const CachedObjResult& result = *resultPtr;
shapes = result.m_shapes;
attribute = result.m_attribute;
return result.m_msg;
}
std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO);
std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
CachedObjResult result;
result.m_msg = err;
result.m_shapes = shapes;
result.m_attribute = attribute;
if (gEnableFileCaching)
{
gCachedObjResults.insert(filename, result);
@@ -60,14 +63,15 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const cha
{
B3_PROFILE("LoadMeshFromObj");
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
{
B3_PROFILE("tinyobj::LoadObj2");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO);
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
}
{
B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj");
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
return gfxShape;
}
}

2
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
View File

@@ -8,8 +8,8 @@ struct GLInstanceGraphicsShape;
int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj(
tinyobj::attrib_t& attribute,
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath,

105
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
View File

@@ -9,7 +9,7 @@
#include "../../OpenGLWindow/GLInstancingRenderer.h"
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading)
GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading)
{
b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
{
@@ -36,19 +36,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
}
GLInstanceVertex vtx0;
vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 0];
vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 1];
vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 2];
tinyobj::index_t v_0 = shape.mesh.indices[f];
vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index];
vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1];
vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
vtx0.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
if (attribute.texcoords.size())
{
int uv0Index = shape.mesh.indices[f] * 2 + 0;
int uv1Index = shape.mesh.indices[f] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())))
int uv0Index = 2 * v_0.texcoord_index;
int uv1Index = 2 * v_0.texcoord_index + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())))
{
vtx0.uv[0] = shape.mesh.texcoords[uv0Index];
vtx0.uv[1] = shape.mesh.texcoords[uv1Index];
vtx0.uv[0] = attribute.texcoords[uv0Index];
vtx0.uv[1] = attribute.texcoords[uv1Index];
}
else
{
@@ -64,19 +65,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
}
GLInstanceVertex vtx1;
vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0];
vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1];
vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2];
tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index];
vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1];
vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
vtx1.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
if (attribute.texcoords.size())
{
int uv0Index = shape.mesh.indices[f + 1] * 2 + 0;
int uv1Index = shape.mesh.indices[f + 1] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
int uv0Index = 2 * v_1.texcoord_index;
int uv1Index = 2 * v_1.texcoord_index + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
{
vtx1.uv[0] = shape.mesh.texcoords[uv0Index];
vtx1.uv[1] = shape.mesh.texcoords[uv1Index];
vtx1.uv[0] = attribute.texcoords[uv0Index];
vtx1.uv[1] = attribute.texcoords[uv1Index];
}
else
{
@@ -92,22 +94,24 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
}
GLInstanceVertex vtx2;
vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0];
vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1];
vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2];
tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index];
vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1];
vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];
vtx2.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
if (attribute.texcoords.size())
{
int uv0Index = shape.mesh.indices[f + 2] * 2 + 0;
int uv1Index = shape.mesh.indices[f + 2] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
int uv0Index = 2 * v_2.texcoord_index;
int uv1Index = 2 * v_2.texcoord_index + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
{
vtx2.uv[0] = shape.mesh.texcoords[uv0Index];
vtx2.uv[1] = shape.mesh.texcoords[uv1Index];
vtx2.uv[0] = attribute.texcoords[uv0Index];
vtx2.uv[1] = attribute.texcoords[uv1Index];
}
else
{
b3Warning("obj texture coordinate out-of-range!");
//b3Warning("obj texture coordinate out-of-range!");
vtx2.uv[0] = 0;
vtx2.uv[1] = 0;
}
@@ -123,16 +127,21 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
btVector3 v2(vtx2.xyzw[0], vtx2.xyzw[1], vtx2.xyzw[2]);
unsigned int maxIndex = 0;
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 2);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 2);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 2);
bool hasNormals = (shape.mesh.normals.size() && maxIndex < shape.mesh.normals.size());
unsigned n0Index = shape.mesh.indices[f].normal_index;
unsigned n1Index = shape.mesh.indices[f + 1].normal_index;
unsigned n2Index = shape.mesh.indices[f + 2].normal_index;
maxIndex = b3Max(maxIndex, 3 * n0Index + 0);
maxIndex = b3Max(maxIndex, 3 * n0Index + 1);
maxIndex = b3Max(maxIndex, 3 * n0Index + 2);
maxIndex = b3Max(maxIndex, 3 * n1Index + 0);
maxIndex = b3Max(maxIndex, 3 * n1Index + 1);
maxIndex = b3Max(maxIndex, 3 * n1Index + 2);
maxIndex = b3Max(maxIndex, 3 * n2Index + 0);
maxIndex = b3Max(maxIndex, 3 * n2Index + 1);
maxIndex = b3Max(maxIndex, 3 * n2Index + 2);
bool hasNormals = (attribute.normals.size() && maxIndex < attribute.normals.size());
if (flatShading || !hasNormals)
{
@@ -159,15 +168,15 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
}
else
{
vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 0];
vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 1];
vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 2]; //shape.mesh.indices[f+1]*3+0
vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 0];
vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 1];
vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 2];
vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 0];
vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 1];
vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 2];
vtx0.normal[0] = attribute.normals[3 * n0Index+ 0];
vtx0.normal[1] = attribute.normals[3 * n0Index+ 1];
vtx0.normal[2] = attribute.normals[3 * n0Index+ 2];
vtx1.normal[0] = attribute.normals[3 * n1Index+ 0];
vtx1.normal[1] = attribute.normals[3 * n1Index+ 1];
vtx1.normal[2] = attribute.normals[3 * n1Index+ 2];
vtx2.normal[0] = attribute.normals[3 * n2Index+ 0];
vtx2.normal[1] = attribute.normals[3 * n2Index+ 1];
vtx2.normal[2] = attribute.normals[3 * n2Index+ 2];
}
vertices->push_back(vtx0);
vertices->push_back(vtx1);

2
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
View File

@@ -4,6 +4,6 @@
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include <vector>
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
#endif //WAVEFRONT2GRAPHICS_H

28
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -509,7 +509,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true;
}
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
{
B3_PROFILE("createConvexHullFromShapes");
btCompoundShape* compound = new btCompoundShape();
@@ -528,20 +528,20 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
for (int f = 0; f < faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
convexHull->addPoint(pt * geomScale, false);
}
@@ -558,8 +558,6 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
return compound;
}
int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
{
UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape);
@@ -718,10 +716,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
return shape;
}

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

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

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

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

4
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@@ -100,7 +100,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
jacob->SetJendActive();
}
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
MatrixRmn AugMat;
// Calculate the change in theta values
switch (ikMethod)
{
@@ -108,7 +108,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK_DLS:
jacob->CalcDeltaThetasDLS(); // Damped least squares method
jacob->CalcDeltaThetasDLS(AugMat); // Damped least squares method
break;
case IK_DLS_SVD:
jacob->CalcDeltaThetasDLSwithSVD();

358
examples/MultiBodyBaseline/MultiBodyBaseline.cpp Normal file
View File

@@ -0,0 +1,358 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 1.
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3
#include "MultiBodyBaseline.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../SoftDemo/BunnyMesh.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The MultiBodyBaseline demo deformable bodies self-collision
static bool g_floatingBase = true;
static float friction = 1.;
class MultiBodyBaseline : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
public:
MultiBodyBaseline(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
}
virtual ~MultiBodyBaseline()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 30;
float pitch = -30;
float yaw = 100;
float targetPos[3] = {0, -10, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
virtual void stepSimulation(float deltaTime);
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);
};
void MultiBodyBaseline::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();
btMultiBodyConstraintSolver* sol;
sol = new btMultiBodyConstraintSolver;
m_solver = sol;
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
m_dynamicsWorld = world;
// m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(0, -10, 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, -40, 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);
}
{
bool damping = true;
bool gyro = false;
int numLinks = 4;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(1, 1, 1);
btVector3 baseHalfExtents(1, 1, 1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
if (numLinks > 0)
{
btScalar q0 = 0.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);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void MultiBodyBaseline::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
void MultiBodyBaseline::stepSimulation(float deltaTime)
{
// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
}
btMultiBody* MultiBodyBaseline::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 = .1f;
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);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = .1f;
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();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void MultiBodyBaseline::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(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];
// 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(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options)
{
return new MultiBodyBaseline(options.m_guiHelper);
}

20
examples/MultiBodyBaseline/MultiBodyBaseline.h Normal file
View File

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

9
examples/MultiThreading/MultiThreadingExample.cpp
View File

@@ -29,13 +29,14 @@ void SamplelsMemoryReleaseFunc(void* ptr);
#ifndef _WIN32
#include "b3PosixThreadSupport.h"
b3ThreadSupportInterface* createThreadSupport(int numThreads)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("testThreads",
SampleThreadFunc,
SamplelsMemoryFunc,
SamplelsMemoryReleaseFunc,
numThreads);
SampleThreadFunc,
SamplelsMemoryFunc,
SamplelsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
return threadSupport;

10
examples/MultiThreading/main.cpp
View File

@@ -22,7 +22,7 @@ subject to the following restrictions:
void SampleThreadFunc(void* userPtr, void* lsMemory);
void* SamplelsMemoryFunc();
void SamplelsMemoryReleaseFunc(void* ptr);
#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"
@@ -34,6 +34,7 @@ b3ThreadSupportInterface* createThreadSupport(int numThreads)
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("testThreads",
SampleThreadFunc,
SamplelsMemoryFunc,
SamplelsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
@@ -99,6 +100,13 @@ void* SamplelsMemoryFunc()
return new SampleThreadLocalStorage;
}
void SamplelsMemoryReleaseFunc(void* ptr)
{
SampleThreadLocalStorage* p = (SampleThreadLocalStorage*)ptr;
delete p;
}
int main(int argc, char** argv)
{
int numThreads = 8;

78
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -139,11 +139,7 @@ static InternalDataRenderer* sData2;
GLint lineWidthRange[2] = {1, 1};
enum
{
eGfxTransparency = 1,
eGfxHasTexture = 2,
};
struct b3GraphicsInstance
{
@@ -492,6 +488,26 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex2, flo
orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex * 4 + 2];
orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex * 4 + 3];
}
void GLInstancingRenderer::writeSingleInstanceFlagsToCPU(int flags, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
int shapeIndex = pg->m_shapeIndex;
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
if (flags & B3_INSTANCE_DOUBLE_SIDED)
{
gfxObj->m_flags |= B3_INSTANCE_DOUBLE_SIDED;
}
else
{
gfxObj->m_flags &= ~B3_INSTANCE_DOUBLE_SIDED;
}
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
@@ -502,11 +518,11 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, in
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
if (color[3] < 1)
{
gfxObj->m_flags |= eGfxTransparency;
gfxObj->m_flags |= B3_INSTANCE_TRANSPARANCY;
}
else
{
gfxObj->m_flags &= ~eGfxTransparency;
gfxObj->m_flags &= ~B3_INSTANCE_TRANSPARANCY;
}
m_data->m_instance_colors_ptr[srcIndex * 4 + 0] = float(color[0]);
@@ -525,11 +541,11 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int
if (color[3] < 1)
{
gfxObj->m_flags |= eGfxTransparency;
gfxObj->m_flags |= B3_INSTANCE_TRANSPARANCY;
}
else
{
gfxObj->m_flags &= ~eGfxTransparency;
gfxObj->m_flags &= ~B3_INSTANCE_TRANSPARANCY;
}
m_data->m_instance_colors_ptr[srcIndex * 4 + 0] = color[0];
@@ -916,7 +932,7 @@ int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const flo
if (color[3] < 1 && color[3] > 0)
{
gfxObj->m_flags |= eGfxTransparency;
gfxObj->m_flags |= B3_INSTANCE_TRANSPARANCY;
}
gfxObj->m_numGraphicsInstances++;
m_data->m_totalNumInstances++;
@@ -1018,11 +1034,11 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId)
if (textureId >= 0 && textureId < m_data->m_textureHandles.size())
{
gfxObj->m_textureIndex = textureId;
gfxObj->m_flags |= eGfxHasTexture;
gfxObj->m_flags |= B3_INSTANCE_TEXTURE;
} else
{
gfxObj->m_textureIndex = -1;
gfxObj->m_flags &= ~eGfxHasTexture;
gfxObj->m_flags &= ~B3_INSTANCE_TEXTURE;
}
}
}
@@ -1110,7 +1126,7 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
if (textureId >= 0)
{
gfxObj->m_textureIndex = textureId;
gfxObj->m_flags |= eGfxHasTexture;
gfxObj->m_flags |= B3_INSTANCE_TEXTURE;
}
gfxObj->m_primitiveType = primitiveType;
@@ -1754,6 +1770,7 @@ static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center, const
result[3 * 4 + 3] = 1.f;
}
void GLInstancingRenderer::drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float colorRGBA[4], int textureIndex, int vertexLayout)
{
int sz = sizeof(GfxVertexFormat0);
@@ -2263,7 +2280,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
inst.m_shapeIndex = obj;
if ((gfxObj->m_flags & eGfxTransparency) == 0)
if ((gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY) == 0)
{
inst.m_instanceId = curOffset;
b3Vector3 centerPosition;
@@ -2313,10 +2330,10 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
//only draw stuff (opaque/transparent) if it is the right pass
int drawThisPass = (pass == 0) == ((gfxObj->m_flags & eGfxTransparency) == 0);
int drawThisPass = (pass == 0) == ((gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY) == 0);
//transparent objects don't cast shadows (to simplify things)
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
if (renderMode == B3_CREATE_SHADOWMAP_RENDERMODE)
drawThisPass = 0;
@@ -2326,7 +2343,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
{
glActiveTexture(GL_TEXTURE0);
GLuint curBindTexture = 0;
if (gfxObj->m_flags & eGfxHasTexture)
if (gfxObj->m_flags & B3_INSTANCE_TEXTURE)
{
curBindTexture = m_data->m_textureHandles[gfxObj->m_textureIndex].m_glTexture;
@@ -2432,6 +2449,11 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
}
else
{
if (gfxObj->m_flags & B3_INSTANCE_DOUBLE_SIDED)
{
glDisable(GL_CULL_FACE);
}
switch (renderMode)
{
case B3_SEGMENTATION_MASK_RENDERMODE:
@@ -2445,7 +2467,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
}
case B3_DEFAULT_RENDERMODE:
{
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDepthMask(false);
glEnable(GL_BLEND);
@@ -2462,7 +2484,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glUniform1i(uniform_texture_diffuse, 0);
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
int instanceId = transparentInstances[i].m_instanceId;
glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid*)((instanceId)*4 * sizeof(float) + m_data->m_maxShapeCapacityInBytes));
@@ -2477,7 +2499,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
}
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDisable(GL_BLEND);
glDepthMask(true);
@@ -2495,7 +2517,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
case B3_USE_SHADOWMAP_RENDERMODE:
{
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDepthMask(false);
glEnable(GL_BLEND);
@@ -2549,7 +2571,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
//gfxObj->m_instanceOffset
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
int instanceId = transparentInstances[i].m_instanceId;
glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid*)((instanceId)*4 * sizeof(float) + m_data->m_maxShapeCapacityInBytes));
@@ -2563,7 +2585,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
}
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDisable(GL_BLEND);
glDepthMask(true);
@@ -2577,7 +2599,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
}
case B3_USE_PROJECTIVE_TEXTURE_RENDERMODE:
{
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDepthMask(false);
glEnable(GL_BLEND);
@@ -2617,7 +2639,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glUniformMatrix4fv(projectiveTexture_TextureMVP, 1, false, &textureMVP[0]);
//sort transparent objects
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
int instanceId = transparentInstances[i].m_instanceId;
glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid*)((instanceId)*4 * sizeof(float) + m_data->m_maxShapeCapacityInBytes));
@@ -2631,7 +2653,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
}
if (gfxObj->m_flags & eGfxTransparency)
if (gfxObj->m_flags & B3_INSTANCE_TRANSPARANCY)
{
glDisable(GL_BLEND);
glDepthMask(true);
@@ -2646,6 +2668,10 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
// b3Assert(0);
}
};
if (gfxObj->m_flags & B3_INSTANCE_DOUBLE_SIDED)
{
glEnable(GL_CULL_FACE);
}
}
//glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances);

1
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -95,6 +95,7 @@ public:
virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
virtual void writeSingleInstanceFlagsToCPU(int flags, int srcIndex2);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2);
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2);

3
examples/OpenGLWindow/MacOpenGLWindowObjC.m
View File

@@ -1,8 +1,11 @@
#ifndef B3_USE_GLFW
#define __OBJC2__ 1
#include <Foundation/NSExtensionContext.h>
#include "MacOpenGLWindowObjC.h"
#define GL_DO_NOT_WARN_IF_MULTI_GL_VERSION_HEADERS_INCLUDED
#import <Cocoa/Cocoa.h>
#include "OpenGLInclude.h"

2
examples/OpenGLWindow/SimpleOpenGL2Renderer.h
View File

@@ -40,7 +40,7 @@ public:
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex) {}
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex) {}
virtual void writeSingleInstanceFlagsToCPU(int flags, int srcIndex) {}
virtual void getCameraViewMatrix(float viewMat[16]) const;
virtual void getCameraProjectionMatrix(float projMat[16]) const;
virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex = -1, int vertexLayout = 0)

30
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -66,6 +66,8 @@ struct SimpleInternalData
int m_upAxis; //y=1 or z=2 is supported
int m_customViewPortWidth;
int m_customViewPortHeight;
int m_mp4Fps;
SimpleInternalData()
: m_fontTextureId(0),
m_largeFontTextureId(0),
@@ -82,7 +84,8 @@ struct SimpleInternalData
m_userPointer(0),
m_upAxis(1),
m_customViewPortWidth(-1),
m_customViewPortHeight(-1)
m_customViewPortHeight(-1),
m_mp4Fps(60)
{
}
};
@@ -1089,6 +1092,11 @@ void SimpleOpenGL3App::swapBuffer()
m_window->startRendering();
}
void SimpleOpenGL3App::setMp4Fps(int fps)
{
m_data->m_mp4Fps = fps;
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
{
@@ -1098,27 +1106,11 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
int height = (int)m_window->getRetinaScale() * m_instancingRenderer->getScreenHeight();
char cmd[8192];
#ifdef _WIN32
sprintf(cmd,
"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b:v 50000k -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s",
width, height, mp4FileName);
m_data->m_mp4Fps, width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd,
"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s",
width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -y -crf 0 -b 50000k -vf vflip %s",width,height,mp4FileName);
// sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName);
if (m_data->m_ffmpegFile)
{

1
examples/OpenGLWindow/SimpleOpenGL3App.h
View File

@@ -14,6 +14,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
class GLPrimitiveRenderer* m_primRenderer;
class GLInstancingRenderer* m_instancingRenderer;
virtual void setBackgroundColor(float red, float green, float blue);
virtual void setMp4Fps(int fps);
SimpleOpenGL3App(const char* title, int width, int height, bool allowRetina = true, int windowType = 0, int renderDevice = -1, int maxNumObjectCapacity = 128 * 1024, int maxShapeCapacityInBytes = 128 * 1024 * 1024);

131
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -294,6 +294,102 @@ public:
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
if (results.m_uniqueObjectIds.size() == 1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d", numJoints);
for (int i = 0; i < numJoints; i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo);
b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
}
for (int i = 0; i < 8; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
}
}
}
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, 0, -0.2);
args.m_startOrientation.setEulerZYX(0, 0, 0);
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
revoluteJoint1.m_parentFrame[2] = 0.02;
revoluteJoint1.m_parentFrame[3] = 0;
revoluteJoint1.m_parentFrame[4] = 0;
revoluteJoint1.m_parentFrame[5] = 0;
revoluteJoint1.m_parentFrame[6] = 1.0;
revoluteJoint1.m_childFrame[0] = 0;
revoluteJoint1.m_childFrame[1] = 0;
revoluteJoint1.m_childFrame[2] = 0;
revoluteJoint1.m_childFrame[3] = 0;
revoluteJoint1.m_childFrame[4] = 0;
revoluteJoint1.m_childFrame[5] = 0;
revoluteJoint1.m_childFrame[6] = 1.0;
revoluteJoint1.m_jointAxis[0] = 1.0;
revoluteJoint1.m_jointAxis[1] = 0.0;
revoluteJoint1.m_jointAxis[2] = 0.0;
revoluteJoint1.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint2;
revoluteJoint2.m_parentFrame[0] = 0.055;
revoluteJoint2.m_parentFrame[1] = 0;
revoluteJoint2.m_parentFrame[2] = 0.02;
revoluteJoint2.m_parentFrame[3] = 0;
revoluteJoint2.m_parentFrame[4] = 0;
revoluteJoint2.m_parentFrame[5] = 0;
revoluteJoint2.m_parentFrame[6] = 1.0;
revoluteJoint2.m_childFrame[0] = 0;
revoluteJoint2.m_childFrame[1] = 0;
revoluteJoint2.m_childFrame[2] = 0;
revoluteJoint2.m_childFrame[3] = 0;
revoluteJoint2.m_childFrame[4] = 0;
revoluteJoint2.m_childFrame[5] = 0;
revoluteJoint2.m_childFrame[6] = 1.0;
revoluteJoint2.m_jointAxis[0] = 1.0;
revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
{
SliderParams slider("Vertical velocity", &sGripperVerticalVelocity);
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);
}
{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
@@ -326,10 +422,19 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
args.m_springElasticStiffness = 1;
args.m_springDampingStiffness = .01;
args.m_springBendingStiffness = .1;
args.m_frictionCoeff = 10;
args.m_useSelfCollision = false;
args.m_useFaceContact = true;
args.m_useBendingSprings = true;
args.m_startPosition.setValue(0, 0, 0);
args.m_startOrientation.setValue(0, 0, 1, 1);
m_robotSim.loadDeformableBody("flat_napkin.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -371,7 +476,8 @@ public:
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}
m_robotSim.setNumSimulationSubSteps(2);
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{
@@ -462,6 +568,21 @@ public:
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
}
}
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
int fingerJointIndices[2] = {0, 1};
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
double maxTorqueValues[2] = {250.0, 50.0};
for (int i = 0; i < 2; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
}
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
eONE_MOTOR_GRASP = 4,
eGRASP_SOFT_BODY = 8,
eSOFTBODY_MULTIBODY_COUPLING = 16,
eGRASP_DEFORMABLE_CLOTH = 32,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

10
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -44,6 +44,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
MatrixRmn AugMat;
bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, 1);
@@ -142,12 +143,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
@@ -193,6 +194,7 @@ bool IKTrajectoryHelper::computeIK2(
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
{
MatrixRmn AugMat;
bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
@@ -250,12 +252,12 @@ bool IKTrajectoryHelper::computeIK2(
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();

172
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -338,6 +338,109 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
int len = strlen(filename);
if (len < MAX_FILENAME_LENGTH)
{
strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename);
}
else
{
command->m_loadSoftBodyArguments.m_simFileName[0] = 0;
}
command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_corotatedMu = corotatedMu;
command->m_loadSoftBodyArguments.m_corotatedLambda = corotatedLambda;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_COROTATED_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu;
command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda;
command->m_loadSoftBodyArguments.m_NeoHookeanDamping = NeoHookeanDamping;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_GRAVITY_FORCE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_collisionHardness = collisionHardness;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_COLLISION_HARDNESS;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision;
command->m_updateFlags |= LOAD_SOFT_BODY_USE_SELF_COLLISION;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_frictionCoeff = frictionCoefficient;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings;
command->m_loadSoftBodyArguments.m_springBendingStiffness = bendingStiffness;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_BENDING_SPRINGS;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_useFaceContact = useFaceContact;
command->m_updateFlags |= LOAD_SOFT_BODY_USE_FACE_CONTACT;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -677,6 +780,14 @@ B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandl
return 0;
}
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor;
command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -803,6 +914,15 @@ B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandl
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_SPARSE_SDF;
command->m_physSimParamArgs.m_sparseSdfVoxelSize = sparseSdfVoxelSize;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -840,6 +960,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3Shared
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_RESET_SIMULATION);
if (command->m_type == CMD_RESET_SIMULATION)
{
command->m_updateFlags = flags;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient, 0, controlMode);
@@ -1340,6 +1473,8 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHand
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = -1;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = -1;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = -1;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
@@ -1347,7 +1482,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHand
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns)
B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns, int replaceHeightfieldIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -1370,6 +1505,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle ph
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = replaceHeightfieldIndex;
cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float));
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
@@ -3035,6 +3171,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHand
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING;
return 0;
@@ -3045,6 +3182,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
@@ -3131,6 +3269,30 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHa
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3])
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR;
command->m_userConstraintArguments.m_parentBodyIndex = softBodyUniqueId;
command->m_userConstraintArguments.m_parentJointIndex = nodeIndex;
command->m_userConstraintArguments.m_childBodyIndex = bodyUniqueId;
command->m_userConstraintArguments.m_childJointIndex = linkIndex;
command->m_userConstraintArguments.m_childFrame[0] = bodyFramePosition[0];
command->m_userConstraintArguments.m_childFrame[1] = bodyFramePosition[1];
command->m_userConstraintArguments.m_childFrame[2] = bodyFramePosition[2];
command->m_userConstraintArguments.m_childFrame[3] = 0.;
command->m_userConstraintArguments.m_childFrame[4] = 0.;
command->m_userConstraintArguments.m_childFrame[5] = 0.;
command->m_userConstraintArguments.m_childFrame[6] = 1.;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -5319,18 +5481,16 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsCl
b3Assert(command);
int len = name ? strlen(name) : 0;
command->m_type = CMD_PROFILE_TIMING;
if (len > 0 && len < (MAX_FILENAME_LENGTH + 1))
{
command->m_type = CMD_PROFILE_TIMING;
strcpy(command->m_profile.m_name, name);
command->m_profile.m_name[len] = 0;
}
else
{
const char* invalid = "InvalidProfileTimingName";
int len = strlen(invalid);
strcpy(command->m_profile.m_name, invalid);
command->m_profile.m_name[len] = 0;
command->m_profile.m_name[0] = 0;
}
command->m_profile.m_type = -1;
command->m_profile.m_durationInMicroSeconds = 0;

21
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -339,6 +339,7 @@ extern "C"
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
@@ -356,7 +357,8 @@ extern "C"
B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics);
B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params);
@@ -372,7 +374,7 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
@@ -490,7 +492,7 @@ extern "C"
B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling);
B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns);
B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns, int replaceHeightfieldIndex);
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]);
@@ -631,6 +633,19 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename);
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping);
B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision);
B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);

42
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1458,7 +1458,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
if (bf.ok())
{
@@ -1529,19 +1529,45 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{
B3_PROFILE("CMD_LOADING_COMPLETED");
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
int* ids = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
int* constraintUids = ids + numBodies;
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = constraintUids[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
}
else
{
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
}
if (numBodies > 0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
for (int i = 0; i < numBodies; i++)
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
int* bodyIds = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numBodies; i++)
{
m_data->m_bodyIdsRequestInfo.push_back(bodyIds[i]);
}
}
else
{
for (int i = 0; i < numBodies; i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1];

58
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -688,7 +688,10 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
{
bf.setFileDNAisMemoryDNA();
}
bf.parse(false);
{
BT_PROFILE("bf.parse");
bf.parse(false);
}
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
@@ -718,7 +721,8 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
bodyJoints->m_baseName = mb->m_baseName;
}
addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
}
}
}
if (bf.ok())
{
@@ -919,17 +923,57 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
clearCachedBodies();
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
btAlignedObjectArray<int> bodyIdArray;
btAlignedObjectArray<int> constraintIdArray;
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
bodyIdArray.reserve(numBodies);
constraintIdArray.reserve(numConstraints);
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
clearCachedBodies();
const int* bodyIds = (int*)m_data->m_bulletStreamDataServerToClient;
const int* constaintIds = bodyIds + numBodies;
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = constaintIds[i];
constraintIdArray.push_back(constraintUid);
}
for (int i = 0; i < numBodies; i++)
{
int bodyUid = bodyIds[i];
bodyIdArray.push_back(bodyUid);
}
}
else
{
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
constraintIdArray.push_back(constraintUid);
}
for (int i = 0; i < numBodies; i++)
{
int bodyUid = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
bodyIdArray.push_back(bodyUid);
}
}
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
int constraintUid = constraintIdArray[i];
m_data->m_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT;
m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
@@ -953,10 +997,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i < numBodies; i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
int bodyUniqueId = bodyIdArray[i];
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
@@ -1174,7 +1218,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
break;
}
case CMD_SYNC_USER_DATA_FAILED:

1966
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

File diff suppressed because it is too large Load Diff

7
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -17,9 +17,12 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface
{
struct PhysicsServerCommandProcessorInternalData* m_data;
void resetSimulation();
void resetSimulation(int flags=0);
void createThreadPool();
class btDeformableMultiBodyDynamicsWorld* getDeformableWorld();
class btSoftMultiBodyDynamicsWorld* getSoftWorld();
protected:
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -114,7 +117,7 @@ public:
void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld();
virtual void createEmptyDynamicsWorld(int flags=0);
virtual void deleteDynamicsWorld();
virtual bool connect()

48
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -127,6 +127,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperChangeTexture,
eGUIHelperRemoveTexture,
eGUIHelperSetVisualizerFlagCheckRenderedFrame,
eGUIHelperUpdateShape,
};
#include <stdio.h>
@@ -545,7 +546,7 @@ MultithreadedDebugDrawer : public btIDebugDraw
btHashMap<ColorWidth, int> m_hashedLines;
public:
void drawDebugDrawerLines()
virtual void drawDebugDrawerLines()
{
if (m_hashedLines.size())
{
@@ -627,7 +628,7 @@ public:
return m_debugMode;
}
virtual void clearLines()
virtual void clearLines() override
{
m_hashedLines.clear();
m_sortedIndices.clear();
@@ -649,13 +650,26 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
public:
MultithreadedDebugDrawer* m_debugDraw;
void drawDebugDrawerLines()
virtual void drawDebugDrawerLines()
{
if (m_debugDraw)
{
m_csGUI->lock();
//draw stuff and flush?
m_debugDraw->drawDebugDrawerLines();
m_csGUI->unlock();
}
}
virtual void clearLines()
{
m_csGUI->lock();
if (m_debugDraw)
{
m_debugDraw->clearLines();
}
m_csGUI->unlock();
}
GUIHelperInterface* m_childGuiHelper;
btHashMap<btHashPtr, int> m_cachedTextureIds;
@@ -846,10 +860,8 @@ public:
delete m_debugDraw;
m_debugDraw = 0;
}
m_debugDraw = new MultithreadedDebugDrawer(this);
rbWorld->setDebugDrawer(m_debugDraw);
m_debugDraw = new MultithreadedDebugDrawer(this);
rbWorld->setDebugDrawer(m_debugDraw);
//m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
}
@@ -865,6 +877,18 @@ public:
workerThreadWait();
}
int m_updateShapeIndex;
float* m_updateShapeVertices;
virtual void updateShape(int shapeIndex, float* vertices)
{
m_updateShapeIndex = shapeIndex;
m_updateShapeVertices = vertices;
m_cs->lock();
m_cs->setSharedParam(1, eGUIHelperUpdateShape);
workerThreadWait();
}
virtual int registerTexture(const unsigned char* texels, int width, int height)
{
int* cachedTexture = m_cachedTextureIds[texels];
@@ -1916,6 +1940,15 @@ void PhysicsServerExample::updateGraphics()
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperUpdateShape:
{
B3_PROFILE("eGUIHelperUpdateShape");
m_multiThreadedHelper->m_childGuiHelper->updateShape(m_multiThreadedHelper->m_updateShapeIndex, m_multiThreadedHelper->m_updateShapeVertices);
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperRegisterGraphicsShape:
{
B3_PROFILE("eGUIHelperRegisterGraphicsShape");
@@ -2039,6 +2072,7 @@ void PhysicsServerExample::updateGraphics()
break;
}
case eGUIHelperSetVisualizerFlagCheckRenderedFrame:
{
if (m_renderedFrames != m_multiThreadedHelper->m_renderedFrames)

54
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -483,17 +483,29 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
SIM_PARAM_UPDATE_SPARSE_SDF = 1 << 29,
};
enum EnumLoadSoftBodyUpdateFlags
{
LOAD_SOFT_BODY_FILE_NAME = 1,
LOAD_SOFT_BODY_UPDATE_SCALE = 2,
LOAD_SOFT_BODY_UPDATE_MASS = 4,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8,
LOAD_SOFT_BODY_INITIAL_POSITION = 16,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 32
LOAD_SOFT_BODY_UPDATE_SCALE = 1<<1,
LOAD_SOFT_BODY_UPDATE_MASS = 1<<2,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3,
LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13,
LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
LOAD_SOFT_BODY_SIM_MESH = 1<<15,
};
enum EnumSimParamInternalSimFlags
@@ -511,7 +523,21 @@ struct LoadSoftBodyArgs
double m_mass;
double m_collisionMargin;
double m_initialPosition[3];
double m_initialOrientation[4];
double m_initialOrientation[4];
double m_springElasticStiffness;
double m_springDampingStiffness;
double m_springBendingStiffness;
double m_corotatedMu;
double m_corotatedLambda;
int m_useBendingSprings;
double m_collisionHardness;
double m_useSelfCollision;
double m_frictionCoeff;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
double m_NeoHookeanDamping;
int m_useFaceContact;
char m_simFileName[MAX_FILENAME_LENGTH];
};
struct b3LoadSoftBodyResultArgs
@@ -763,21 +789,6 @@ struct CalculateInverseKinematicsResultArgs
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
};
enum EnumUserConstraintFlags
{
USER_CONSTRAINT_ADD_CONSTRAINT = 1,
USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
USER_CONSTRAINT_REQUEST_INFO = 64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
USER_CONSTRAINT_CHANGE_ERP = 1024,
USER_CONSTRAINT_REQUEST_STATE = 2048,
};
enum EnumBodyChangeFlags
{
@@ -965,6 +976,7 @@ struct b3CreateUserShapeData
int m_numHeightfieldColumns;
double m_rgbaColor[4];
double m_specularColor[3];
int m_replaceHeightfieldIndex;
};
#define MAX_COMPOUND_COLLISION_SHAPES 16

39
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,7 +7,10 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201908110
#define SHARED_MEMORY_MAGIC_NUMBER 201911280
//#define SHARED_MEMORY_MAGIC_NUMBER 201911180
//#define SHARED_MEMORY_MAGIC_NUMBER 201909030
//#define SHARED_MEMORY_MAGIC_NUMBER 201908110
//#define SHARED_MEMORY_MAGIC_NUMBER 201908050
//#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
//#define SHARED_MEMORY_MAGIC_NUMBER 201904030
@@ -306,6 +309,23 @@ struct b3UserDataValue
const char* m_data1;
};
enum EnumUserConstraintFlags
{
USER_CONSTRAINT_ADD_CONSTRAINT = 1,
USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
USER_CONSTRAINT_REQUEST_INFO = 64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
USER_CONSTRAINT_CHANGE_ERP = 1024,
USER_CONSTRAINT_REQUEST_STATE = 2048,
USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
};
struct b3UserConstraint
{
int m_parentBodyIndex;
@@ -340,6 +360,13 @@ enum DynamicsActivationState
eActivationStateDisableWakeup = 32,
};
enum b3BodyType
{
BT_RIGID_BODY = 1,
BT_MULTI_BODY = 2,
BT_SOFT_BODY = 3,
};
struct b3DynamicsInfo
{
double m_mass;
@@ -353,6 +380,7 @@ struct b3DynamicsInfo
double m_contactStiffness;
double m_contactDamping;
int m_activationState;
int m_bodyType;
double m_angularDamping;
double m_linearDamping;
double m_ccdSweptSphereRadius;
@@ -554,6 +582,13 @@ enum b3NotificationType
SOFTBODY_CHANGED = 9,
};
enum b3ResetSimulationFlags
{
RESET_USE_DEFORMABLE_WORLD=1,
RESET_USE_DISCRETE_DYNAMICS_WORLD=2,
RESET_USE_SIMPLE_BROADPHASE=4,
};
struct b3BodyNotificationArgs
{
int m_bodyUniqueId;
@@ -938,6 +973,7 @@ struct b3PhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
double m_warmStartingFactor;
double m_articulatedWarmStartingFactor;
int m_useRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
@@ -961,6 +997,7 @@ struct b3PhysicsSimulationParameters
int m_constraintSolverType;
int m_minimumSolverIslandSize;
int m_reportSolverAnalytics;
double m_sparseSdfVoxelSize;
};

83
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}
void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
b3InitResetSimulationSetFlags(command, flag);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
{
if (!isConnected())
@@ -607,7 +620,7 @@ int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, in
return -1;
}
int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo)
int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3RobotUserConstraint* jointInfo)
{
if (!isConnected())
{
@@ -616,16 +629,35 @@ int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3Joi
}
b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
if (jointInfo->m_flags & eJointChangeMaxForce)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_maxAppliedForce);
}
if (jointInfo->m_flags & eJointChangeChildFramePosition)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
b3InitChangeUserConstraintSetGearRatio(commandHandle, jointInfo->m_gearRatio);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
b3InitChangeUserConstraintSetERP(commandHandle, jointInfo->m_erp);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
b3InitChangeUserConstraintSetGearAuxLink(commandHandle, jointInfo->m_gearAuxLink);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, jointInfo->m_relativePositionTarget);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
}
if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
}
@@ -1118,7 +1150,7 @@ void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double camer
}
}
void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName)
{
if (!isConnected())
{
@@ -1127,10 +1159,16 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
}
b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
if (durationInMicroSeconds >= 0)
if (profileName.length())
{
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
b3SetProfileTimingType(commandHandle, 0);
}
else
{
b3SetProfileTimingType(commandHandle, 1);
}
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
@@ -1151,6 +1189,35 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
b3LoadSoftBodySetScale(command, args.m_scale);
b3LoadSoftBodySetMass(command, args.m_mass);
b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
if (args.m_NeoHookeanMu > 0)
{
b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
}
if (args.m_springElasticStiffness > 0)
{
b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
}
b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
{
mouseEventsData->m_numMouseEvents = 0;

160
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -89,6 +89,64 @@ struct b3RobotSimulatorLoadSoftBodyArgs
}
};
struct b3RobotSimulatorLoadDeformableBodyArgs
{
btVector3 m_startPosition;
btQuaternion m_startOrientation;
double m_scale;
double m_mass;
double m_collisionMargin;
double m_springElasticStiffness;
double m_springDampingStiffness;
double m_springBendingStiffness;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
double m_NeoHookeanDamping;
bool m_useSelfCollision;
bool m_useFaceContact;
bool m_useBendingSprings;
double m_frictionCoeff;
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin),
m_springElasticStiffness(-1),
m_springDampingStiffness(-1),
m_springBendingStiffness(-1),
m_NeoHookeanMu(-1),
m_NeoHookeanDamping(-1),
m_useSelfCollision(false),
m_useFaceContact(false),
m_useBendingSprings(false),
m_frictionCoeff(0)
{
}
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
{
b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
}
b3RobotSimulatorLoadDeformableBodyArgs()
{
b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
}
b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin)
: m_startPosition(btVector3(0, 0, 0)),
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin)
{
}
};
struct b3RobotSimulatorLoadFileResults
{
btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -468,6 +526,100 @@ struct b3RobotSimulatorCreateMultiBodyArgs
}
};
struct b3RobotUserConstraint : public b3UserConstraint
{
int m_userUpdateFlags;//see EnumUserConstraintFlags
void setErp(double erp)
{
m_erp = erp;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_ERP;
}
void setMaxAppliedForce(double maxForce)
{
m_maxAppliedForce = maxForce;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE;
}
void setGearRatio(double gearRatio)
{
m_gearRatio = gearRatio;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO;
}
void setGearAuxLink(int link)
{
m_gearAuxLink = link;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
}
void setRelativePositionTarget(double target)
{
m_relativePositionTarget = target;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET;
}
void setChildPivot(double pivot[3])
{
m_childFrame[0] = pivot[0];
m_childFrame[1] = pivot[1];
m_childFrame[2] = pivot[2];
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
}
void setChildFrameOrientation(double orn[4])
{
m_childFrame[3] = orn[0];
m_childFrame[4] = orn[1];
m_childFrame[5] = orn[2];
m_childFrame[6] = orn[3];
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
}
b3RobotUserConstraint()
:m_userUpdateFlags(0)
{
m_parentBodyIndex = -1;
m_parentJointIndex = -1;
m_childBodyIndex = -1;
m_childJointIndex = -1;
//position
m_parentFrame[0] = 0;
m_parentFrame[1] = 0;
m_parentFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_parentFrame[3] = 0;
m_parentFrame[4] = 0;
m_parentFrame[5] = 0;
m_parentFrame[6] = 1;
//position
m_childFrame[0] = 0;
m_childFrame[1] = 0;
m_childFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_childFrame[3] = 0;
m_childFrame[4] = 0;
m_childFrame[5] = 0;
m_childFrame[6] = 1;
m_jointAxis[0] = 0;
m_jointAxis[1] = 0;
m_jointAxis[2] = 1;
m_jointType = eFixedType;
m_maxAppliedForce = 500;
m_userConstraintUniqueId = -1;
m_gearRatio = -1;
m_gearAuxLink = -1;
m_relativePositionTarget = 0;
m_erp = 0;
}
};
struct b3RobotJointInfo : public b3JointInfo
{
b3RobotJointInfo()
@@ -534,6 +686,8 @@ public:
void syncBodies();
void resetSimulation();
void resetSimulation(int flag);
btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
btVector3 getEulerFromQuaternion(const btQuaternion &quat);
@@ -564,7 +718,7 @@ public:
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3RobotUserConstraint*jointInfo);
void removeConstraint(int constraintId);
@@ -610,7 +764,7 @@ public:
void getVREvents(b3VREventsData *vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData *keyboardEventsData);
void submitProfileTiming(const std::string &profileName, int durationInMicroSeconds = 1);
void submitProfileTiming(const std::string &profileName);
// JFC: added these 24 methods
@@ -685,6 +839,8 @@ public:
int getConstraintUniqueId(int serialIndex);
void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();

4
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
View File

@@ -25,7 +25,7 @@ struct b3CustomCollisionFilter
int obB = ((m_objectUniqueIdB & 0xf) << 8);
int linkA = ((m_linkIndexA & 0xff) << 16);
int linkB = ((m_linkIndexB & 0xff) << 24);
int key = obA + obB + linkA + linkB;
long long int key = obA + obB + linkA + linkB;
// Thomas Wang's hash
key += ~(key << 15);
key ^= (key >> 10);
@@ -33,7 +33,7 @@ struct b3CustomCollisionFilter
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
return (int) key;
}
bool equals(const b3CustomCollisionFilter& other) const
{

10
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -216,7 +216,7 @@ struct EGLRendererVisualShapeConverterInternalData
ci.m_title = "PyBullet";
ci.m_width = m_swWidth;
ci.m_height = m_swHeight;
ci.m_renderDevice = 0;
ci.m_renderDevice = -1;
m_window->createWindow(ci);
m_window->setWindowTitle(ci.m_title);
@@ -1276,7 +1276,10 @@ void EGLRendererVisualShapeConverter::render(const float viewMat[16], const floa
render();
m_data->m_camera.disableVRCamera();
// don't disableVRCamera, see issue https://github.com/bulletphysics/bullet3/issues/2390
// todo: check out why
//
// m_data->m_camera.disableVRCamera();
//cout<<viewMat[4*0 + 0]<<" "<<viewMat[4*0+1]<<" "<<viewMat[4*0+2]<<" "<<viewMat[4*0+3] << endl;
//cout<<viewMat[4*1 + 0]<<" "<<viewMat[4*1+1]<<" "<<viewMat[4*1+2]<<" "<<viewMat[4*1+3] << endl;
@@ -1570,7 +1573,7 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
void EGLRendererVisualShapeConverter::resetAll()
{
m_data->m_cachedTextureIds.clear();
for (int i = 0; i < m_data->m_swRenderInstances.size(); i++)
{
EGLRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -1597,6 +1600,7 @@ void EGLRendererVisualShapeConverter::resetAll()
m_data->m_visualShapes.clear();
m_data->m_graphicsIndexToSegmentationMask.clear();
m_data->m_instancingRenderer->removeAllInstances();
m_data->m_cachedVisualShapes.clear();
}
void EGLRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)

4
examples/SharedMemory/premake4.lua
View File

@@ -72,8 +72,6 @@ myfiles =
"plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
"plugins/pdControlPlugin/pdControlPlugin.cpp",
"plugins/pdControlPlugin/pdControlPlugin.h",
"../OpenGLWindow/SimpleCamera.cpp",
"../OpenGLWindow/SimpleCamera.h",
"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
@@ -115,6 +113,8 @@ myfiles =
files {
myfiles,
"../OpenGLWindow/SimpleCamera.cpp",
"../OpenGLWindow/SimpleCamera.h",
"main.cpp",
}

2
examples/StandaloneMain/main_opengl_single_example.cpp
View File

@@ -94,7 +94,7 @@ int main(int argc, char* argv[])
app->m_instancingRenderer->updateCamera(app->getUpAxis());
btScalar dtSec = btScalar(clock.getTimeInSeconds());
if (dtSec < 0.1)
if (dtSec > 0.1)
dtSec = 0.1;
example->stepSimulation(dtSec);

16
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot()
m_tree->Compute();
}
void Jacobian::CalcDeltaThetas()
void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat)
{
switch (CurrentUpdateMode)
{
@@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas()
CalcDeltaThetasPseudoinverse();
break;
case JACOB_DLS:
CalcDeltaThetasDLS();
CalcDeltaThetasDLS(AugMat);
break;
case JACOB_SDLS:
CalcDeltaThetasSDLS();
@@ -327,7 +327,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
}
}
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
@@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve(dS, &dT1);
U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta);
// Compute JInv in damped least square form
@@ -379,7 +379,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
}
}
void Jacobian::CalcDeltaThetasDLS()
void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
@@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS()
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve(dS, &dT1);
U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta);
// Scale back to not exceed maximum angle changes
@@ -404,7 +404,7 @@ void Jacobian::CalcDeltaThetasDLS()
}
}
void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
@@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1);
U.Solve(dT1, &dTheta);
U.Solve(dT1, &dTheta, AugMat);
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();

8
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -66,15 +66,15 @@ public:
void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void CalcDeltaThetas(MatrixRmn& AugMat); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas();
void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLS(MatrixRmn& AugMat);
void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat);
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV);
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat);
void UpdateThetas();
void UpdateThetaDot();

6
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
View File

@@ -30,7 +30,6 @@ subject to the following restrictions:
#include "MatrixRmn.h"
MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix
// Fill the diagonal entries with the value d. The rest of the matrix is unchanged.
void MatrixRmn::SetDiagonalEntries(double d)
@@ -354,12 +353,13 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B,
// Solves the equation (*this)*xVec = b;
// Uses row operations. Assumes *this is square and invertible.
// No error checking for divide by zero or instability (except with asserts)
void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec) const
void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const
{
assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
// Copy this matrix and b into an Augmented Matrix
MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1);
AugMat.SetSize(NumRows, NumCols + 1);
AugMat.LoadAsSubmatrix(*this);
AugMat.SetColumn(NumRows, b);

9
examples/ThirdPartyLibs/BussIK/MatrixRmn.h
View File

@@ -117,7 +117,7 @@ public:
MatrixRmn& AddToDiagonal(const VectorRn& dVec);
// Solving systems of linear equations
void Solve(const VectorRn& b, VectorRn* x) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.
void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.
// Row Echelon Form and Reduced Row Echelon Form routines
// Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables.
@@ -150,13 +150,6 @@ private:
double* x; // Array of vector entries - stored in column order
long AllocSize; // Allocated size of the x array
static MatrixRmn WorkMatrix; // Temporary work matrix
static MatrixRmn& GetWorkMatrix() { return WorkMatrix; }
static MatrixRmn& GetWorkMatrix(long numRows, long numCols)
{
WorkMatrix.SetSize(numRows, numCols);
return WorkMatrix;
}
// Internal helper routines for SVD calculations
static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);

143
examples/ThirdPartyLibs/Wavefront/README.md
View File

@@ -38,51 +38,102 @@ Licensed under 2 clause BSD.
Usage
-----
Data format
attrib_t contains single and linear array of vertex data(position, normal and texcoord).
std::string inputfile = "cornell_box.obj";
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, inputfile.c_str());
if (!err.empty()) {
std::cerr << err << std::endl;
exit(1);
}
std::cout << "# of shapes : " << shapes.size() << std::endl;
for (size_t i = 0; i < shapes.size(); i++) {
printf("shape[%ld].name = %s\n", i, shapes[i].name.c_str());
printf("shape[%ld].indices: %ld\n", i, shapes[i].mesh.indices.size());
assert((shapes[i].mesh.indices.size() % 3) == 0);
for (size_t f = 0; f < shapes[i].mesh.indices.size(); f++) {
printf(" idx[%ld] = %d\n", f, shapes[i].mesh.indices[f]);
}
printf("shape[%ld].vertices: %ld\n", i, shapes[i].mesh.positions.size());
assert((shapes[i].mesh.positions.size() % 3) == 0);
for (size_t v = 0; v < shapes[i].mesh.positions.size() / 3; v++) {
printf(" v[%ld] = (%f, %f, %f)\n", v,
shapes[i].mesh.positions[3*v+0],
shapes[i].mesh.positions[3*v+1],
shapes[i].mesh.positions[3*v+2]);
}
printf("shape[%ld].material.name = %s\n", i, shapes[i].material.name.c_str());
printf(" material.Ka = (%f, %f ,%f)\n", shapes[i].material.ambient[0], shapes[i].material.ambient[1], shapes[i].material.ambient[2]);
printf(" material.Kd = (%f, %f ,%f)\n", shapes[i].material.diffuse[0], shapes[i].material.diffuse[1], shapes[i].material.diffuse[2]);
printf(" material.Ks = (%f, %f ,%f)\n", shapes[i].material.specular[0], shapes[i].material.specular[1], shapes[i].material.specular[2]);
printf(" material.Tr = (%f, %f ,%f)\n", shapes[i].material.transmittance[0], shapes[i].material.transmittance[1], shapes[i].material.transmittance[2]);
printf(" material.Ke = (%f, %f ,%f)\n", shapes[i].material.emission[0], shapes[i].material.emission[1], shapes[i].material.emission[2]);
printf(" material.Ns = %f\n", shapes[i].material.shininess);
printf(" material.map_Ka = %s\n", shapes[i].material.ambient_texname.c_str());
printf(" material.map_Kd = %s\n", shapes[i].material.diffuse_texname.c_str());
printf(" material.map_Ks = %s\n", shapes[i].material.specular_texname.c_str());
printf(" material.map_Ns = %s\n", shapes[i].material.normal_texname.c_str());
std::map<std::string, std::string>::iterator it(shapes[i].material.unknown_parameter.begin());
std::map<std::string, std::string>::iterator itEnd(shapes[i].material.unknown_parameter.end());
for (; it != itEnd; it++) {
printf(" material.%s = %s\n", it->first.c_str(), it->second.c_str());
}
printf("\n");
attrib_t::vertices => 3 floats per vertex
v[0] v[1] v[2] v[3] v[n-1]
+-----------+-----------+-----------+-----------+ +-----------+
| x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+-----------+-----------+-----------+-----------+ +-----------+
attrib_t::normals => 3 floats per vertex
n[0] n[1] n[2] n[3] n[n-1]
+-----------+-----------+-----------+-----------+ +-----------+
| x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+-----------+-----------+-----------+-----------+ +-----------+
attrib_t::texcoords => 2 floats per vertex
t[0] t[1] t[2] t[3] t[n-1]
+-----------+-----------+-----------+-----------+ +-----------+
| u | v | u | v | u | v | u | v | .... | u | v |
+-----------+-----------+-----------+-----------+ +-----------+
attrib_t::colors => 3 floats per vertex(vertex color. optional)
c[0] c[1] c[2] c[3] c[n-1]
+-----------+-----------+-----------+-----------+ +-----------+
| x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+-----------+-----------+-----------+-----------+ +-----------+
Each shape_t::mesh_t does not contain vertex data but contains array index to attrib_t. See loader_example.cc for more details.
mesh_t::indices => array of vertex indices.
+----+----+----+----+----+----+----+----+----+----+ +--------+
| i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-1) |
+----+----+----+----+----+----+----+----+----+----+ +--------+
Each index has an array index to attrib_t::vertices, attrib_t::normals and attrib_t::texcoords.
mesh_t::num_face_vertices => array of the number of vertices per face(e.g. 3 = triangle, 4 = quad , 5 or more = N-gons).
+---+---+---+ +---+
| 3 | 4 | 3 | ...... | 3 |
+---+---+---+ +---+
| | | |
| | | +-----------------------------------------+
| | | |
| | +------------------------------+ |
| | | |
| +------------------+ | |
| | | |
|/ |/ |/ |/
mesh_t::indices
| face[0] | face[1] | face[2] | | face[n-1] |
+----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+
| i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-3) | i(n-2) | i(n-1) |
+----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+
```c++
#define TINYOBJLOADER_IMPLEMENTATION // define this in only *one* .cc
#include "tiny_obj_loader.h"
std::string inputfile = "cornell_box.obj";
tinyobj::attrib_t attrib;
std::vector<tinyobj::shape_t> shapes;
LoadObj(attrib, shapes, inputfile.c_str());
// Loop over shapes
for (size_t s = 0; s < shapes.size(); s++) {
// Loop over faces(polygon)
size_t index_offset = 0;
for (size_t f = 0; f < shapes[s].mesh.indices.size(); f++) {
int fv = 3;
// Loop over vertices in the face.
for (size_t v = 0; v < fv; v++) {
// access to vertex
tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v];
tinyobj::real_t vx = attrib.vertices[3*idx.vertex_index+0];
tinyobj::real_t vy = attrib.vertices[3*idx.vertex_index+1];
tinyobj::real_t vz = attrib.vertices[3*idx.vertex_index+2];
tinyobj::real_t nx = attrib.normals[3*idx.normal_index+0];
tinyobj::real_t ny = attrib.normals[3*idx.normal_index+1];
tinyobj::real_t nz = attrib.normals[3*idx.normal_index+2];
tinyobj::real_t tx = attrib.texcoords[2*idx.texcoord_index+0];
tinyobj::real_t ty = attrib.texcoords[2*idx.texcoord_index+1];
}
index_offset += fv;
}
}
```

453
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -70,25 +70,6 @@ std::istream& safeGetline(std::istream& is, std::string& t)
}
}
#endif
struct vertex_index
{
int v_idx, vt_idx, vn_idx, dummy;
};
struct MyIndices
{
int m_offset;
int m_numIndices;
};
// for std::map
static inline bool operator<(const vertex_index& a, const vertex_index& b)
{
if (a.v_idx != b.v_idx) return (a.v_idx < b.v_idx);
if (a.vn_idx != b.vn_idx) return (a.vn_idx < b.vn_idx);
if (a.vt_idx != b.vt_idx) return (a.vt_idx < b.vt_idx);
return false;
}
static inline bool isSpace(const char c)
{
@@ -101,25 +82,33 @@ static inline bool isNewLine(const char c)
}
// Make index zero-base, and also support relative index.
static inline int fixIndex(int idx, int n)
static inline bool fixIndex(int idx, int n, int* ret)
{
int i;
if (!ret)
{
return false;
}
if (idx > 0)
{
i = idx - 1;
(*ret) = idx - 1;
return true;
}
else if (idx == 0)
{
i = 0;
}
else
{ // negative value = relative
i = n + idx;
}
return i;
}
if (idx == 0)
{
// zero is not allowed according to the spec.
return false;
}
if (idx < 0)
{
(*ret) = n + idx; // negative value = relative
return true;
}
return false; // never reach here.
}
static inline std::string parseString(const char*& token)
{
std::string s;
@@ -156,170 +145,184 @@ static inline void parseFloat3(
z = parseFloat(token);
}
// Parse triples: i, i/j/k, i//k, i/j
static vertex_index parseTriple(
const char*& token,
int vsize,
int vnsize,
int vtsize)
// Parse triples with index offsets: i, i/j/k, i//k, i/j
static bool parseTriple(const char** token, int vsize, int vnsize, int vtsize,
vertex_index_t* ret)
{
vertex_index vi;
vi.vn_idx = -1;
vi.vt_idx = -1;
vi.v_idx = -1;
vi.v_idx = fixIndex(atoi(token), vsize);
token += strcspn(token, "/ \t\r");
if (token[0] != '/')
{
return vi;
}
token++;
// i//k
if (token[0] == '/')
{
token++;
vi.vn_idx = fixIndex(atoi(token), vnsize);
token += strcspn(token, "/ \t\r");
return vi;
}
// i/j/k or i/j
vi.vt_idx = fixIndex(atoi(token), vtsize);
token += strcspn(token, "/ \t\r");
if (token[0] != '/')
{
return vi;
}
// i/j/k
token++; // skip '/'
vi.vn_idx = fixIndex(atoi(token), vnsize);
token += strcspn(token, "/ \t\r");
return vi;
}
static unsigned int
updateVertex(
std::map<vertex_index, unsigned int>& vertexCache,
std::vector<float>& positions,
std::vector<float>& normals,
std::vector<float>& texcoords,
const std::vector<float>& in_positions,
const std::vector<float>& in_normals,
const std::vector<float>& in_texcoords,
const vertex_index& i)
{
const std::map<vertex_index, unsigned int>::iterator it = vertexCache.find(i);
if (it != vertexCache.end())
{
// found cache
return it->second;
}
assert(static_cast<int>(in_positions.size()) > (3 * i.v_idx + 2));
positions.push_back(in_positions[3 * i.v_idx + 0]);
positions.push_back(in_positions[3 * i.v_idx + 1]);
positions.push_back(in_positions[3 * i.v_idx + 2]);
if (i.vn_idx >= 0 && ((3 * i.vn_idx + 2) < in_normals.size()))
{
normals.push_back(in_normals[3 * i.vn_idx + 0]);
normals.push_back(in_normals[3 * i.vn_idx + 1]);
normals.push_back(in_normals[3 * i.vn_idx + 2]);
}
if (i.vt_idx >= 0)
{
int numTexCoords = in_texcoords.size();
int index0 = 2 * i.vt_idx + 0;
int index1 = 2 * i.vt_idx + 1;
if (index0 >= 0 && (index0) < numTexCoords)
{
texcoords.push_back(in_texcoords[index0]);
}
if (index1 >= 0 && (index1) < numTexCoords)
{
texcoords.push_back(in_texcoords[index1]);
}
}
unsigned int idx = positions.size() / 3 - 1;
vertexCache[i] = idx;
return idx;
}
static bool
exportFaceGroupToShape(
shape_t& shape,
const std::vector<float>& in_positions,
const std::vector<float>& in_normals,
const std::vector<float>& in_texcoords,
const std::vector<MyIndices>& faceGroup,
const material_t material,
const std::string name,
std::vector<vertex_index>& allIndices)
{
if (faceGroup.empty())
if (!ret)
{
return false;
}
vertex_index_t vi(-1);
if (!fixIndex(atoi((*token)), vsize, &(vi.v_idx)))
{
return false;
}
(*token) += strcspn((*token), "/ \t\r");
if ((*token)[0] != '/')
{
(*ret) = vi;
return true;
}
(*token)++;
// i//k
if ((*token)[0] == '/')
{
(*token)++;
if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx)))
{
return false;
}
(*token) += strcspn((*token), "/ \t\r");
(*ret) = vi;
return true;
}
// i/j/k or i/j
if (!fixIndex(atoi((*token)), vtsize, &(vi.vt_idx)))
{
return false;
}
(*token) += strcspn((*token), "/ \t\r");
if ((*token)[0] != '/')
{
(*ret) = vi;
return true;
}
// i/j/k
(*token)++; // skip '/'
if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx)))
{
return false;
}
(*token) += strcspn((*token), "/ \t\r");
(*ret) = vi;
return true;
}
static bool exportFaceGroupToShape(shape_t* shape, const std::vector<face_t>& face_group,
const material_t material, const std::string& name,
const std::vector<float>& v)
{
if (face_group.empty())
{
return false;
}
shape->name = name;
// Flattened version of vertex data
std::vector<float> positions;
std::vector<float> normals;
std::vector<float> texcoords;
std::map<vertex_index, unsigned int> vertexCache;
std::vector<unsigned int> indices;
// Flatten vertices and indices
for (size_t i = 0; i < faceGroup.size(); i++)
for (size_t i = 0; i < face_group.size(); i++)
{
const MyIndices& face = faceGroup[i];
vertex_index i0 = allIndices[face.m_offset];
vertex_index i1;
i1.vn_idx = -1;
i1.vt_idx = -1;
i1.v_idx = -1;
vertex_index i2 = allIndices[face.m_offset + 1];
size_t npolys = face.m_numIndices; //.size();
const face_t& face = face_group[i];
size_t npolys = face.size();
if (npolys < 3)
{
// Polygon -> triangle fan conversion
for (size_t k = 2; k < npolys; k++)
// Face must have 3+ vertices.
continue;
}
vertex_index_t i0 = face[0];
vertex_index_t i1(-1);
vertex_index_t i2 = face[1];
face_t remainingFace = face; // copy
size_t guess_vert = 0;
vertex_index_t ind[3];
// How many iterations can we do without decreasing the remaining
// vertices.
size_t remainingIterations = face.size();
size_t previousRemainingVertices = remainingFace.size();
while (remainingFace.size() > 3 && remainingIterations > 0)
{
npolys = remainingFace.size();
if (guess_vert >= npolys)
{
i1 = i2;
i2 = allIndices[face.m_offset + k];
guess_vert -= npolys;
}
unsigned int v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0);
unsigned int v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1);
unsigned int v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2);
if (previousRemainingVertices != npolys)
{
// The number of remaining vertices decreased. Reset counters.
previousRemainingVertices = npolys;
remainingIterations = npolys;
}
else
{
// We didn't consume a vertex on previous iteration, reduce the
// available iterations.
remainingIterations--;
}
indices.push_back(v0);
indices.push_back(v1);
indices.push_back(v2);
for (size_t k = 0; k < 3; k++)
{
ind[k] = remainingFace[(guess_vert + k) % npolys];
size_t vi = size_t(ind[k].v_idx);
}
// this triangle is an ear
{
index_t idx0, idx1, idx2;
idx0.vertex_index = ind[0].v_idx;
idx0.normal_index = ind[0].vn_idx;
idx0.texcoord_index = ind[0].vt_idx;
idx1.vertex_index = ind[1].v_idx;
idx1.normal_index = ind[1].vn_idx;
idx1.texcoord_index = ind[1].vt_idx;
idx2.vertex_index = ind[2].v_idx;
idx2.normal_index = ind[2].vn_idx;
idx2.texcoord_index = ind[2].vt_idx;
shape->mesh.indices.push_back(idx0);
shape->mesh.indices.push_back(idx1);
shape->mesh.indices.push_back(idx2);
}
// remove v1 from the list
size_t removed_vert_index = (guess_vert + 1) % npolys;
while (removed_vert_index + 1 < npolys)
{
remainingFace[removed_vert_index] =
remainingFace[removed_vert_index + 1];
removed_vert_index += 1;
}
remainingFace.pop_back();
}
if (remainingFace.size() == 3)
{
i0 = remainingFace[0];
i1 = remainingFace[1];
i2 = remainingFace[2];
{
index_t idx0, idx1, idx2;
idx0.vertex_index = i0.v_idx;
idx0.normal_index = i0.vn_idx;
idx0.texcoord_index = i0.vt_idx;
idx1.vertex_index = i1.v_idx;
idx1.normal_index = i1.vn_idx;
idx1.texcoord_index = i1.vt_idx;
idx2.vertex_index = i2.v_idx;
idx2.normal_index = i2.vn_idx;
idx2.texcoord_index = i2.vt_idx;
shape->mesh.indices.push_back(idx0);
shape->mesh.indices.push_back(idx1);
shape->mesh.indices.push_back(idx2);
}
}
}
//
// Construct shape.
//
shape.name = name;
shape.mesh.positions.swap(positions);
shape.mesh.normals.swap(normals);
shape.mesh.texcoords.swap(texcoords);
shape.mesh.indices.swap(indices);
shape.material = material;
shape->material = material;
return true;
}
@@ -329,7 +332,6 @@ void InitMaterial(material_t& material)
material.ambient_texname = "";
material.diffuse_texname = "";
material.specular_texname = "";
material.normal_texname = "";
for (int i = 0; i < 3; i++)
{
material.ambient[i] = 0.f;
@@ -369,8 +371,8 @@ std::string LoadMtl(
return err.str();
}
#else
int fileHandle = fileIO->fileOpen(filepath.c_str(),"r");
if (fileHandle<0)
int fileHandle = fileIO->fileOpen(filepath.c_str(), "r");
if (fileHandle < 0)
{
err << "Cannot open file [" << filepath << "]" << std::endl;
return err.str();
@@ -396,7 +398,7 @@ std::string LoadMtl(
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
if (line)
{
linebuf=line;
linebuf = line;
}
#endif
@@ -420,7 +422,7 @@ std::string LoadMtl(
// Skip leading space.
const char* token = linebuf.c_str();
token += strspn(token, " \t");
assert(token);
if (token[0] == '\0') continue; // empty line
@@ -574,12 +576,13 @@ std::string LoadMtl(
}
}
#ifndef USE_STREAM
while (line);
while (line)
;
#endif
// flush last material.
material_map.insert(std::pair<std::string, material_t>(material.name, material));
if (fileHandle>=0)
if (fileHandle >= 0)
{
fileIO->fileClose(fileHandle);
}
@@ -588,11 +591,16 @@ std::string LoadMtl(
std::string
LoadObj(
attrib_t& attrib,
std::vector<shape_t>& shapes,
const char* filename,
const char* mtl_basepath,
CommonFileIOInterface* fileIO)
{
attrib.vertices.clear();
attrib.normals.clear();
attrib.texcoords.clear();
shapes.clear();
std::string tmp = filename;
if (!mtl_basepath)
{
@@ -604,13 +612,6 @@ LoadObj(
mtl_basepath = tmp.c_str();
//fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename);
}
shapes.resize(0);
std::vector<vertex_index> allIndices;
allIndices.reserve(1024 * 1024);
MyIndices face;
std::stringstream err;
#ifdef USE_STREAM
std::ifstream ifs(filename);
@@ -620,8 +621,8 @@ LoadObj(
return err.str();
}
#else
int fileHandle = fileIO->fileOpen(filename,"r");
if (fileHandle<0)
int fileHandle = fileIO->fileOpen(filename, "r");
if (fileHandle < 0)
{
err << "Cannot open file [" << filename << "]" << std::endl;
return err.str();
@@ -629,16 +630,15 @@ LoadObj(
#endif
std::vector<float> v;
v.reserve(1024 * 1024);
std::vector<float> vn;
vn.reserve(1024 * 1024);
std::vector<float> vt;
vt.reserve(1024 * 1024);
//std::vector<std::vector<vertex_index> > faceGroup;
std::vector<MyIndices> faceGroup;
faceGroup.reserve(1024 * 1024);
std::string name;
int greatest_v_idx = -1;
int greatest_vn_idx = -1;
int greatest_vt_idx = -1;
std::vector<face_t> faceGroup;
// material
std::map<std::string, material_t> material_map;
material_t material;
@@ -664,10 +664,9 @@ LoadObj(
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
if (line)
{
linebuf=line;
linebuf = line;
}
#endif
// Trim newline '\r\n' or '\r'
if (linebuf.size() > 0)
{
@@ -734,23 +733,34 @@ LoadObj(
token += 2;
token += strspn(token, " \t");
face.m_offset = allIndices.size();
face.m_numIndices = 0;
face_t face;
face.reserve(3);
while (!isNewLine(token[0]))
{
vertex_index vi = parseTriple(token, v.size() / 3, vn.size() / 3, vt.size() / 2);
allIndices.push_back(vi);
face.m_numIndices++;
int n = strspn(token, " \t\r");
vertex_index_t vi;
if (!parseTriple(&token, static_cast<int>(v.size() / 3),
static_cast<int>(vn.size() / 3),
static_cast<int>(vt.size() / 2), &vi))
{
err << "Failed parse `f' line(e.g. zero value for face index.";
return err.str();
}
greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx;
greatest_vn_idx =
greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx;
greatest_vt_idx =
greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx;
face.push_back(vi);
size_t n = strspn(token, " \t\r");
token += n;
}
faceGroup.push_back(face);
continue;
}
// use mtl
if ((0 == strncmp(token, "usemtl", 6)) && isSpace((token[6])))
{
@@ -777,10 +787,10 @@ LoadObj(
token += 7;
sscanf(token, "%s", namebuf);
std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO);
std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath, fileIO);
if (!err_mtl.empty())
{
//faceGroup.resize(0); // for safety
//face_group.resize(0); // for safety
//return err_mtl;
}
continue;
@@ -791,7 +801,7 @@ LoadObj(
{
// flush previous face group.
shape_t shape;
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
if (ret)
{
shapes.push_back(shape);
@@ -827,7 +837,7 @@ LoadObj(
{
// flush previous face group.
shape_t shape;
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
if (ret)
{
shapes.push_back(shape);
@@ -847,18 +857,23 @@ LoadObj(
// Ignore unknown command.
}
#ifndef USE_STREAM
while (line);
while (line)
;
#endif
shape_t shape;
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
if (ret)
{
shapes.push_back(shape);
}
faceGroup.resize(0); // for safety
if (fileHandle>=0)
attrib.vertices.swap(v);
attrib.normals.swap(vn);
attrib.texcoords.swap(vt);
if (fileHandle >= 0)
{
fileIO->fileClose(fileHandle);
}

43
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
View File

@@ -14,6 +14,17 @@ struct CommonFileIOInterface;
namespace tinyobj
{
struct vertex_index_t
{
int v_idx, vt_idx, vn_idx;
vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {}
explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {}
vertex_index_t(int vidx, int vtidx, int vnidx)
: v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {}
};
typedef std::vector<vertex_index_t> face_t;
typedef struct
{
std::string name;
@@ -24,21 +35,27 @@ typedef struct
float transmittance[3];
float emission[3];
float shininess;
float transparency;
float transparency; // 1 == opaque; 0 == fully transparent
std::string ambient_texname;
std::string diffuse_texname;
std::string specular_texname;
std::string ambient_texname; // map_Ka
std::string diffuse_texname; // map_Kd
std::string specular_texname; // map_Ks
std::string normal_texname;
std::map<std::string, std::string> unknown_parameter;
} material_t;
// Index struct to support different indices for vtx/normal/texcoord.
// -1 means not used.
typedef struct
{
std::vector<float> positions;
std::vector<float> normals;
std::vector<float> texcoords;
std::vector<unsigned int> indices;
int vertex_index;
int normal_index;
int texcoord_index;
} index_t;
typedef struct
{
std::vector<index_t> indices;
} mesh_t;
typedef struct
@@ -48,6 +65,14 @@ typedef struct
mesh_t mesh;
} shape_t;
// Vertex attributes
struct attrib_t
{
std::vector<float> vertices; // 'v'(xyz)
std::vector<float> normals; // 'vn'
std::vector<float> texcoords; // 'vt'(uv)
attrib_t() {}
};
/// Loads .obj from a file.
/// 'shapes' will be filled with parsed shape data
/// The function returns error string.
@@ -55,12 +80,14 @@ typedef struct
/// 'mtl_basepath' is optional, and used for base path for .mtl file.
#ifdef USE_STREAM
std::string LoadObj(
attrib_t& attrib,
std::vector<shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath = NULL);
#else
std::string
LoadObj(
attrib_t& attrib,
std::vector<shape_t>& shapes,
const char* filename,
const char* mtl_basepath,

4
examples/ThirdPartyLibs/glad/glad/gl.h
View File

@@ -2007,12 +2007,16 @@ typedef khronos_ssize_t GLsizeiptr;
#if defined(__ENVIRONMENT_MAC_OS_X_VERSION_MIN_REQUIRED__) && (__ENVIRONMENT_MAC_OS_X_VERSION_MIN_REQUIRED__ > 1060)
typedef long GLintptrARB;
#else
#ifndef __gltypes_h_
typedef ptrdiff_t GLintptrARB;
#endif
#endif
#if defined(__ENVIRONMENT_MAC_OS_X_VERSION_MIN_REQUIRED__) && (__ENVIRONMENT_MAC_OS_X_VERSION_MIN_REQUIRED__ > 1060)
typedef long GLsizeiptrARB;
#else
#ifndef __gltypes_h_
typedef ptrdiff_t GLsizeiptrARB;
#endif
#endif
typedef int64_t GLint64EXT;
typedef uint64_t GLuint64EXT;

15
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -156,8 +156,11 @@ struct Shader : public IShader
float index_x = b3Max(float(0.0), b3Min(float(m_width - 1), p[0]));
float index_y = b3Max(float(0.0), b3Min(float(m_height - 1), p[1]));
int idx = int(index_x) + int(index_y) * m_width; // index in the shadowbuffer array
float shadow = 0.8 + 0.2 * (m_shadowBuffer->at(idx) < -depth + 0.05); // magic coeff to avoid z-fighting
float shadow = 1.0;
if (m_shadowBuffer && idx >=0 && idx <m_shadowBuffer->size())
{
shadow = 0.8 + 0.2 * (m_shadowBuffer->at(idx) < -depth + 0.05); // magic coeff to avoid z-fighting
}
Vec3f bn = (varying_nrm * bar).normalize();
Vec2f uv = varying_uv * bar;
@@ -174,7 +177,13 @@ struct Shader : public IShader
for (int i = 0; i < 3; ++i)
{
color[i] = b3Min(int(m_ambient_coefficient * color[i] + shadow * (m_diffuse_coefficient * diffuse + m_specular_coefficient * specular) * color[i] * m_light_color[i]), 255);
int orgColor = 0;
float floatColor = (m_ambient_coefficient * color[i] + shadow * (m_diffuse_coefficient * diffuse + m_specular_coefficient * specular) * color[i] * m_light_color[i]);
if (floatColor==floatColor)
{
orgColor=int(floatColor);
}
color[i] = b3Min(orgColor, 255);
}
return false;

7
examples/TinyRenderer/model.cpp
View File

@@ -126,10 +126,13 @@ int Model::nfaces()
std::vector<int> Model::face(int idx)
{
std::vector<int> face;
for (int i = 0; i < (int)faces_[idx].size(); i++) face.push_back(faces_[idx][i][0]);
return face;
face.reserve((int)faces_[idx].size());
for (int i = 0; i < (int)faces_[idx].size(); i++)
face.push_back(faces_[idx][i][0]);
return face;
}
Vec3f Model::vert(int i)
{
return verts_[i];

2
examples/pybullet/examples/changeTexture.py
View File

@@ -34,7 +34,7 @@ for i in range(100000):
start = time.time()
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL)
end = time.time()
print("rendering duraction")
print("rendering duration")
print(end - start)
p.stopStateLogging(logId)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)

4
examples/pybullet/examples/createMultiBodyBatch.py
View File

@@ -117,14 +117,14 @@ for x in range(32):
batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,
bodyUids = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.changeVisualShape(bodyUids[0], -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=", p.getNumBodies())

29
examples/pybullet/examples/deformable_anchor.py Normal file
View File

@@ -0,0 +1,29 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
gravZ=-10
p.setGravity(0, 0, gravZ)
planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
p.createSoftBodyAnchor(clothId ,0,-1,-1)
p.createSoftBodyAnchor(clothId ,1,-1,-1)
p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0])
p.createSoftBodyAnchor(clothId ,2,boxId,-1, [-0.5,-0.5,0])
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,gravZ)
sleep(1./240.)

28
examples/pybullet/examples/deformable_ball.py Normal file
View File

@@ -0,0 +1,28 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5)
p.setTimeStep(0.001)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
#logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "perf.json")
while p.isConnected():
p.setGravity(0,0,-10)
sleep(1./240.)
#p.resetSimulation()
#p.stopStateLogging(logId)

20
examples/pybullet/examples/deformable_torus.py Normal file
View File

@@ -0,0 +1,20 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,-10)
sleep(1./240.)

23
examples/pybullet/examples/heightfield.py
View File

@@ -1,6 +1,6 @@
import pybullet as p
import pybullet_data as pd
import math
import time
p.connect(p.GUI)
@@ -11,6 +11,7 @@ textureId = -1
useProgrammatic = 0
useTerrainFromPNG = 1
useDeepLocoCSV = 2
updateHeightfield = False
heightfieldSource = useProgrammatic
import random
@@ -30,7 +31,7 @@ if heightfieldSource==useProgrammatic:
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, )
terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
terrain = p.createMultiBody(0, terrainShape)
p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
@@ -114,7 +115,23 @@ for i in range(p.getNumJoints(sphereUid)):
while (p.isConnected()):
#keys = p.getKeyboardEvents()
keys = p.getKeyboardEvents()
if updateHeightfield and heightfieldSource==useProgrammatic:
for j in range (int(numHeightfieldColumns/2)):
for i in range (int(numHeightfieldRows/2) ):
height = random.uniform(0,heightPerturbationRange)#+math.sin(time.time())
heightfieldData[2*i+2*j*numHeightfieldRows]=height
heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
#GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
#GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
#flags = p.GEOM_CONCAVE_INTERNAL_EDGE
flags = 0
terrainShape2 = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, flags = flags, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, replaceHeightfieldIndex = terrainShape)
#print(keys)
#getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
#p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)

12
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -84,32 +84,32 @@ p.addUserDebugText("Kinematic",
p.addUserDebugText("Stable PD (Py)",
[startLocations[4][0], startLocations[4][1] + 1, startLocations[4][2]],
[0, 0, 0])
flags=p.URDF_MAINTAIN_LINK_ORDER+p.URDF_USE_SELF_COLLISION
humanoid = p.loadURDF("humanoid/humanoid.urdf",
startLocations[0],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
flags=flags)
humanoid2 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[1],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
flags=flags)
humanoid3 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[2],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
flags=flags)
humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[3],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
flags=flags)
humanoid5 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[4],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
flags=flags)
humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
startLocations[0], [0, 0, 0, 1])

15
examples/pybullet/examples/load_soft_body.py
View File

@@ -1,19 +1,24 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.DIRECT)
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
bunnyId = p.loadSoftBody("bunny.obj")
planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("bunny.obj")#.obj")#.vtk")
#meshData = p.getMeshData(bunnyId)
#print("meshData=",meshData)
p.loadURDF("cube_small.urdf", [1, 0, 1])
#p.loadURDF("cube_small.urdf", [1, 0, 1])
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
print(p.getDynamicsInfo(planeId, -1))
#print(p.getDynamicsInfo(bunnyId, 0))
print(p.getDynamicsInfo(boxId, -1))
p.changeDynamics(boxId,-1,mass=10)
while p.isConnected():
p.setGravity(0, 0, -10)
if (useRealTimeSimulation):

39
examples/pybullet/examples/sceneAabb.py Normal file
View File

@@ -0,0 +1,39 @@
import pybullet as p
import time
import numpy as np
p.connect(p.GUI)
p.loadURDF("plane.urdf")
p.loadURDF("sphere2.urdf",[0,0,2])
dt = 1./240.
p.setTimeStep(dt)
def getSceneAABB():
aabbMins=[]
aabbMaxs=[]
for i in range(p.getNumBodies()):
uid = p.getBodyUniqueId(i)
aabb = p.getAABB(uid)
aabbMins.append(np.array(aabb[0]))
aabbMaxs.append(np.array(aabb[1]))
if len(aabbMins):
sceneAABBMin = aabbMins[0]
sceneAABBMax = aabbMaxs[0]
for aabb in aabbMins:
sceneAABBMin = np.minimum(sceneAABBMin,aabb)
for aabb in aabbMaxs:
sceneAABBMax = np.maximum(sceneAABBMax,aabb)
print("sceneAABBMin=",sceneAABBMin)
print("sceneAABBMax=",sceneAABBMax)
getSceneAABB()
while (1):
p.stepSimulation()
time.sleep(dt)

28
examples/pybullet/examples/test_inertia.py Normal file
View File

@@ -0,0 +1,28 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
useDeformable = True
if useDeformable:
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
gravZ=-10
p.setGravity(0, 0, gravZ)
planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
clothId = p.loadSoftBody("bunny.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1, useMassSpring=1, springElasticStiffness=100, springDampingStiffness=.001, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
p.setTimeStep(0.0005)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,gravZ)
sleep(1./240.)

24
examples/pybullet/examples/video_sync_mp4.py Normal file
View File

@@ -0,0 +1,24 @@
import pybullet as p
import time
#by default, PyBullet runs at 240Hz
p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240")
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
p.loadURDF("plane.urdf")
#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter.
r2d2 = p.loadURDF("r2d2.urdf",[0,0,45])
#disable linear damping
p.changeDynamics(r2d2,-1, linearDamping=0)
p.setGravity(0,0,-10)
for i in range (3*240):
txt = "frame "+str(i)
item = p.addUserDebugText(txt, [0,1,0])
p.stepSimulation()
#synchronize the visualizer (rendering frames for the video mp4) with stepSimulation
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
#print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2])
p.removeUserDebugItem(item)
p.disconnect()

Some files were not shown because too many files have changed in this diff Show More