PyBullet/C-API: implement createVisualShapeArray, with multiple visual shapes (require 1 texture max, since visual shapes are merged)
This commit is contained in:
Erwin Coumans
@@ -3946,9 +3946,15 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
|
||||
btTransform childTrans;
|
||||
childTrans.setIdentity();
|
||||
const char* pathPrefix = "";
|
||||
if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1)
|
||||
|
||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
btAlignedObjectArray<BulletURDFTexture> 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,95 +3966,93 @@ 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<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
btAlignedObjectArray<BulletURDFTexture> 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
|
||||
{
|
||||
|
||||
}
|
||||
@@ -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],
|
||||
@@ -4077,41 +4082,43 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
|
||||
}
|
||||
|
||||
|
||||
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
|
||||
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures);
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
}
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
if (1)
|
||||
{
|
||||
if (1)
|
||||
int textureIndex = -1;
|
||||
if (textures.size())
|
||||
{
|
||||
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;
|
||||
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)
|
||||
{
|
||||
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 : "";
|
||||
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;
|
||||
}
|
||||
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
|
||||
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
|
||||
81
examples/pybullet/examples/createVisualShapeArray.py
Normal file
81
examples/pybullet/examples/createVisualShapeArray.py
Normal file
@@ -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
|
||||
@@ -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<numShapeTypes; s++)
|
||||
{
|
||||
int shapeType = pybullet_internalGetIntFromSequence(shapeTypeArraySeq, s);
|
||||
if (shapeType >= 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<numLengths)
|
||||
{
|
||||
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
|
||||
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
|
||||
if (radius > 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."},
|
||||
|
||||
|
||||
Reference in New Issue
Block a user