Merge pull request #3996 from wkentaro/addUserDebugPoints

Add pybullet.addUserDebugPoints

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 ec697cdbf..ae6015da4 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -4192,6 +4192,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 fb0ac3d7e..e73930b58 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -234,6 +234,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 ebbfbdc80..a7cf6d560 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1647,6 +1647,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;
 
@@ -3043,9 +3044,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();
@@ -6148,6 +6156,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 <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);
+		}
 	}
 }
 
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..341c93123 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
@@ -876,6 +877,9 @@ struct UserDebugDrawArgs
 	double m_objectDebugColorRGB[3];
 	int m_objectUniqueId;
 	int m_linkIndex;
+
+	int m_debugPointNum;
+	double m_pointSize;
 };
 
 struct UserDebugDrawResultArgs
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 52589aa59..f78f464f9 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 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,
 	 "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."},