diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 034ca3d6f..f295458a8 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -969,6 +969,53 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { switch (visual->m_geometry.m_meshFileType) { + case UrdfGeometry::MEMORY_VERTICES: + { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); + glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); + + for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); + glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); + glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); + glmesh->m_vertices->at(i).xyzw[3] = 1; + btVector3 normal(visual->m_geometry.m_vertices[i]); + if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) + { + normal = visual->m_geometry.m_normals[i]; + } + else + { + normal.safeNormalize(); + } + + btVector3 uv(0.5, 0.5, 0); + if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) + { + uv = visual->m_geometry.m_uvs[i]; + } + glmesh->m_vertices->at(i).normal[0] = normal[0]; + glmesh->m_vertices->at(i).normal[1] = normal[1]; + glmesh->m_vertices->at(i).normal[2] = normal[2]; + glmesh->m_vertices->at(i).uv[0] = uv[0]; + glmesh->m_vertices->at(i).uv[1] = uv[1]; + + } + for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) + { + glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; + + } + glmesh->m_numIndices = visual->m_geometry.m_indices.size(); + glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); + + break; + } case UrdfGeometry::FILE_OBJ: { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 63ad3e30e..4edf850b2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -87,6 +87,12 @@ struct UrdfGeometry std::string m_meshFileName; btVector3 m_meshScale; + btArray m_vertices; + btArray m_uvs; + btArray m_normals; + btArray m_indices; + + UrdfMaterial m_localMaterial; bool m_hasLocalMaterial; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f41d37254..01371d357 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1377,6 +1377,9 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy { indexUpload[i]=indices[i]; } + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = 0; + command->m_createUserShapeArgs.m_numUserShapes++; cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes); delete [] data; @@ -1386,6 +1389,82 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy return -1; } +B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs) +{ + if (numUVs == 0 && numNormals == 0) + { + return b3CreateCollisionShapeAddConcaveMesh(physClient, commandHandle, meshScale, vertices, numVertices, indices, numIndices); + } + + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)); + if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)) + { + int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes; + if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >= 0) + { + int i = 0; + if (numVertices>B3_MAX_NUM_VERTICES) + numVertices = B3_MAX_NUM_VERTICES; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; + + int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals*sizeof(double)*3+numUVs*sizeof(double)*2; + char* data = new char[totalUploadSizeInBytes]; + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3); + double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)); + double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+ numNormals * sizeof(double) * 3); + for (i = 0; iB3_MAX_NUM_INDICES) + numIndices = B3_MAX_NUM_INDICES; + + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices; + for (i = 0; im_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = numNormals; + for (i = 0; i < numNormals; i++) + { + normalUpload[i * 3 + 0] = normals[i * 3 + 0]; + normalUpload[i * 3 + 1] = normals[i * 3 + 1]; + normalUpload[i * 3 + 2] = normals[i * 3 + 2]; + } + + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = numUVs; + for (i = 0; i < numUVs; i++) + { + uvUpload[i * 2 + 0] = uvs[i * 2 + 0]; + uvUpload[i * 2 + 1] = uvs[i * 2 + 1]; + } + command->m_createUserShapeArgs.m_numUserShapes++; + cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes); + delete[] data; + return shapeIndex; + } + } + return -1; +} + + B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]) { return b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 28fb2d7c9..0545c899d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -486,6 +486,9 @@ extern "C" B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height); B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant); B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]); + B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs); + + B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags); B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]); B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double specularColor[/*3*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 43d18ad91..3c995149f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2520,7 +2520,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface if (m_data->m_pluginManager.getRenderInterface()) { CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); - m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO); + int visualShapeUniqueid = m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO); + colObj->setUserIndex3(visualShapeUniqueid); } } virtual void setBodyUniqueId(int bodyId) @@ -4706,18 +4707,68 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct case URDF_GEOM_MESH: { std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; - const std::string& error_message_prefix = ""; - std::string out_found_filename; - int out_type; - if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) + if (fileName.length()) { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + const std::string& error_message_prefix = ""; + std::string out_found_filename; + int out_type; + if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + if (foundFile) + { + visualShape.m_geometry.m_meshFileType = out_type; + visualShape.m_geometry.m_meshFileName = fileName; + } + else + { + + } } + else + { + visualShape.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; + int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numVertices; + int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numIndices; + int numUVs = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numUVs; + int numNormals = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numNormals; - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - visualShape.m_geometry.m_meshFileType = out_type; - visualShape.m_geometry.m_meshFileName = fileName; + if (numVertices > 0 && numIndices >0) + { + char* data = bufferServerToClient; + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3); + double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)); + double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+numNormals*sizeof(double)*3); + for (int i = 0; i < numIndices; i++) + { + visualShape.m_geometry.m_indices.push_back(indexUpload[i]); + } + for (int i = 0; i < numVertices; i++) + { + btVector3 v0(vertexUpload[i * 3 + 0], + vertexUpload[i * 3 + 1], + vertexUpload[i * 3 + 2]); + visualShape.m_geometry.m_vertices.push_back(v0); + } + for (int i = 0; i < numNormals; i++) + { + btVector3 normal(normalUpload[i * 3 + 0], + normalUpload[i * 3 + 1], + normalUpload[i * 3 + 2]); + visualShape.m_geometry.m_normals.push_back(normal); + } + for (int i = 0; i < numUVs; i++) + { + btVector3 uv(uvUpload[i * 2 + 0], uvUpload[i * 2 + 1],0); + visualShape.m_geometry.m_uvs.push_back(uv); + } + } + } visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2737ac30f..adab74ee6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -932,6 +932,8 @@ struct b3CreateUserShapeData int m_visualFlags; int m_numVertices; int m_numIndices; + int m_numUVs; + int m_numNormals; double m_rgbaColor[4]; double m_specularColor[3]; }; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 6fb538089..359a251eb 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -361,6 +361,49 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa { case UrdfGeometry::MEMORY_VERTICES: { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); + glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); + + for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); + glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); + glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); + glmesh->m_vertices->at(i).xyzw[3] = 1; + btVector3 normal(visual->m_geometry.m_vertices[i]); + if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) + { + normal = visual->m_geometry.m_normals[i]; + } + else + { + normal.safeNormalize(); + } + + btVector3 uv(0.5, 0.5, 0); + if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) + { + uv = visual->m_geometry.m_uvs[i]; + } + glmesh->m_vertices->at(i).normal[0] = normal[0]; + glmesh->m_vertices->at(i).normal[1] = normal[1]; + glmesh->m_vertices->at(i).normal[2] = normal[2]; + glmesh->m_vertices->at(i).uv[0] = uv[0]; + glmesh->m_vertices->at(i).uv[1] = uv[1]; + + } + for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) + { + glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; + + } + glmesh->m_numIndices = visual->m_geometry.m_indices.size(); + glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); + break; } case UrdfGeometry::FILE_OBJ: diff --git a/examples/pybullet/examples/createTexturedMeshVisualShape.py b/examples/pybullet/examples/createTexturedMeshVisualShape.py new file mode 100644 index 000000000..3dbc10c2e --- /dev/null +++ b/examples/pybullet/examples/createTexturedMeshVisualShape.py @@ -0,0 +1,168 @@ +import pybullet as p +import time +import math + +def getRayFromTo(mouseX,mouseY): + width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() + camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] + farPlane = 10000 + rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] + invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) + rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] + rayFrom = camPos + oneOverWidth = float(1)/float(width) + oneOverHeight = float(1)/float(height) + dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] + dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] + rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] + rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], + rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], + rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] + return rayFrom,rayTo + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +p.setPhysicsEngineParameter(numSolverIterations=10) +p.setTimeStep(1./120.) +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") +#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) +p.loadURDF("plane100.urdf", useMaximalCoordinates=True) +#disable rendering during creation. +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) +#disable tinyrenderer, software (CPU) renderer, we don't use it here +p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) + +shift = [0,-0.02,0] +meshScale=[0.1,0.1,0.1] + +vertices=[ +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,1.000000,1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[1.000000,1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[1.000000,1.000000,1.000000], +[1.000000,1.000000,-1.000000] +] + +normals=[ +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000] +] + +uvs=[ +[0.750000,0.250000], +[1.000000,0.250000], +[1.000000,0.000000], +[0.750000,0.000000], +[0.500000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000], +[0.500000,0.000000], +[0.500000,0.000000], +[0.750000,0.000000], +[0.750000,0.250000], +[0.500000,0.250000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.000000,0.250000], +[0.000000,0.500000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.500000,0.250000], +[0.500000,0.500000], +[0.000000,0.000000], +[0.000000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000] +] +indices=[ 0, 1, 2, 0, 2, 3, #//ground face + 6, 5, 4, 7, 6, 4, #//top face + 10, 9, 8, 11, 10, 8, + 12, 13, 14, 12, 14, 15, + 18, 17, 16, 19, 18, 16, + 20, 21, 22, 20, 22, 23] + +#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices, uvs=uvs, normals=normals) +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) + +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") +collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) + +texUid = p.loadTexture("tex256.png") + +rangex = 1 +rangey = 1 +for i in range (rangex): + for j in range (rangey ): + bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) + p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +p.stopStateLogging(logId) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(1) + +colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] +currentColor = 0 + +while (1): + p.getCameraImage(320,200) + mouseEvents = p.getMouseEvents() + for e in mouseEvents: + if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): + mouseX = e[1] + mouseY = e[2] + rayFrom,rayTo=getRayFromTo(mouseX,mouseY) + rayInfo = p.rayTest(rayFrom,rayTo) + #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) + for l in range(len(rayInfo)): + hit = rayInfo[l] + objectUid = hit[0] + if (objectUid>=1): + #p.removeBody(objectUid) + p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) + currentColor+=1 + if (currentColor>=len(colors)): + currentColor=0 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index befb0086d..7452fec60 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -185,6 +185,34 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) return 0; } +// vector - double[2] which will be set by values from obVec +static int pybullet_internalSetVector2d(PyObject* obVec, double vector[2]) +{ + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + if (seq) + { + len = PySequence_Size(obVec); + assert(len == 2); + if (len == 2) + { + for (i = 0; i < len; i++) + { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + PyErr_Clear(); + return 0; +} + // vector - double[3] which will be set by values from obVec static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { @@ -6773,6 +6801,45 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe return numVerticesOut; } + +static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices) +{ + int numUVOut = 0; + + if (uvsObj) + { + PyObject* seqVerticesObj = PySequence_Fast(uvsObj, "expected a sequence of uvs"); + if (seqVerticesObj) + { + int numVerticesSrc = PySequence_Size(seqVerticesObj); + { + int i; + + if (numVerticesSrc > B3_MAX_NUM_VERTICES) + { + PyErr_SetString(SpamError, "Number of uvs exceeds the maximum."); + Py_DECREF(seqVerticesObj); + return 0; + } + for (i = 0; i < numVerticesSrc; i++) + { + PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i); + double uv[2]; + if (pybullet_internalSetVector2d(vertexObj, uv)) + { + if (uvs) + { + uvs[numUVOut * 2 + 0] = uv[0]; + uvs[numUVOut * 2 + 1] = uv[1]; + } + numUVOut++; + } + } + } + } + } + return numUVOut; +} static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices) { int numIndicesOut=0; @@ -7183,9 +7250,14 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb PyObject* halfExtentsObj = 0; - static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist, - &shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId)) + PyObject* verticesObj = 0; + PyObject* indicesObj = 0; + PyObject* normalsObj = 0; + PyObject* uvsObj = 0; + + static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "vertices", "indices", "normals", "uvs", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOOOOOi", kwlist, + &shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &verticesObj, &indicesObj, &normalsObj, &uvsObj, &physicsClientId)) { return NULL; } @@ -7228,6 +7300,45 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb pybullet_internalSetVectord(meshScaleObj, meshScale); shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName, meshScale); } + + if (shapeType == GEOM_MESH && verticesObj && indicesObj) + { + int numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES); + int numIndices = extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES); + int numNormals = extractVertices(normalsObj, 0, B3_MAX_NUM_VERTICES); + int numUVs = extractUVs(uvsObj, 0, B3_MAX_NUM_VERTICES); + + double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0; + int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0; + double* normals = numNormals ? malloc(numNormals * 3 * sizeof(double)) : 0; + double* uvs = numUVs ? malloc(numUVs * 2 * sizeof(double)) : 0; + + numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES); + pybullet_internalSetVectord(meshScaleObj, meshScale); + + if (indicesObj) + { + numIndices = extractIndices(indicesObj, indices, B3_MAX_NUM_INDICES); + } + if (numNormals) + { + extractVertices(normalsObj, normals, numNormals); + } + if (numUVs) + { + extractUVs(uvsObj, uvs, numUVs); + } + + if (numIndices) + { + shapeIndex = b3CreateVisualShapeAddMesh2(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices, normals, numNormals, uvs, numUVs); + } + free(uvs); + free(normals); + free(vertices); + free(indices); + } + if (shapeType == GEOM_PLANE) { double planeConstant = 0;