AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

PyBullet: allow createVisualShape to pass vertices, indices, normals and uv coordinates. This can be combined with changeVisualShape to set the texture.

Browse Source
This commit is contained in:
Erwin Coumans 2019-01-29 12:03:11 -08:00
parent 63683e8f02
commit b257bd731b
9 changed files with 522 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
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:
{

6
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -87,6 +87,12 @@ struct UrdfGeometry
std::string m_meshFileName;
btVector3 m_meshScale;
btArray<btVector3> m_vertices;
btArray<btVector3> m_uvs;
btArray<btVector3> m_normals;
btArray<int> m_indices;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;

79
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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; i<numVertices; i++)
{
vertexUpload[i * 3 + 0] = vertices[i * 3 + 0];
vertexUpload[i * 3 + 1] = vertices[i * 3 + 1];
vertexUpload[i * 3 + 2] = vertices[i * 3 + 2];
}
if (numIndices>B3_MAX_NUM_INDICES)
numIndices = B3_MAX_NUM_INDICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
for (i = 0; i<numIndices; i++)
{
indexUpload[i] = indices[i];
}
command->m_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);

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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*/]);

69
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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]);

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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];
};

43
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@ -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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
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:

168
examples/pybullet/examples/createTexturedMeshVisualShape.py Normal file
View 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

117
examples/pybullet/pybullet.c
View File

@ -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;