From 9cf93cebfd93c2228f89ddd6d944358dfda48778 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 20 Apr 2020 20:59:32 -0700 Subject: [PATCH 1/2] allow createCollisionShape to load stl. Still, it is best to convert it to obj and use v-hacd for concave shapes. --- .../PhysicsServerCommandProcessor.cpp | 92 +++++++++++++++---- .../TinyRendererVisualShapeConverter.cpp | 7 +- 2 files changed, 82 insertions(+), 17 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e1807e3543..c52524dc83 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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 shapes; tinyobj::attrib_t attribute; std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); @@ -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); } @@ -5041,7 +5100,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str } convexHull->recalcLocalAabb(); convexHull->optimizeConvexHull(); - + compound->addChildShape(childTransform, convexHull); } } @@ -5205,6 +5264,7 @@ static void gatherVertices(const btTransform& trans, const btCollisionShape* col } default: { + printf("?\n"); } } } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 5c32386839..6d0ce3fa29 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -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: { From cbb459f5d41a321a6048d9a749eb4086e6145944 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 21 Apr 2020 12:15:44 -0700 Subject: [PATCH 2/2] fixes for TCP connection: report serverStatusOut.m_numDataStreamBytes when storing results in bufferServerToClient --- .../PhysicsServerCommandProcessor.cpp | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c52524dc83..8e5ac6721e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5274,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) @@ -5321,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; @@ -5352,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; @@ -5361,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; } @@ -6176,6 +6178,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; @@ -6278,12 +6281,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; } @@ -6312,14 +6318,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; } @@ -6349,6 +6358,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; } @@ -10329,7 +10339,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; @@ -10726,6 +10736,7 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru sharedBuf[element] = massMatrix(i, j); } } + serverCmd.m_numDataStreamBytes = sizeInBytes; serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; } } @@ -12434,6 +12445,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; } @@ -12443,6 +12455,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; }