Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

revert exposing getSharedMemoryStreamBuffer / adding to command structure

Browse Source 
use 16k rays by default
add uploadRaysToSharedMemory method
This commit is contained in:
erwincoumans
2018-06-16 08:14:00 -07:00
parent 23f9250e04
commit cb1fce7899
14 changed files with 147 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/SharedMemory/PhysicsClient.h
View File

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

53
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
rayDataStream[0].m_rayFromPosition[1] = rayFromWorldY;
rayDataStream[0].m_rayFromPosition[2] = rayFromWorldZ;
rayDataStream[0].m_rayToPosition[0] = rayToWorldX;
rayDataStream[0].m_rayToPosition[1] = rayToWorldY;
rayDataStream[0].m_rayToPosition[2] = rayToWorldZ;
double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ};
double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ};
cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1);
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;
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++;
}
cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1);
}
}
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)
{

3
examples/SharedMemory/PhysicsClientC_API.h
View File

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

29
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

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

3
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

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

34
examples/SharedMemory/PhysicsDirect.cpp
View File

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

3
examples/SharedMemory/PhysicsDirect.h
View File

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

8
examples/SharedMemory/PhysicsLoopBack.cpp
View File

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

3
examples/SharedMemory/PhysicsLoopBack.h
View File

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

8
examples/SharedMemory/SharedMemoryCommands.h
View File

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

11
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

43
examples/pybullet/examples/batchRayCast.py
View File

@@ -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")
for i in range (10):
if (not useGui):
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 (results[i][0]<0):
# p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
# else:
# p.addUserDebugLine(rayFrom[i],rayTo[i], rayHitColor)
if (useGui):
p.removeAllUserDebugItems()
for i in range (numRays):
hitObjectUid=results[i][0]
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)

4
examples/pybullet/pybullet.c
View File

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

7
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

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