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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Xuchen Han
2019-11-25 15:29:25 -08:00
parent 6c34c91ca7 574343405d
commit abd7a556e1
15 changed files with 524 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

251
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -53,7 +53,7 @@
#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#include "LinearMath/TaskScheduler/btThreadSupportInterface.h"
#include "Wavefront/tiny_obj_loader.h"
#ifndef SKIP_COLLISION_FILTER_PLUGIN
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
#endif
@@ -1636,18 +1636,14 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration;
//#ifndef SKIP_DEFORMABLE_BODY
#ifndef SKIP_DEFORMABLE_BODY
btDeformableBodySolver* m_deformablebodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
//#else//SKIP_DEFORMABLE_BODY
//#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#endif
//#else//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btMultiBodyDynamicsWorld* m_dynamicsWorld;
//#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
//#endif//SKIP_DEFORMABLE_BODY
int m_constraintSolverType;
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
@@ -1713,6 +1709,9 @@ struct PhysicsServerCommandProcessorInternalData
m_dispatcher(0),
m_solver(0),
m_collisionConfiguration(0),
#ifndef SKIP_DEFORMABLE_BODY
m_deformablebodySolver(0),
#endif
m_dynamicsWorld(0),
m_deformablebodySolver(0),
m_constraintSolverType(-1),
@@ -2652,23 +2651,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
}
if (flags & RESET_USE_DEFORMABLE_WORLD)
if (flags & RESET_USE_DEFORMABLE_WORLD)
{
#ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
#endif
}
if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
{
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
}
@@ -5127,24 +5128,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
{
btSoftBody* psb = bodyHandle->m_softBody;
int totalBytesPerVertex = sizeof(btVector3);
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
btVector3* verticesOut = (btVector3*)bufferServerToClient;
for (int i = 0; i < verticesCopied; ++i)
{
if (separateRenderMesh)
{
const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
else
{
const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
if (separateRenderMesh)
{
const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
else
{
const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
}
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
@@ -8070,7 +8071,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
{
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
}
{
btSoftBody* psb = NULL;
@@ -8084,13 +8085,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type(0);
std::string out_found_filename, out_found_sim_filename;
int out_type(0), out_sim_type(0);
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (out_type == UrdfGeometry::FILE_OBJ)
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
{
bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
}
else
{
out_sim_type = out_type;
out_found_sim_filename = out_found_filename;
}
if (out_sim_type == UrdfGeometry::FILE_OBJ)
{
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
@@ -8149,7 +8159,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_filename.c_str());
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
btScalar corotated_mu, corotated_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{
@@ -8186,37 +8196,55 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
if (psb != NULL)
{
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
bool foundRenderMesh = false;
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_RENDER_MESH)
{
int out_render_type(0);
std::string out_found_render_filename;
char relativeRenderFileName[1024];
char pathRenderPrefix[1024];
pathRenderPrefix[0] = 0;
if (fileIO->findResourcePath(loadSoftBodyArgs.m_renderFileName, relativeRenderFileName, 1024))
{
b3FileUtils::extractPath(relativeRenderFileName, pathRenderPrefix, 1024);
}
foundRenderMesh = UrdfFindMeshFile(fileIO, pathRenderPrefix, relativeRenderFileName, error_message_prefix, &out_found_render_filename, &out_render_type);
// load render mesh
if (foundRenderMesh)
{
btSoftBodyHelpers::readRenderMeshFromObj(out_found_render_filename.c_str(), psb);
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
}
}
// load render mesh
if (out_found_sim_filename != out_found_filename)
{
// load render mesh
{
tinyobj::attrib_t attribute;
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
if (!foundRenderMesh)
{
psb->m_renderNodes.resize(0);
}
// This value should really be a exposed parameter
psb->setCollisionQuadrature(5);
for (int s = 0; s < (int)shapes.size(); s++)
{
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
int vertexCount = attribute.vertices.size()/3;
for (int v=0;v<vertexCount;v++)
{
btSoftBody::Node n;
n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]);
psb->m_renderNodes.push_back(n);
}
for (int f = 0; f < faceCount; f += 3)
{
if (f < 0 && f >= int(shape.mesh.indices.size()))
{
continue;
}
tinyobj::index_t v_0 = shape.mesh.indices[f];
tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
btSoftBody::Face ff;
ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index];
psb->m_renderFaces.push_back(ff);
}
}
}
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
}
else
{
psb->m_renderNodes.resize(0);
}
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
deformWorld->addForce(psb, gravityForce);
@@ -8563,10 +8591,21 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody)
{
btSoftBody* sb = body->m_softBody;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
btVector3 aabbMin, aabbMax;
sb->getAabb(aabbMin, aabbMax);
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
setDefaultRootWorldAABB(serverCmd);
}
#endif
return hasStatus;
@@ -9441,6 +9480,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF)
{
#ifndef SKIP_DEFORMABLE_BODY
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
@@ -9449,6 +9489,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
deformWorld ->getWorldInfo().m_sparsesdf.Reset();
}
}
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
@@ -9457,6 +9499,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
softWorld->getWorldInfo().m_sparsesdf.Reset();
}
}
#endif
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
@@ -10647,6 +10690,84 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR)
{
#ifndef SKIP_DEFORMABLE_BODY
InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (sbodyHandle)
{
if (sbodyHandle->m_softBody)
{
int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex;
if (nodeIndex>=0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size())
{
int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex;
if (bodyUniqueId<=0)
{
//fixed anchor (mass = 0)
sbodyHandle->m_softBody->setMass(nodeIndex,0.0);
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} else
{
InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (mbodyHandle && mbodyHandle->m_multiBody)
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex;
if (linkIndex<0)
{
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
} else
{
if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
{
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getLinkCollider(linkIndex));
}
}
}
}
if (mbodyHandle && mbodyHandle->m_rigidBody)
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
//todo: expose those values
bool disableCollisionBetweenLinkedBodies = true;
//btVector3 localPivot(0,0,0);
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_rigidBody);
}
#if 1
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
bool disableCollisionBetweenLinkedBodies = true;
btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]);
sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies);
}
#endif
}
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
}
}
#endif
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
{
int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;