revert exposing getSharedMemoryStreamBuffer / adding to command structure

use 16k rays by default
add uploadRaysToSharedMemory method

examples/SharedMemory/PhysicsClient.h

@@ -43,8 +43,9 @@ public:
     virtual void setSharedMemoryKey(int key) = 0;
 
     virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
-    virtual char* getSharedMemoryStreamBuffer() = 0;
+	
-
+	virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) = 0;
+	
     virtual int getNumDebugLines() const = 0;
 
     virtual const float* getDebugLinesFrom() const = 0;

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2736,16 +2736,13 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
     struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
     b3Assert(command);
     command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
-	command->m_requestRaycastIntersections.m_numRays = 1;
+	command->m_requestRaycastIntersections.m_numRays = 0;
 	command->m_requestRaycastIntersections.m_numThreads = 1;
-	b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
+	
-	rayDataStream[0].m_rayFromPosition[0] = rayFromWorldX;
+	double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ};
-	rayDataStream[0].m_rayFromPosition[1] = rayFromWorldY;
+	double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ};
-	rayDataStream[0].m_rayFromPosition[2] = rayFromWorldZ;
+	cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1);
-	rayDataStream[0].m_rayToPosition[0] = rayToWorldX;
+	
-	rayDataStream[0].m_rayToPosition[1] = rayToWorldY;
-	rayDataStream[0].m_rayToPosition[2] = rayToWorldZ;
-
     return (b3SharedMemoryCommandHandle)command;
 }
 
@@ -2770,31 +2767,37 @@ B3_SHARED_API  void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm
 	command->m_requestRaycastIntersections.m_numThreads = numThreads;
 }
 
-B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
+B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
 {
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
 	b3Assert(command);
 	b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
-	b3Assert(command->m_client)
+
-	PhysicsClient *cl = command->m_client;
-	b3Assert(cl);
 	if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
 	{
-		int numRays = command->m_requestRaycastIntersections.m_numRays;
+		cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1);
-		if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
-		{
-			b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
-			rayDataStream[numRays].m_rayFromPosition[0] = rayFromWorld[0];
-			rayDataStream[numRays].m_rayFromPosition[1] = rayFromWorld[1];
-			rayDataStream[numRays].m_rayFromPosition[2] = rayFromWorld[2];
-			rayDataStream[numRays].m_rayToPosition[0] = rayToWorld[0];
-			rayDataStream[numRays].m_rayToPosition[1] = rayToWorld[1];
-			rayDataStream[numRays].m_rayToPosition[2] = rayToWorld[2];
-			command->m_requestRaycastIntersections.m_numRays++;
-		}
 	}
 }
 
+B3_SHARED_API	void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
+	b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
+	if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
+	{
+		cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);
+	}
+}
+
+
 
 B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -544,7 +544,8 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
 B3_SHARED_API	b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
 // Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use.
 B3_SHARED_API  void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads);
-B3_SHARED_API	void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
+B3_SHARED_API	void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
+B3_SHARED_API	void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays);
 
 B3_SHARED_API	void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1671,7 +1671,6 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
 struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
     static int sequence = 0;
     m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
-    m_data->m_testBlock1->m_clientCommands[0].m_client = this;
     return &m_data->m_testBlock1->m_clientCommands[0];
 }
 
@@ -1691,9 +1690,6 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
     return false;
 }
 
-char* PhysicsClientSharedMemory::getSharedMemoryStreamBuffer() {
-  return m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
-}
 
 void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) {
     btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
@@ -1709,6 +1705,31 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
     }
 }
 
+void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
+{
+	int curNumRays = command.m_requestRaycastIntersections.m_numRays;
+	int newNumRays = curNumRays + numRays;
+	btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
+
+	if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
+	{
+		for (int i=0;i<numRays;i++)
+		{
+			b3RayData* rayDataStream = (b3RayData *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+			rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
+			rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
+			rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
+			rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
+			rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
+			rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
+			command.m_requestRaycastIntersections.m_numRays++;
+		}
+
+	}
+}
+
+
+
 void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
 {
 	cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -54,7 +54,8 @@ public:
     virtual void setSharedMemoryKey(int key);
 
     virtual void uploadBulletFileToSharedMemory(const char* data, int len);
-    virtual char* getSharedMemoryStreamBuffer();
+    
+	virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
 
     virtual int getNumDebugLines() const;
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -112,7 +112,7 @@ PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool pas
 	m_data = new PhysicsDirectInternalData;
 	m_data->m_commandProcessor = physSdk;
 	m_data->m_ownsCommandProcessor = passSdkOwnership;
-	m_data->m_command.m_client = this;
+
 }
 
 PhysicsDirect::~PhysicsDirect()
@@ -1330,16 +1330,11 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn
     return false;
 }
 
-///todo: move this out of the
+
 void PhysicsDirect::setSharedMemoryKey(int key)
 {
-	//m_data->m_physicsServer->setSharedMemoryKey(key);
-	//m_data->m_physicsClient->setSharedMemoryKey(key);
 }
 
-char* PhysicsDirect::getSharedMemoryStreamBuffer() {
-	return m_data->m_bulletStreamDataServerToClient;
-}
 
 void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
 {
@@ -1354,6 +1349,31 @@ void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
 	//m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
 }
 
+void PhysicsDirect::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
+{
+	int curNumRays = command.m_requestRaycastIntersections.m_numRays;
+	int newNumRays = curNumRays + numRays;
+	btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
+
+	if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
+	{
+		for (int i=0;i<numRays;i++)
+		{
+			b3RayData* rayDataStream = (b3RayData *)m_data->m_bulletStreamDataServerToClient;
+			rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
+			rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
+			rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
+			rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
+			rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
+			rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
+			command.m_requestRaycastIntersections.m_numRays++;
+		}
+
+	}
+
+}
+
+
 int PhysicsDirect::getNumDebugLines() const
 {
 	return m_data->m_debugLinesFrom.size();

examples/SharedMemory/PhysicsDirect.h

@@ -78,7 +78,8 @@ public:
     virtual void setSharedMemoryKey(int key);
 
     void uploadBulletFileToSharedMemory(const char* data, int len);
-    virtual char* getSharedMemoryStreamBuffer();
+    
+	virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
 
     virtual int getNumDebugLines() const;
 

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -140,15 +140,17 @@ void PhysicsLoopBack::setSharedMemoryKey(int key)
 	m_data->m_physicsClient->setSharedMemoryKey(key);
 }
 
-char* PhysicsLoopBack::getSharedMemoryStreamBuffer() {
-	return m_data->m_physicsClient->getSharedMemoryStreamBuffer();
-}
 
 void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len)
 {
 	m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
 }
 
+void PhysicsLoopBack::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
+{
+	m_data->m_physicsClient->uploadRaysToSharedMemory(command, rayFromWorldArray, rayToWorldArray, numRays);
+}
+
 int PhysicsLoopBack::getNumDebugLines() const
 {
 	return m_data->m_physicsClient->getNumDebugLines();

examples/SharedMemory/PhysicsLoopBack.h

@@ -58,7 +58,8 @@ public:
     virtual void setSharedMemoryKey(int key);
 
     void uploadBulletFileToSharedMemory(const char* data, int len);
-    virtual char* getSharedMemoryStreamBuffer();
+	
+	virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
 
     virtual int getNumDebugLines() const;
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -23,6 +23,12 @@
 	typedef unsigned long long int smUint64_t;
 #endif
 
+#ifdef __APPLE__
+    #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024)
+#else
+    #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
+#endif
+
 #define SHARED_MEMORY_SERVER_TEST_C
 #define MAX_DEGREE_OF_FREEDOM 128
 #define MAX_NUM_SENSORS 256
@@ -1011,7 +1017,7 @@ struct SharedMemoryCommand
 	int m_type;
 	smUint64_t	m_timeStamp;
 	int	m_sequenceNumber;
-	struct PhysicsClient *m_client;
+	
 	
 	//m_updateFlags is a bit fields to tell which parameters need updating
     //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;

examples/SharedMemory/SharedMemoryPublic.h

@@ -4,8 +4,11 @@
 #define SHARED_MEMORY_KEY 12347
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
+//Please don't replace an existing magic number:
+//instead, only ADD a new one at the top, comment-out previous one
 
 #define SHARED_MEMORY_MAGIC_NUMBER 201806150
+//#define SHARED_MEMORY_MAGIC_NUMBER 201806020
 //#define SHARED_MEMORY_MAGIC_NUMBER 201801170
 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080
 //#define SHARED_MEMORY_MAGIC_NUMBER 201801010
@@ -17,11 +20,7 @@
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001
 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024
 
-#ifdef __APPLE__
+
-    #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024)
-#else
-    #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
-#endif
 
 enum EnumSharedMemoryClientCommand
 {
@@ -583,7 +582,7 @@ typedef union {
     struct b3RayData a;
     struct b3RayHitInfo b;
 } RAY_DATA_UNION;
-#define MAX_RAY_INTERSECTION_BATCH_SIZE SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE / sizeof( RAY_DATA_UNION )
+#define MAX_RAY_INTERSECTION_BATCH_SIZE 16*1024
 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
 #define VISUAL_SHAPE_MAX_PATH_LEN 1024
 

examples/pybullet/examples/batchRayCast.py

@@ -2,9 +2,15 @@ import pybullet as p
 import time
 import math
 
-p.connect(p.DIRECT)#GUI)
+useGui = True
+
+if (useGui):
+	p.connect(p.GUI)
+else:
+	p.connect(p.DIRECT)
+
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
 
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
 p.loadURDF("samurai.urdf")
 p.loadURDF("r2d2.urdf",[3,3,1])
 
@@ -12,7 +18,7 @@ p.loadURDF("r2d2.urdf",[3,3,1])
 rayFrom=[]
 rayTo=[]
 
-numRays = 2048
+numRays = 1024
 rayLen = 13
 
 
@@ -20,25 +26,38 @@ 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")
+if (not useGui):
-for i in range (10):
+	timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
+
+numSteps = 10
+if (useGui):
+	numSteps = 327680
+
+for i in range (numSteps):
 	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 (useGui):
-	#	if (results[i][0]<0):
+		p.removeAllUserDebugItems()	
-	#		p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
+		for i in range (numRays):
-	#	else:
+			hitObjectUid=results[i][0]
-	#		p.addUserDebugLine(rayFrom[i],rayTo[i], rayHitColor)
+			
+			
+			if (hitObjectUid<0):
+				p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
+			else:
+				hitPosition = results[i][3]
+				p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor)
 		
 	#time.sleep(1./240.)
 	
-p.stopStateLogging(timingLog)
+if (not useGui):
+	p.stopStateLogging(timingLog)

examples/pybullet/pybullet.c

@@ -4729,7 +4729,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 			} else
 			{
 				int i;
-
+				
 				if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE)
 				{
 					PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
@@ -4748,7 +4748,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 					if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
 						(pybullet_internalSetVectord(rayToObj, rayToWorld)))
 					{
-						b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);	
+						b3RaycastBatchAddRay(sm, commandHandle, rayFromWorld, rayToWorld);	
 					} else
 					{
 						PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "btSequentialImpulseConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
+
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btCpuFeatureUtility.h"
 
@@ -780,8 +781,12 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
     }
     else
     {
+		bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK);
         // Incorrectly set collision object flags can degrade performance in various ways.
-        //btAssert( body.isStaticOrKinematicObject() );
+		if (!isMultiBodyType)
+		{
+			btAssert( body.isStaticOrKinematicObject() );
+		}
         //it could be a multibody link collider
         // all fixed bodies (inf mass) get mapped to a single solver id
         if ( m_fixedBodyId < 0 )