From 75ec8f3fd9d26b1327fe5f6cc18cad12254d4a02 Mon Sep 17 00:00:00 2001
From: erwincoumans <erwin.coumans@gmail.com>
Date: Sat, 16 Jun 2018 06:20:15 -0700
Subject: [PATCH] PyBullet: add batchRayCast.py example

---
 examples/pybullet/examples/batchRayCast.py | 44 ++++++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100644 examples/pybullet/examples/batchRayCast.py

diff --git a/examples/pybullet/examples/batchRayCast.py b/examples/pybullet/examples/batchRayCast.py
new file mode 100644
index 000000000..1862c6918
--- /dev/null
+++ b/examples/pybullet/examples/batchRayCast.py
@@ -0,0 +1,44 @@
+import pybullet as p
+import time
+import math
+
+p.connect(p.DIRECT)#GUI)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+p.loadURDF("samurai.urdf")
+p.loadURDF("r2d2.urdf",[3,3,1])
+
+
+rayFrom=[]
+rayTo=[]
+
+numRays = 2048
+rayLen = 13
+
+
+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])
+
+timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
+for i in range (10):
+	p.stepSimulation()
+	for j in range (8):
+		results = p.rayTestBatch(rayFrom,rayTo,j+1)
+	
+	for i in range (10):
+		p.removeAllUserDebugItems()	
+		p.removeAllUserDebugItems()	
+		
+	
+	rayHitColor = [1,0,0]
+	rayMissColor = [0,1,0]
+	#for i in range (numRays):
+	#	if (results[i][0]<0):
+	#		p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
+	#	else:
+	#		p.addUserDebugLine(rayFrom[i],rayTo[i], rayHitColor)
+		
+	#time.sleep(1./240.)
+	
+p.stopStateLogging(timingLog)
\ No newline at end of file