AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-04 17:40:06 +00:00
Code Issues Releases Wiki Activity

Add pybullet.addUserDebugPoints

Browse Source
This commit is contained in:
Kentaro Wada 2021-10-14 14:14:33 +01:00
parent 48dc1c45da
commit 00dcce85f5
17 changed files with 365 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@ -112,6 +112,7 @@ struct GUIHelperInterface
virtual int addUserDebugText3D(const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags, int replaceItemUid) { return -1; }
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid) { return -1; };
virtual int addUserDebugPoints(const double debugPointPositionXYZ[], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid, int debugPointNum) { return -1; };
virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue) { return -1; };
virtual int readUserDebugParameter(int itemUniqueId, double* value) { return 0; }
@ -219,6 +220,10 @@ struct DummyGUIHelper : public GUIHelperInterface
{
return -1;
}
virtual int addUserDebugPoints(const double debugPointPositionXYZ[3], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid, int debugPointNum)
{
return -1;
};
virtual void removeUserDebugItem(int debugItemUniqueId)
{
}

1
examples/CommonInterfaces/CommonRenderInterface.h
View File

@ -80,6 +80,7 @@ struct CommonRenderInterface
virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth) = 0;
virtual void drawPoint(const float* position, const float color[4], float pointDrawSize) = 0;
virtual void drawPoint(const double* position, const double color[4], double pointDrawSize) = 0;
virtual void drawPoints(const float* positions, const float* colors, int numPoints, int pointStrideInBytes, float pointDrawSize) = 0;
virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex = -1, int vertexLayout = 0) = 0;
virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType = B3_GL_TRIANGLES, int textureIndex = -1) = 0;

22
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@ -302,6 +302,7 @@ static GLint lines_ModelViewMatrix = 0;
static GLint lines_ProjectionMatrix = 0;
static GLint lines_position = 0;
static GLint lines_colour = 0;
static GLint lines_colourIn = 0;
GLuint lineVertexBufferObject = 0;
GLuint lineVertexArrayObject = 0;
GLuint lineIndexVbo = 0;
@ -1247,6 +1248,7 @@ void GLInstancingRenderer::InitShaders()
lines_ModelViewMatrix = glGetUniformLocation(linesShader, "ModelViewMatrix");
lines_ProjectionMatrix = glGetUniformLocation(linesShader, "ProjectionMatrix");
lines_colour = glGetUniformLocation(linesShader, "colour");
lines_colourIn = glGetAttribLocation(linesShader, "colourIn");
lines_position = glGetAttribLocation(linesShader, "position");
glLinkProgram(linesShader);
glUseProgram(linesShader);
@ -1889,7 +1891,7 @@ void GLInstancingRenderer::drawPoint(const float* positions, const float color[4
{
drawPoints(positions, color, 1, 3 * sizeof(float), pointDrawSize);
}
void GLInstancingRenderer::drawPoints(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, float pointDrawSize)
void GLInstancingRenderer::drawPoints(const float* positions, const float* colors, int numPoints, int pointStrideInBytes, float pointDrawSize)
{
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, 0);
@ -1898,13 +1900,11 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
glUseProgram(linesShader);
glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
glUniform4f(lines_colour, color[0], color[1], color[2], color[3]);
glUniform4f(lines_colour, 0, 0, 0, -1);
glPointSize(pointDrawSize);
glBindVertexArray(lineVertexArrayObject);
glBindBuffer(GL_ARRAY_BUFFER, lineVertexBufferObject);
int maxPointsInBatch = MAX_POINTS_IN_BATCH;
int remainingPoints = numPoints;
int offsetNumPoints = 0;
@ -1913,10 +1913,16 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
int curPointsInBatch = b3Min(maxPointsInBatch, remainingPoints);
if (curPointsInBatch)
{
glBufferSubData(GL_ARRAY_BUFFER, 0, curPointsInBatch * pointStrideInBytes, positions + offsetNumPoints * (pointStrideInBytes / sizeof(float)));
glEnableVertexAttribArray(0);
int numFloats = 3; // pointStrideInBytes / sizeof(float);
glVertexAttribPointer(0, numFloats, GL_FLOAT, GL_FALSE, pointStrideInBytes, 0);
glBindBuffer(GL_ARRAY_BUFFER, lineVertexBufferObject);
glBufferSubData(GL_ARRAY_BUFFER, 0, curPointsInBatch * pointStrideInBytes, positions + offsetNumPoints * 3);
glEnableVertexAttribArray(lines_position);
glVertexAttribPointer(lines_position, 3, GL_FLOAT, GL_FALSE, pointStrideInBytes, 0);
glBindBuffer(GL_ARRAY_BUFFER, lineVertexArrayObject);
glBufferSubData(GL_ARRAY_BUFFER, 0, curPointsInBatch * 4 * sizeof(float), colors + offsetNumPoints * 4);
glEnableVertexAttribArray(lines_colourIn);
glVertexAttribPointer(lines_colourIn, 4, GL_FLOAT, GL_FALSE, 4 * sizeof(float), 0);
glDrawArrays(GL_POINTS, 0, curPointsInBatch);
remainingPoints -= curPointsInBatch;
offsetNumPoints += curPointsInBatch;

2
examples/OpenGLWindow/Shaders/linesVS.glsl
View File

@ -11,7 +11,7 @@ out vec4 colourV;
void main (void)
{
colourV = colour;
colourV = (colour[3] == -1) ? colourIn : colour;
gl_Position = ProjectionMatrix * ModelViewMatrix * position;
}

3
examples/OpenGLWindow/Shaders/linesVS.h
View File

@ -5,10 +5,11 @@ static const char* linesVertexShader= \
"uniform mat4 ProjectionMatrix;\n"
"uniform vec4 colour;\n"
"in vec4 position;\n"
"in vec4 colourIn;\n"
"out vec4 colourV;\n"
"void main (void)\n"
"{\n"
" colourV = colour;\n"
" colourV = (colour[3] == -1) ? colourIn : colour;\n"
" gl_Position = ProjectionMatrix * ModelViewMatrix * position;\n"
" \n"
"}\n"

3
examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
View File

@ -636,6 +636,9 @@ void SimpleOpenGL2Renderer::drawPoint(const float* position, const float color[4
void SimpleOpenGL2Renderer::drawPoint(const double* position, const double color[4], double pointDrawSize)
{
}
void SimpleOpenGL2Renderer::drawPoints(const float* positions, const float* colors, int numPoints, int pointStrideInBytes, float pointDrawSize)
{
}
void SimpleOpenGL2Renderer::updateShape(int shapeIndex, const float* vertices, int numVertices)
{

2
examples/OpenGLWindow/SimpleOpenGL2Renderer.h
View File

@ -82,6 +82,8 @@ public:
virtual void drawPoint(const double* position, const double color[4], double pointDrawSize);
virtual void drawPoints(const float* positions, const float* colors, int numPoints, int pointStrideInBytes, float pointDrawSize);
virtual void updateShape(int shapeIndex, const float* vertices, int numVertices);
virtual void clearZBuffer();

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -4163,6 +4163,44 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3Physics
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddPoints3D(b3PhysicsClientHandle physClient, const double positionsXYZ[/*3n*/], const double colorsRGB[/*3n*/], const double pointSize, const double lifeTime, const int pointNum)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_HAS_POINTS;
command->m_userDebugDrawArgs.m_debugPointNum = pointNum;
command->m_userDebugDrawArgs.m_pointSize = pointSize;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
int totalUploadSizeInBytes = pointNum * sizeof(double) * 3 * 2;
char* data = new char[totalUploadSizeInBytes];
double* pointPositionsUpload = (double*) data;
double* pointColorsUpload = (double*)(data + pointNum * sizeof(double) * 3);
for (int i = 0; i < pointNum; i++)
{
pointPositionsUpload[i * 3 + 0] = positionsXYZ[i * 3 + 0];
pointPositionsUpload[i * 3 + 1] = positionsXYZ[i * 3 + 1];
pointPositionsUpload[i * 3 + 2] = positionsXYZ[i * 3 + 2];
}
for (int i = 0; i < pointNum; i++)
{
pointColorsUpload[i * 3 + 0] = colorsRGB[i * 3 + 0];
pointColorsUpload[i * 3 + 1] = colorsRGB[i * 3 + 1];
pointColorsUpload[i * 3 + 2] = colorsRGB[i * 3 + 2];
}
cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
delete[] data;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, const double positionXYZ[3], const double colorRGB[3], double textSize, double lifeTime)
{

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -231,6 +231,7 @@ extern "C"
/// Add/remove user-specific debug lines and debug text messages
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, const double fromXYZ[/*3*/], const double toXYZ[/*3*/], const double colorRGB[/*3*/], double lineWidth, double lifeTime);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddPoints3D(b3PhysicsClientHandle physClient, const double positionsXYZ[/*3n*/], const double colorsRGB[/*3*/], double pointSize, double lifeTime, int pointNum);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, const double positionXYZ[/*3*/], const double colorRGB[/*3*/], double textSize, double lifeTime);
B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);

49
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1643,6 +1643,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<const unsigned char*> m_heightfieldDatas;
btAlignedObjectArray<int> m_allocatedTextures;
btAlignedObjectArray<unsigned char*> m_allocatedTexturesRequireFree;
btAlignedObjectArray<double*> m_debugPointsDatas;
btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
@ -3021,9 +3022,16 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
//we can't free them right away, due to caching based on memory pointer in PhysicsServerExample
free(m_data->m_allocatedTexturesRequireFree[i]);
}
for (int i = 0; i < m_data->m_debugPointsDatas.size(); i++)
{
free(m_data->m_debugPointsDatas[i]);
}
m_data->m_heightfieldDatas.clear();
m_data->m_allocatedTextures.clear();
m_data->m_allocatedTexturesRequireFree.clear();
m_data->m_debugPointsDatas.clear();
m_data->m_meshInterfaces.clear();
m_data->m_collisionShapes.clear();
m_data->m_bulletCollisionShape2UrdfCollision.clear();
@ -6073,6 +6081,47 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_POINTS)
{
int replaceItemUid = -1;
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)
{
replaceItemUid = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId;
}
int pointNum = clientCmd.m_userDebugDrawArgs.m_debugPointNum;
double* pointPositionsUpload = (double*)bufferServerToClient;
double* pointPositions = (double*)malloc(pointNum * 3 * sizeof(double));
double* pointColorsUpload = (double*)(bufferServerToClient + pointNum * 3 * sizeof(double));
double* pointColors = (double*)malloc(pointNum * 3 * sizeof(double));
for (int i = 0; i < pointNum; i++) {
pointPositions[i * 3 + 0] = pointPositionsUpload[i * 3 + 0];
pointPositions[i * 3 + 1] = pointPositionsUpload[i * 3 + 1];
pointPositions[i * 3 + 2] = pointPositionsUpload[i * 3 + 2];
pointColors[i * 3 + 0] = pointColorsUpload[i * 3 + 0];
pointColors[i * 3 + 1] = pointColorsUpload[i * 3 + 1];
pointColors[i * 3 + 2] = pointColorsUpload[i * 3 + 2];
}
m_data->m_debugPointsDatas.push_back(pointPositions);
m_data->m_debugPointsDatas.push_back(pointColors);
int uid = m_data->m_guiHelper->addUserDebugPoints(
pointPositions,
pointColors,
clientCmd.m_userDebugDrawArgs.m_pointSize,
clientCmd.m_userDebugDrawArgs.m_lifeTime,
trackingVisualShapeIndex,
replaceItemUid,
clientCmd.m_userDebugDrawArgs.m_debugPointNum);
if (uid >= 0)
{
serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
{
m_data->m_guiHelper->removeAllUserDebugItems();

118
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -134,6 +134,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperResetCamera,
eGUIHelperChangeGraphicsInstanceFlags,
eGUIHelperSetRgbBackground,
eGUIUserDebugAddPoints,
};
#include <stdio.h>
@ -491,6 +492,19 @@ struct UserDebugDrawLine
int m_replaceItemUid;
};
struct UserDebugDrawPoint
{
const double* m_debugPointPositions;
const double* m_debugPointColors;
int m_debugPointNum;
double m_pointSize;
double m_lifeTime;
int m_itemUniqueId;
int m_trackingVisualShapeIndex;
int m_replaceItemUid;
};
struct UserDebugParameter
{
char m_text[1024];
@ -1425,6 +1439,55 @@ public:
return m_resultDebugLineUid;
}
btAlignedObjectArray<UserDebugDrawPoint> m_userDebugPoints;
UserDebugDrawPoint m_tmpPoint;
int m_resultDebugPointUid;
virtual int addUserDebugPoints(const double* debugPointPositions, const double* debugPointColors, double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid, int debugPointNum)
{
m_tmpPoint.m_lifeTime = lifeTime;
m_tmpPoint.m_pointSize = pointSize;
m_tmpPoint.m_itemUniqueId = replaceItemUid < 0 ? m_uidGenerator++ : replaceItemUid;
m_tmpPoint.m_debugPointPositions = debugPointPositions;
m_tmpPoint.m_debugPointColors = debugPointColors;
m_tmpPoint.m_debugPointNum = debugPointNum;
m_tmpPoint.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
m_tmpPoint.m_replaceItemUid = replaceItemUid;
//don't block when replacing an item
if (replaceItemUid>=0 && replaceItemUid<m_userDebugPoints.size())
{
//find the right slot
int slot=-1;
for (int i=0;i<m_userDebugPoints.size();i++)
{
if (replaceItemUid == m_userDebugPoints[i].m_itemUniqueId)
{
slot = i;
}
}
if (slot>=0)
{
m_userDebugPoints[slot] = m_tmpPoint;
}
m_resultDebugPointUid = replaceItemUid;
}
else
{
m_cs->lock();
setSharedParam(1, eGUIUserDebugAddPoints);
m_resultDebugPointUid = -1;
workerThreadWait();
}
return m_resultDebugPointUid;
}
int m_removeDebugItemUid;
virtual void removeUserDebugItem(int debugItemUniqueId)
@ -2601,6 +2664,29 @@ void PhysicsServerExample::updateGraphics()
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIUserDebugAddPoints:
{
B3_PROFILE("eGUIUserDebugAddPoints");
if (m_multiThreadedHelper->m_tmpPoint.m_replaceItemUid >= 0)
{
for (int i = 0; i < m_multiThreadedHelper->m_userDebugPoints.size(); i++)
{
if (m_multiThreadedHelper->m_userDebugPoints[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpPoint.m_replaceItemUid)
{
m_multiThreadedHelper->m_userDebugPoints[i] = m_multiThreadedHelper->m_tmpPoint;
m_multiThreadedHelper->m_resultDebugPointUid = m_multiThreadedHelper->m_tmpPoint.m_replaceItemUid;
}
}
}
else
{
m_multiThreadedHelper->m_userDebugPoints.push_back(m_multiThreadedHelper->m_tmpPoint);
m_multiThreadedHelper->m_resultDebugPointUid = m_multiThreadedHelper->m_userDebugPoints[m_multiThreadedHelper->m_userDebugPoints.size() - 1].m_itemUniqueId;
}
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIUserDebugRemoveItem:
{
B3_PROFILE("eGUIUserDebugRemoveItem");
@ -2615,6 +2701,16 @@ void PhysicsServerExample::updateGraphics()
}
}
for (int i = 0; i < m_multiThreadedHelper->m_userDebugPoints.size(); i++)
{
if (m_multiThreadedHelper->m_userDebugPoints[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
{
m_multiThreadedHelper->m_userDebugPoints.swap(i, m_multiThreadedHelper->m_userDebugPoints.size() - 1);
m_multiThreadedHelper->m_userDebugPoints.pop_back();
break;
}
}
for (int i = 0; i < m_multiThreadedHelper->m_userDebugText.size(); i++)
{
if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
@ -2932,6 +3028,28 @@ void PhysicsServerExample::drawUserDebugLines()
m_multiThreadedHelper->m_userDebugText[i].textSize);
*/
}
for (int i = 0; i < m_multiThreadedHelper->m_userDebugPoints.size(); i++) {
const double* positions = m_multiThreadedHelper->m_userDebugPoints[i].m_debugPointPositions;
const double* colors = m_multiThreadedHelper->m_userDebugPoints[i].m_debugPointColors;
const int pointNum = m_multiThreadedHelper->m_userDebugPoints[i].m_debugPointNum;
const double sz = m_multiThreadedHelper->m_userDebugPoints[i].m_pointSize;
float* pos = (float*)malloc(pointNum * 3 * sizeof(float));
float* clr = (float*)malloc(pointNum * 4 * sizeof(float));
for (int i = 0; i < pointNum; i++) {
pos[i * 3 + 0] = (float)positions[i * 3 + 0];
pos[i * 3 + 1] = (float)positions[i * 3 + 1];
pos[i * 3 + 2] = (float)positions[i * 3 + 2];
clr[i * 4 + 0] = (float)colors[i * 3 + 0];
clr[i * 4 + 1] = (float)colors[i * 3 + 1];
clr[i * 4 + 2] = (float)colors[i * 3 + 2];
clr[i * 4 + 3] = 1.f;
}
m_guiHelper->getAppInterface()->m_renderer->drawPoints(pos, clr, pointNum, 3 * sizeof(float), sz);
free(pos);
free(clr);
}
}
}

4
examples/SharedMemory/RemoteGUIHelper.cpp
View File

@ -627,6 +627,10 @@ int RemoteGUIHelper::addUserDebugLine(const double debugLineFromXYZ[3], const do
{
return -1;
}
int RemoteGUIHelper::addUserDebugPoints(const double debugPointPositionXYZ[3], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid, int debugPointNum)
{
return -1;
}
void RemoteGUIHelper::removeUserDebugItem(int debugItemUniqueId)
{
}

1
examples/SharedMemory/RemoteGUIHelper.h
View File

@ -69,6 +69,7 @@ struct RemoteGUIHelper : public GUIHelperInterface
virtual void drawText3D(const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag);
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid);
virtual int addUserDebugPoints(const double debugPointPositionXYZ[3], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid, int debugPointNum);
virtual void removeUserDebugItem(int debugItemUniqueId);
virtual void removeAllUserDebugItems();

4
examples/SharedMemory/RemoteGUIHelperTCP.cpp
View File

@ -639,6 +639,10 @@ int RemoteGUIHelperTCP::addUserDebugLine(const double debugLineFromXYZ[3], const
{
return -1;
}
int RemoteGUIHelperTCP::addUserDebugPoints(const double debugPointPositionXYZ[3], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid)
{
return -1;
}
void RemoteGUIHelperTCP::removeUserDebugItem(int debugItemUniqueId)
{
}

1
examples/SharedMemory/RemoteGUIHelperTCP.h
View File

@ -66,6 +66,7 @@ struct RemoteGUIHelperTCP : public GUIHelperInterface
virtual void drawText3D(const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag);
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid);
virtual int addUserDebugPoints(const double debugPointPositionXYZ[3], const double debugPointColorRGB[3], double pointSize, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid);
virtual void removeUserDebugItem(int debugItemUniqueId);
virtual void removeAllUserDebugItems();

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -847,6 +847,7 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_HAS_PARENT_OBJECT = 1024,
USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID = 2048,
USER_DEBUG_REMOVE_ALL_PARAMETERS = 4096,
USER_DEBUG_HAS_POINTS = 8192,
};
struct UserDebugDrawArgs
@ -856,6 +857,9 @@ struct UserDebugDrawArgs
double m_debugLineColorRGB[3];
double m_lineWidth;
int m_debugPointNum;
double m_pointSize;
double m_lifeTime;
int m_itemUniqueId;

157
examples/pybullet/pybullet.c
View File

@ -6398,6 +6398,119 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
}
}
static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
{
int numVerticesOut = 0;
if (verticesObj)
{
PyObject* seqVerticesObj = PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
if (seqVerticesObj)
{
int numVerticesSrc = PySequence_Size(seqVerticesObj);
{
int i;
if (numVerticesSrc > maxNumVertices)
{
PyErr_SetString(SpamError, "Number of vertices exceeds the maximum.");
Py_DECREF(seqVerticesObj);
return 0;
}
for (i = 0; i < numVerticesSrc; i++)
{
PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i);
double vertex[3];
if (pybullet_internalSetVectord(vertexObj, vertex))
{
if (vertices)
{
vertices[numVerticesOut * 3 + 0] = vertex[0];
vertices[numVerticesOut * 3 + 1] = vertex[1];
vertices[numVerticesOut * 3 + 2] = vertex[2];
}
numVerticesOut++;
}
}
}
}
}
return numVerticesOut;
}
static PyObject* pybullet_addUserDebugPoints(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int res = 0;
int parentObjectUniqueId = -1;
int parentLinkIndex = -1;
PyObject* pointPositionsObj = 0;
PyObject* pointColorsObj = 0;
double pointSize = 1.f;
double lifeTime = 0.f;
int physicsClientId = 0;
int debugItemUniqueId = -1;
int replaceItemUniqueId = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"pointPositions", "pointColorsRGB", "pointSize", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "replaceItemUniqueId", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ddiiii", kwlist, &pointPositionsObj, &pointColorsObj, &pointSize, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &replaceItemUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
int numPositions = extractVertices(pointPositionsObj, 0, B3_MAX_NUM_VERTICES);
if (numPositions == 0) {
return NULL;
}
double* pointPositions = numPositions ? malloc(numPositions * 3 * sizeof(double)) : 0;
numPositions = extractVertices(pointPositionsObj, pointPositions, B3_MAX_NUM_VERTICES);
int numColors = extractVertices(pointColorsObj, 0, B3_MAX_NUM_VERTICES);
double* pointColors = numColors ? malloc(numColors * 3 * sizeof(double)) : 0;
numColors = extractVertices(pointColorsObj, pointColors, B3_MAX_NUM_VERTICES);
if (numColors != numPositions) {
PyErr_SetString(SpamError, "numColors must match numPositions.");
return NULL;
}
commandHandle = b3InitUserDebugDrawAddPoints3D(sm, pointPositions, pointColors, pointSize, lifeTime, numPositions);
free(pointPositions);
free(pointColors);
if (parentObjectUniqueId >= 0)
{
b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId, parentLinkIndex);
}
if (replaceItemUniqueId >= 0)
{
b3UserDebugItemSetReplaceItemUniqueId(commandHandle, replaceItemUniqueId);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
{
debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
}
{
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
}
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
@ -8519,46 +8632,6 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL;
}
static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
{
int numVerticesOut = 0;
if (verticesObj)
{
PyObject* seqVerticesObj = PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
if (seqVerticesObj)
{
int numVerticesSrc = PySequence_Size(seqVerticesObj);
{
int i;
if (numVerticesSrc > B3_MAX_NUM_VERTICES)
{
PyErr_SetString(SpamError, "Number of vertices exceeds the maximum.");
Py_DECREF(seqVerticesObj);
return 0;
}
for (i = 0; i < numVerticesSrc; i++)
{
PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i);
double vertex[3];
if (pybullet_internalSetVectord(vertexObj, vertex))
{
if (vertices)
{
vertices[numVerticesOut * 3 + 0] = vertex[0];
vertices[numVerticesOut * 3 + 1] = vertex[1];
vertices[numVerticesOut * 3 + 2] = vertex[2];
}
numVerticesOut++;
}
}
}
}
}
return numVerticesOut;
}
static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
{
int numUVOut = 0;
@ -12836,6 +12909,10 @@ static PyMethodDef SpamMethods[] = {
"Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."},
{"addUserDebugPoints", (PyCFunction)pybullet_addUserDebugPoints, METH_VARARGS | METH_KEYWORDS,
"Add a user debug draw point with pointPositions[3], pointColorsRGB[3], pointSize, lifeTime. "
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."},
{"addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS,
"Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."},