Add pybullet.addUserDebugPoints

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)
 	{
 	}

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;

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;

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

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"

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)
 {

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

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

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)
 {

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

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

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