From 00dcce85f5af360ee8d05e739fdbc16361c68058 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 14 Oct 2021 14:14:33 +0100 Subject: Add pybullet.addUserDebugPoints --- .../CommonInterfaces/CommonGUIHelperInterface.h | 5 + examples/CommonInterfaces/CommonRenderInterface.h | 1 + examples/OpenGLWindow/GLInstancingRenderer.cpp | 22 +-- examples/OpenGLWindow/Shaders/linesVS.glsl | 2 +- examples/OpenGLWindow/Shaders/linesVS.h | 3 +- examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp | 3 + examples/OpenGLWindow/SimpleOpenGL2Renderer.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 38 +++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 49 +++++++ examples/SharedMemory/PhysicsServerExample.cpp | 118 ++++++++++++++++ examples/SharedMemory/RemoteGUIHelper.cpp | 4 + examples/SharedMemory/RemoteGUIHelper.h | 1 + examples/SharedMemory/RemoteGUIHelperTCP.cpp | 4 + examples/SharedMemory/RemoteGUIHelperTCP.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 4 + examples/pybullet/pybullet.c | 157 +++++++++++++++------ 17 files changed, 365 insertions(+), 50 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 7f8316fd1..2d4500374 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -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) { } diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 03e148457..01f5499ed 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -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; diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index c0e2f2370..75e3d4cd9 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -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; diff --git a/examples/OpenGLWindow/Shaders/linesVS.glsl b/examples/OpenGLWindow/Shaders/linesVS.glsl index f28363dac..335fac7bc 100644 --- a/examples/OpenGLWindow/Shaders/linesVS.glsl +++ b/examples/OpenGLWindow/Shaders/linesVS.glsl @@ -11,7 +11,7 @@ out vec4 colourV; void main (void) { - colourV = colour; + colourV = (colour[3] == -1) ? colourIn : colour; gl_Position = ProjectionMatrix * ModelViewMatrix * position; } diff --git a/examples/OpenGLWindow/Shaders/linesVS.h b/examples/OpenGLWindow/Shaders/linesVS.h index 8fc6eb3e7..0054b14cf 100644 --- a/examples/OpenGLWindow/Shaders/linesVS.h +++ b/examples/OpenGLWindow/Shaders/linesVS.h @@ -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" diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 0d7a56403..0d005f24e 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -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) { diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index 4355de558..f6d30ce49 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -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(); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5b070713a..d0d9d9318 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b6ec6d63b..87a0c69b0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 708ea7463..3f2d233f5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1643,6 +1643,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_heightfieldDatas; btAlignedObjectArray m_allocatedTextures; btAlignedObjectArray m_allocatedTexturesRequireFree; + btAlignedObjectArray m_debugPointsDatas; btHashMap m_bulletCollisionShape2UrdfCollision; btAlignedObjectArray 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(); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 6613de42b..3e11b30b4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -134,6 +134,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperResetCamera, eGUIHelperChangeGraphicsInstanceFlags, eGUIHelperSetRgbBackground, + eGUIUserDebugAddPoints, }; #include @@ -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 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=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); + } } } diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index b5754e185..4ef9395a1 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -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) { } diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h index 3d74b03fd..5ac890048 100644 --- a/examples/SharedMemory/RemoteGUIHelper.h +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -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(); diff --git a/examples/SharedMemory/RemoteGUIHelperTCP.cpp b/examples/SharedMemory/RemoteGUIHelperTCP.cpp index 8573fd0de..d4c463338 100644 --- a/examples/SharedMemory/RemoteGUIHelperTCP.cpp +++ b/examples/SharedMemory/RemoteGUIHelperTCP.cpp @@ -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) { } diff --git a/examples/SharedMemory/RemoteGUIHelperTCP.h b/examples/SharedMemory/RemoteGUIHelperTCP.h index 8a4458ee2..19e3a5957 100644 --- a/examples/SharedMemory/RemoteGUIHelperTCP.h +++ b/examples/SharedMemory/RemoteGUIHelperTCP.h @@ -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(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5305769c3..c2c5b720f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3c5d8f3ae..97ccea317 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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."}, -- cgit v1.2.1 From b22a2432b5b77c21c5f0fdfa3cc6e23368670f29 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Mon, 25 Oct 2021 21:04:42 +0100 Subject: Fix pybullet.addUserDebugPoints help --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 97ccea317..db7246b5e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -12910,7 +12910,7 @@ static PyMethodDef SpamMethods[] = { "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. " + "Add user debug draw points 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, -- cgit v1.2.1 From fd98afe7642fa2d566375886298680f1f49e9bd1 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Mon, 25 Oct 2021 21:07:06 +0100 Subject: Add new members at the last of a struct --- examples/SharedMemory/SharedMemoryCommands.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c2c5b720f..341c93123 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -857,9 +857,6 @@ struct UserDebugDrawArgs double m_debugLineColorRGB[3]; double m_lineWidth; - int m_debugPointNum; - double m_pointSize; - double m_lifeTime; int m_itemUniqueId; @@ -880,6 +877,9 @@ struct UserDebugDrawArgs double m_objectDebugColorRGB[3]; int m_objectUniqueId; int m_linkIndex; + + int m_debugPointNum; + double m_pointSize; }; struct UserDebugDrawResultArgs -- cgit v1.2.1