Skip to content

Commit

Permalink
Merge pull request bulletphysics#2757 from erwincoumans/master
Browse files Browse the repository at this point in the history 
fixes for tcp server, allow createCollisionShape to load stl
  • Loading branch information
erwincoumans authored Apr 22, 2020
2 parents 687780a + cbb459f commit e1f5729
Show file tree
Hide file tree
Showing 2 changed files with 99 additions and 21 deletions.
113 changes: 93 additions & 20 deletions examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4979,22 +4979,81 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
urdfColObj.m_geometry.m_meshFileType = out_type;

if (out_type == UrdfGeometry::FILE_STL)
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH)
{
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
glmesh = LoadMeshFromSTL(relativeFileName, fileIO);
}
if (out_type == UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
if (out_type == UrdfGeometry::FILE_STL)
{
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
glmesh = LoadMeshFromSTL(relativeFileName, fileIO);
}

if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH)
if (out_type == UrdfGeometry::FILE_OBJ)
{
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
glmesh = LoadMeshFromObj(relativeFileName, pathPrefix, fileIO);
}
else
if (glmesh)
{
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}

btTriangleMesh* meshInterface = new btTriangleMesh();
m_data->m_meshInterfaces.push_back(meshInterface);

for (int i = 0; i < glmesh->m_numIndices / 3; i++)
{
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3)).xyzw;
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 1)).xyzw;
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 2)).xyzw;
meshInterface->addTriangle(
btVector3(v0[0], v0[1], v0[2])* meshScale,
btVector3(v1[0], v1[1], v1[2])* meshScale,
btVector3(v2[0], v2[1], v2[2])* meshScale);
}

btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
compound->addChildShape(childTransform, trimesh);
}
}
else
{
if (out_type == UrdfGeometry::FILE_STL)
{
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
glmesh = LoadMeshFromSTL(relativeFileName, fileIO);

B3_PROFILE("createConvexHullFromShapes");
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
for (int vv = 0; vv < glmesh->m_numvertices; vv++)
{
btVector3 pt(
glmesh->m_vertices->at(vv).xyzw[0],
glmesh->m_vertices->at(vv).xyzw[1],
glmesh->m_vertices->at(vv).xyzw[2]);
convexHull->addPoint(pt * meshScale, false);
}
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_INITIALIZE_SAT_FEATURES)
{
convexHull->initializePolyhedralFeatures();
}

convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();

compound->addChildShape(childTransform, convexHull);
}
if (out_type == UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape

std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
Expand All @@ -5019,19 +5078,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
btVector3 pt;
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]);
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 * meshScale, false);

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]);
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 * meshScale, false);

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]);
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 * meshScale, false);
}

Expand All @@ -5041,7 +5100,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();

compound->addChildShape(childTransform, convexHull);
}
}
Expand Down Expand Up @@ -5205,6 +5264,7 @@ static void gatherVertices(const btTransform& trans, const btCollisionShape* col
}
default:
{
printf("?\n");
}
}
}
Expand All @@ -5214,6 +5274,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
BT_PROFILE("CMD_REQUEST_MESH_DATA");
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED;
serverStatusOut.m_numDataStreamBytes = 0;
int sizeInBytes = 0;

InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
if (bodyHandle)
Expand Down Expand Up @@ -5261,6 +5322,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
{
verticesOut[i] = vertices[i];
}
sizeInBytes = verticesCopied * sizeof(btVector3);
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
Expand Down Expand Up @@ -5292,7 +5354,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
verticesOut[i] = n.m_x;
}
}

sizeInBytes = verticesCopied * sizeof(btVector3);
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
Expand All @@ -5301,7 +5363,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
}

serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_numDataStreamBytes = sizeInBytes;

return hasStatus;
}
Expand Down Expand Up @@ -6117,6 +6179,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads);

serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData);
serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays;
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
return hasStatus;
Expand Down Expand Up @@ -6219,12 +6282,15 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
int usz = m_data->m_userConstraints.size();
int* constraintUid = bodyUids + actualNumBodies;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;

for (int i = 0; i < usz; i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
constraintUid[i] = key;
}

serverStatusOut.m_numDataStreamBytes = sizeof(int) * (actualNumBodies + usz);

serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
return hasStatus;
}
Expand Down Expand Up @@ -6253,14 +6319,17 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
}
}
}
int sizeInBytes = sizeof(int) * userDataHandles.size();
if (userDataHandles.size())
{
memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size());
memcpy(bufferServerToClient, &userDataHandles[0], sizeInBytes);
}
// Only clear the client-side cache when a full sync is requested
serverStatusOut.m_syncUserDataArgs.m_clearCachedUserDataEntries = clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies == 0;
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_numDataStreamBytes = sizeInBytes;
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;

return hasStatus;
}

Expand Down Expand Up @@ -6290,6 +6359,7 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S
{
memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size());
}
serverStatusOut.m_numDataStreamBytes = userData->m_bytes.size();
return hasStatus;
}

Expand Down Expand Up @@ -10272,7 +10342,7 @@ bool PhysicsServerCommandProcessor::processRequestAabbOverlapCommand(const struc
overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i];
overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i];
}

serverCmd.m_numDataStreamBytes = numOverlap * totalBytesPerObject;
serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED;

//int m_startingOverlappingObjectIndex;
Expand Down Expand Up @@ -10669,6 +10739,7 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
sharedBuf[element] = massMatrix(i, j);
}
}
serverCmd.m_numDataStreamBytes = sizeInBytes;
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
}
Expand Down Expand Up @@ -12377,6 +12448,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(cons
{
//extract shape info from base collider
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes*sizeof(b3CollisionShapeData);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
Expand All @@ -12386,6 +12458,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(cons
if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex))
{
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes * sizeof(b3CollisionShapeData);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
Expand Down
7 changes: 6 additions & 1 deletion examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -444,7 +444,12 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
break;
}
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO);

char relativeFileName[1024];
if (fileIO->findResourcePath(visual->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
glmesh = LoadMeshFromSTL(relativeFileName, fileIO);
}
break;
case UrdfGeometry::FILE_COLLADA:
{
Expand Down

0 comments on commit e1f5729

Please sign in to comment.