PyBullet: allow to replace existing debug lines in addUserDebugLine through the replaceItemUniqueId argument

See batchRayCast.py for example usage

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -103,7 +103,7 @@ struct GUIHelperInterface
 	virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag){}
 	
 	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){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		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue){return -1;};
 	virtual int		readUserDebugParameter(int itemUniqueId, double* value) { return 0;}
 

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -75,7 +75,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	{
 		return -1;
 	}
-	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	
+	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;
 	}

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3063,7 +3063,6 @@ B3_SHARED_API void b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHa
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
 	b3Assert(command);
 	b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
-	b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT);
 	command->m_userDebugDrawArgs.m_replaceItemUniqueId = replaceItemUniqueId;
 	command->m_updateFlags |= USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID;
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4582,13 +4582,21 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha
 
 	if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE)
 	{
+		int replaceItemUid = -1;
+		if (clientCmd.m_updateFlags&USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)
+		{
+			replaceItemUid = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId;
+		}
+		
 		int uid = m_data->m_guiHelper->addUserDebugLine(
 			clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ,
 			clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ,
 			clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB,
 			clientCmd.m_userDebugDrawArgs.m_lineWidth,
 			clientCmd.m_userDebugDrawArgs.m_lifeTime,
-			trackingVisualShapeIndex);
+			trackingVisualShapeIndex,
+			replaceItemUid
+			);
 
 		if (uid>=0)
 		{

examples/SharedMemory/PhysicsServerExample.cpp

@@ -515,6 +515,7 @@ struct UserDebugDrawLine
 	double m_lifeTime;
 	int m_itemUniqueId;
 	int m_trackingVisualShapeIndex;
+	int m_replaceItemUid;
 };
 
 struct UserDebugParameter
@@ -1275,11 +1276,14 @@ public:
 	btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
 	UserDebugDrawLine m_tmpLine;
 	int m_resultDebugLineUid;
-	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex)
+	
+	
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid)
 	{
 		m_tmpLine.m_lifeTime = lifeTime;
 		m_tmpLine.m_lineWidth = lineWidth;
-		m_tmpLine.m_itemUniqueId = m_uidGenerator++;
+		
+		m_tmpLine.m_itemUniqueId = replaceItemUid<0? m_uidGenerator++ : replaceItemUid;
 		m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
 		m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
 		m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
@@ -1292,6 +1296,7 @@ public:
 		m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
 		m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
 		m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
+		m_tmpLine.m_replaceItemUid = replaceItemUid;
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIUserDebugAddLine);
         m_resultDebugLineUid=-1;
@@ -2365,8 +2370,21 @@ void	PhysicsServerExample::updateGraphics()
 	{
 		B3_PROFILE("eGUIUserDebugAddLine");
 
+		if (m_multiThreadedHelper->m_tmpLine.m_replaceItemUid>=0)
+		{
+			for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
+			{
+				if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpLine.m_replaceItemUid)
+				{
+					m_multiThreadedHelper->m_userDebugLines[i] = m_multiThreadedHelper->m_tmpLine;
+					m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_tmpLine.m_replaceItemUid;
+				}
+			}
+		} else
+		{
 			m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
 			m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId;
+		}
 		m_multiThreadedHelper->mainThreadRelease();
 		break;
 	}

examples/pybullet/examples/batchRayCast.py

@@ -9,22 +9,34 @@ if (useGui):
 else:
 	p.connect(p.DIRECT)
 
+p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
 #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
 
-p.loadURDF("samurai.urdf")
+#p.loadURDF("samurai.urdf")
 p.loadURDF("r2d2.urdf",[3,3,1])
 
 
 rayFrom=[]
 rayTo=[]
+rayIds=[]
 
 numRays = 1024
+	
 rayLen = 13
 
 
+rayHitColor = [1,0,0]
+rayMissColor = [0,1,0]
+
+replaceLines = True
+	
 for i in range (numRays):
 	rayFrom.append([0,0,1])
 	rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1])
+	if (replaceLines):
+		rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
+	else:
+		rayIds.append(-1)
 
 if (not useGui):
 	timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
@@ -38,24 +50,26 @@ for i in range (numSteps):
 	for j in range (8):
 		results = p.rayTestBatch(rayFrom,rayTo,j+1)
 	
-	for i in range (10):
+	#for i in range (10):
-		p.removeAllUserDebugItems()	
+	#	p.removeAllUserDebugItems()	
+		
 		
 	
 	
-	rayHitColor = [1,0,0]
-	rayMissColor = [0,1,0]
 	if (useGui):
+		if (not replaceLines):
 			p.removeAllUserDebugItems()
+			
 		for i in range (numRays):
 			hitObjectUid=results[i][0]
 			
 			
 			if (hitObjectUid<0):
-				p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
+				hitPosition =[0,0,0]
+				p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i])
 			else:
 				hitPosition = results[i][3]
-				p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor)
+				p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor,replaceItemUniqueId=rayIds[i])
 		
 	#time.sleep(1./240.)
 	

examples/pybullet/pybullet.c

@@ -4261,10 +4261,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 	double lifeTime = 0.f;
 	int physicsClientId = 0;
 	int debugItemUniqueId = -1;
+	int replaceItemUniqueId = -1;
 	b3PhysicsClientHandle sm = 0;
-	static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
+	static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "replaceItemUniqueId", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &replaceItemUniqueId, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -4293,6 +4294,7 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 		res = pybullet_internalSetVectord(lineColorRGBObj, colorRGB);
 	}
 
+	
 	commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime);
 
 	if (parentObjectUniqueId>=0)
@@ -4300,6 +4302,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 		b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex);
 	}
 
+	if (replaceItemUniqueId>=0)
+	{
+		b3UserDebugItemSetReplaceItemUniqueId(commandHandle,replaceItemUniqueId);
+	}
+
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
 	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)