diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f98c290ce..8e487517b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3946,9 +3946,15 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct btTransform childTrans; childTrans.setIdentity(); const char* pathPrefix = ""; - if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1) + + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + btAlignedObjectArray textures; + + + for (int userShapeIndex = 0; userShapeIndex< clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++) { - int userShapeIndex = 0; UrdfVisual visualShape; visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; @@ -3960,97 +3966,95 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct switch (visualShape.m_geometry.m_type) { - case URDF_GEOM_CYLINDER: + case URDF_GEOM_CYLINDER: + { + visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight; + visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius; + break; + } + case URDF_GEOM_BOX: + { + visualShape.m_geometry.m_boxSize.setValue(2.*visShape.m_boxHalfExtents[0], + 2.*visShape.m_boxHalfExtents[1], + 2.*visShape.m_boxHalfExtents[2]); + break; + } + case URDF_GEOM_SPHERE: + { + visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius; + break; + + } + case URDF_GEOM_CAPSULE: + { + visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo; + if (visualShape.m_geometry.m_hasFromTo) + { + visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0], + visShape.m_capsuleFrom[1], + visShape.m_capsuleFrom[2]); + visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0], + visShape.m_capsuleTo[1], + visShape.m_capsuleTo[2]); + } + else { visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight; visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius; - break; } - case URDF_GEOM_BOX: + break; + } + 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 (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) { - visualShape.m_geometry.m_boxSize.setValue(2.*visShape.m_boxHalfExtents[0], - 2.*visShape.m_boxHalfExtents[1], - 2.*visShape.m_boxHalfExtents[2]); - break; - } - case URDF_GEOM_SPHERE: - { - visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius; - break; - - } - case URDF_GEOM_CAPSULE: - { - visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo; - if (visualShape.m_geometry.m_hasFromTo) - { - visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0], - visShape.m_capsuleFrom[1], - visShape.m_capsuleFrom[2]); - visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0], - visShape.m_capsuleTo[1], - visShape.m_capsuleTo[2]); - } - else - { - visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight; - visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius; - } - break; - } - 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 (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - visualShape.m_geometry.m_meshFileType = out_type; - visualShape.m_geometry.m_meshFileName = fileName; - - 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]); - break; - + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - default: - { - } + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + visualShape.m_geometry.m_meshFileType = out_type; + visualShape.m_geometry.m_meshFileName = fileName; + + 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]); + break; + + } + + default: + { + } }; visualShape.m_name = "in_memory"; - visualShape.m_materialName=""; - visualShape.m_sourceFileLocation="in_memory_unknown_line"; + visualShape.m_materialName = ""; + visualShape.m_sourceFileLocation = "in_memory_unknown_line"; visualShape.m_linkLocalFrame.setIdentity(); visualShape.m_geometry.m_hasLocalMaterial = false; - - - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - btAlignedObjectArray textures; - bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;; - bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;; - visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular; + + + + bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR) != 0;; + bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR) != 0;; + visualShape.m_geometry.m_hasLocalMaterial = hasRGBA | hasSpecular; if (visualShape.m_geometry.m_hasLocalMaterial) { if (hasRGBA) { - visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); - } else + visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); + } + else { - + } if (hasSpecular) { @@ -4058,13 +4062,14 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]); - } else + } + else { - visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4); + visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4, 0.4, 0.4); } } - - if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0) + + if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform != 0) { childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1], @@ -4076,42 +4081,44 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3])); } - - u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); - - if (vertices.size() && indices.size()) - { - if (1) - { - int textureIndex = -1; - if (textures.size()) - { - - textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1,textures[0].m_width,textures[0].m_height); - } - int graphicsIndex = -1; - { - B3_PROFILE("registerGraphicsShape"); - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - if (graphicsIndex>=0) - { - int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); - InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); - visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; - visualHandle->m_tinyRendererVisualShapeIndex = -1; - //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance - //store needed info for tinyrenderer - visualHandle->m_localInertiaFrame = localInertiaFrame; - visualHandle->m_visualShape = visualShape; - visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; - } + u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures); + + } + + if (vertices.size() && indices.size()) + { + if (1) + { + int textureIndex = -1; + if (textures.size()) + { + + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height); + } + int graphicsIndex = -1; + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + if (graphicsIndex >= 0) + { + int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); + InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); + visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; + visualHandle->m_tinyRendererVisualShapeIndex = -1; + //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance + //store needed info for tinyrenderer + visualHandle->m_localInertiaFrame = localInertiaFrame; + //visualHandle->m_visualShape1 = visualShape; + visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; + + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; } } } } + return hasStatus; } @@ -6083,7 +6090,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str for (int l=0;lgetNumLinks();l++) { serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; diff --git a/examples/pybullet/examples/createVisualShapeArray.py b/examples/pybullet/examples/createVisualShapeArray.py new file mode 100644 index 000000000..36f94690c --- /dev/null +++ b/examples/pybullet/examples/createVisualShapeArray.py @@ -0,0 +1,81 @@ +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] +shift1 = [0,0.1,0] +shift2 = [0,0,0] + +meshScale=[0.1,0.1,0.1] +#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) +visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck.obj",""], visualFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) +collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck_vhacd.obj",""], collisionFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) + + +rangex =2 +rangey = 2 +for i in range (rangex): + for j in range (rangey ): + mb = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i*2)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*4,1], useMaximalCoordinates=False) + p.changeVisualShape(mb,-1,rgbaColor=[1,1,1,1]) +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 + +p.getCameraImage(64,64, renderer=p.ER_BULLET_HARDWARE_OPENGL) + +while (1): + + 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>=0): + #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 0fd41f4bd..954856aa5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6135,6 +6135,235 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb return NULL; } + +static PyObject* pybullet_createVisualShapeArray(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + PyObject* shapeTypeArray = 0; + PyObject* radiusArray = 0; + PyObject* halfExtentsObjArray = 0; + PyObject* lengthArray = 0; + PyObject* fileNameArray = 0; + PyObject* meshScaleObjArray = 0; + PyObject* planeNormalObjArray = 0; + PyObject* flagsArray = 0; + PyObject* visualFramePositionObjArray = 0; + PyObject* visualFrameOrientationObjArray = 0; + + static char* kwlist[] = { "shapeTypes", "radii", "halfExtents", "lengths", "fileNames", "meshScales", "planeNormals", + "flags", "visualFramePositions", "visualFrameOrientations", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|OOOOOOOOOi", kwlist, + &shapeTypeArray, &radiusArray, &halfExtentsObjArray, &lengthArray, &fileNameArray, &meshScaleObjArray, &planeNormalObjArray, &flagsArray, &visualFramePositionObjArray, &visualFrameOrientationObjArray, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + + + { + b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm); + int numShapeTypes = 0; + int numRadius = 0; + int numHalfExtents = 0; + int numLengths = 0; + int numFileNames = 0; + int numMeshScales = 0; + int numPlaneNormals = 0; + int numFlags = 0; + int numPositions = 0; + int numOrientations = 0; + + int s; + PyObject* shapeTypeArraySeq = shapeTypeArray ? PySequence_Fast(shapeTypeArray, "expected a sequence of shape types") : 0; + PyObject* radiusArraySeq = radiusArray ? PySequence_Fast(radiusArray, "expected a sequence of radii") : 0; + PyObject* halfExtentsArraySeq = halfExtentsObjArray ? PySequence_Fast(halfExtentsObjArray, "expected a sequence of half extents") : 0; + PyObject* lengthArraySeq = lengthArray ? PySequence_Fast(lengthArray, "expected a sequence of lengths") : 0; + PyObject* fileNameArraySeq = fileNameArray ? PySequence_Fast(fileNameArray, "expected a sequence of filename") : 0; + PyObject* meshScaleArraySeq = meshScaleObjArray ? PySequence_Fast(meshScaleObjArray, "expected a sequence of mesh scale") : 0; + PyObject* planeNormalArraySeq = planeNormalObjArray ? PySequence_Fast(planeNormalObjArray, "expected a sequence of plane normal") : 0; + PyObject* flagsArraySeq = flagsArray ? PySequence_Fast(flagsArray, "expected a sequence of flags") : 0; + PyObject* positionArraySeq = visualFramePositionObjArray ? PySequence_Fast(visualFramePositionObjArray, "expected a sequence of visual frame positions") : 0; + PyObject* orientationArraySeq = visualFrameOrientationObjArray ? PySequence_Fast(visualFrameOrientationObjArray, "expected a sequence of visual frame orientations") : 0; + + if (shapeTypeArraySeq == 0) + { + PyErr_SetString(SpamError, "expected a sequence of shape types"); + return NULL; + } + + numShapeTypes = shapeTypeArray ? PySequence_Size(shapeTypeArray) : 0; + numRadius = radiusArraySeq ? PySequence_Size(radiusArraySeq) : 0; + numHalfExtents = halfExtentsArraySeq ? PySequence_Size(halfExtentsArraySeq) : 0; + numLengths = lengthArraySeq ? PySequence_Size(lengthArraySeq) : 0; + numFileNames = fileNameArraySeq ? PySequence_Size(fileNameArraySeq) : 0; + numMeshScales = meshScaleArraySeq ? PySequence_Size(meshScaleArraySeq) : 0; + numPlaneNormals = planeNormalArraySeq ? PySequence_Size(planeNormalArraySeq) : 0; + + for (s = 0; s= GEOM_SPHERE) + { + + int shapeIndex = -1; + + if (shapeType == GEOM_SPHERE && s <= numRadius) + { + double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s); + if (radius > 0) + { + shapeIndex = b3CreateVisualShapeAddSphere(commandHandle, radius); + } + } + if (shapeType == GEOM_BOX) + { + PyObject* halfExtentsObj = 0; + double halfExtents[3] = { 1, 1, 1 }; + + if (halfExtentsArraySeq && s <= numHalfExtents) + { + if (PyList_Check(halfExtentsArraySeq)) + { + halfExtentsObj = PyList_GET_ITEM(halfExtentsArraySeq, s); + } + else + { + halfExtentsObj = PyTuple_GET_ITEM(halfExtentsArraySeq, s); + } + } + pybullet_internalSetVectord(halfExtentsObj, halfExtents); + shapeIndex = b3CreateVisualShapeAddBox(commandHandle, halfExtents); + } + if (shapeType == GEOM_CAPSULE && s <= numRadius) + { + double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s); + double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s); + if (radius > 0 && height >= 0) + { + shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle, radius, height); + } + } + if (shapeType == GEOM_CYLINDER && s <= numRadius && s 0 && height >= 0) + { + shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle, radius, height); + } + } + if (shapeType == GEOM_MESH) + { + double meshScale[3] = { 1, 1, 1 }; + + PyObject* meshScaleObj = meshScaleArraySeq ? PyList_GET_ITEM(meshScaleArraySeq, s) : 0; + PyObject* fileNameObj = fileNameArraySeq ? PyList_GET_ITEM(fileNameArraySeq, s) : 0; + const char* fileName = 0; + + if (fileNameObj) + { +#if PY_MAJOR_VERSION >= 3 + PyObject* ob = PyUnicode_AsASCIIString(fileNameObj); + fileName = PyBytes_AS_STRING(ob); +#else + fileName = PyString_AsString(fileNameObj); +#endif + } + if (meshScaleObj) + { + pybullet_internalSetVectord(meshScaleObj, meshScale); + } + if (fileName) + { + shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName, meshScale); + } + + } + if (shapeType == GEOM_PLANE) + { + PyObject* planeNormalObj = planeNormalArraySeq ? PyList_GET_ITEM(planeNormalArraySeq, s) : 0; + double planeNormal[3]; + double planeConstant = 0; + pybullet_internalSetVectord(planeNormalObj, planeNormal); + shapeIndex = b3CreateVisualShapeAddPlane(commandHandle, planeNormal, planeConstant); + } + if (flagsArraySeq) + { + int flags = pybullet_internalGetIntFromSequence(flagsArraySeq, s); + b3CreateVisualSetFlag(commandHandle, shapeIndex, flags); + } + if (positionArraySeq || orientationArraySeq) + { + PyObject* visualFramePositionObj = positionArraySeq ? PyList_GET_ITEM(positionArraySeq, s) : 0; + PyObject* visualFrameOrientationObj = orientationArraySeq ? PyList_GET_ITEM(orientationArraySeq, s) : 0; + double visualFramePosition[3] = { 0, 0, 0 }; + double visualFrameOrientation[4] = { 0, 0, 0, 1 }; + if (visualFramePositionObj) + { + pybullet_internalSetVectord(visualFramePositionObj, visualFramePosition); + } + + if (visualFrameOrientationObj) + { + pybullet_internalSetVector4d(visualFrameOrientationObj, visualFrameOrientation); + } + if (shapeIndex >= 0) + { + b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition, visualFrameOrientation); + } + + } + } + + + + } + + if (shapeTypeArraySeq) + Py_DECREF(shapeTypeArraySeq); + if (radiusArraySeq) + Py_DECREF(radiusArraySeq); + if (halfExtentsArraySeq) + Py_DECREF(halfExtentsArraySeq); + if (lengthArraySeq) + Py_DECREF(lengthArraySeq); + if (fileNameArraySeq) + Py_DECREF(fileNameArraySeq); + if (meshScaleArraySeq) + Py_DECREF(meshScaleArraySeq); + if (planeNormalArraySeq) + Py_DECREF(planeNormalArraySeq); + if (flagsArraySeq) + Py_DECREF(flagsArraySeq); + if (positionArraySeq) + Py_DECREF(positionArraySeq); + if (orientationArraySeq) + Py_DECREF(orientationArraySeq); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + int uid = b3GetStatusVisualShapeUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + + PyErr_SetString(SpamError, "createVisualShapeArray failed."); + return NULL; +} static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -8372,11 +8601,14 @@ static PyMethodDef SpamMethods[] = { "Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, { "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS, - "Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise." }, + "Create collision shapes. Returns a non-negative (int) unique id, if successfull, negative otherwise." }, {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS, "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + { "createVisualShapeArray", (PyCFunction)pybullet_createVisualShapeArray, METH_VARARGS | METH_KEYWORDS, + "Create visual shapes. Returns a non-negative (int) unique id, if successfull, negative otherwise." }, + {"createMultiBody", (PyCFunction)pybullet_createMultiBody, METH_VARARGS | METH_KEYWORDS, "Create a multi body. Returns a non-negative (int) unique id, if successfull, negative otherwise."},