PyBullet: allow createVisualShape to pass vertices, indices, normals and uv coordinates. This can be combined with changeVisualShape to set the texture.
This commit is contained in:
Erwin Coumans
168
examples/pybullet/examples/createTexturedMeshVisualShape.py
Normal file
168
examples/pybullet/examples/createTexturedMeshVisualShape.py
Normal file
@@ -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
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user