Skip to content

Commit

Permalink
Add pybullet.addUserDebugPoints
Browse files Browse the repository at this point in the history
  • Loading branch information
wkentaro committed Oct 16, 2021
1 parent 48dc1c4 commit 9e73df2
Show file tree
Hide file tree
Showing 17 changed files with 368 additions and 50 deletions.
5 changes: 5 additions & 0 deletions examples/CommonInterfaces/CommonGUIHelperInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -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; }

Expand Down Expand Up @@ -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)
{
}
Expand Down
1 change: 1 addition & 0 deletions examples/CommonInterfaces/CommonRenderInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
22 changes: 14 additions & 8 deletions examples/OpenGLWindow/GLInstancingRenderer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion examples/OpenGLWindow/Shaders/linesVS.glsl
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ out vec4 colourV;

void main (void)
{
colourV = colour;
colourV = (colour[3] == -1) ? colourIn : colour;
gl_Position = ProjectionMatrix * ModelViewMatrix * position;

}
3 changes: 2 additions & 1 deletion examples/OpenGLWindow/Shaders/linesVS.h
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
3 changes: 3 additions & 0 deletions examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 2 additions & 0 deletions examples/OpenGLWindow/SimpleOpenGL2Renderer.h
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ class SimpleOpenGL2Renderer : public CommonRenderInterface

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();
Expand Down
38 changes: 38 additions & 0 deletions examples/SharedMemory/PhysicsClientC_API.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
1 change: 1 addition & 0 deletions examples/SharedMemory/PhysicsClientC_API.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
52 changes: 52 additions & 0 deletions examples/SharedMemory/PhysicsServerCommandProcessor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -6073,6 +6081,50 @@ 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));
for (int i = 0; i < pointNum; i++) {
pointPositions[i] = pointPositionsUpload[i];
pointPositions[i + 1] = pointPositionsUpload[i + 1];
pointPositions[i + 2] = pointPositionsUpload[i + 2];
}
m_data->m_debugPointsDatas.push_back(pointPositions);

double* pointColorsUpload = (double*)(bufferServerToClient + pointNum * 3 * sizeof(double));
double* pointColors = (double*)malloc(pointNum * 3 * sizeof(double));
for (int i = 0; i < pointNum; i++) {
pointColors[i] = pointColorsUpload[i];
pointColors[i + 1] = pointColorsUpload[i + 1];
pointColors[i + 2] = pointColorsUpload[i + 2];
}
m_data->m_debugPointsDatas.push_back(pointColors);

int uid = m_data->m_guiHelper->addUserDebugPoints(
&pointPositions[0],
&pointColors[0],
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();
Expand Down
Loading

0 comments on commit 9e73df2

Please sign in to comment.