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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwincoumans
2018-06-16 06:06:18 -07:00
parent fbbd675ed6 5b3e43d4d8
commit bb8cbcdaae
14 changed files with 226 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/SharedMemory/PhysicsClient.h
View File

@@ -43,6 +43,7 @@ public:
virtual void setSharedMemoryKey(int key) = 0; virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
virtual char* getSharedMemoryStreamBuffer() = 0;
virtual int getNumDebugLines() const = 0; virtual int getNumDebugLines() const = 0;

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2737,12 +2737,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
b3Assert(command); b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_numRays = 1; command->m_requestRaycastIntersections.m_numRays = 1;
command->m_requestRaycastIntersections.m_rayFromPositions[0][0] = rayFromWorldX; command->m_requestRaycastIntersections.m_numThreads = 1;
command->m_requestRaycastIntersections.m_rayFromPositions[0][1] = rayFromWorldY; b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
command->m_requestRaycastIntersections.m_rayFromPositions[0][2] = rayFromWorldZ; rayDataStream[0].m_rayFromPosition[0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_rayToPositions[0][0] = rayToWorldX; rayDataStream[0].m_rayFromPosition[1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_rayToPositions[0][1] = rayToWorldY; rayDataStream[0].m_rayFromPosition[2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_rayToPositions[0][2] = rayToWorldZ; rayDataStream[0].m_rayToPosition[0] = rayToWorldX;
rayDataStream[0].m_rayToPosition[1] = rayToWorldY;
rayDataStream[0].m_rayToPosition[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command; return (b3SharedMemoryCommandHandle)command;
} }
@@ -2757,25 +2759,37 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_requestRaycastIntersections.m_numRays = 0; command->m_requestRaycastIntersections.m_numRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1;
return (b3SharedMemoryCommandHandle)command; return (b3SharedMemoryCommandHandle)command;
} }
B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads) {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
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(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); 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) if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{ {
int numRays = command->m_requestRaycastIntersections.m_numRays; int numRays = command->m_requestRaycastIntersections.m_numRays;
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE) if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{ {
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][0] = rayFromWorld[0]; b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][1] = rayFromWorld[1]; rayDataStream[numRays].m_rayFromPosition[0] = rayFromWorld[0];
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][2] = rayFromWorld[2]; rayDataStream[numRays].m_rayFromPosition[1] = rayFromWorld[1];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][0] = rayToWorld[0]; rayDataStream[numRays].m_rayFromPosition[2] = rayFromWorld[2];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][1] = rayToWorld[1]; rayDataStream[numRays].m_rayToPosition[0] = rayToWorld[0];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][2] = rayToWorld[2]; rayDataStream[numRays].m_rayToPosition[1] = rayToWorld[1];
rayDataStream[numRays].m_rayToPosition[2] = rayToWorld[2];
command->m_requestRaycastIntersections.m_numRays++; command->m_requestRaycastIntersections.m_numRays++;
} }
} }

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -542,6 +542,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY, double rayToWorldZ);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); 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(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);

8
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -955,9 +955,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Printf("Raycast completed"); b3Printf("Raycast completed");
} }
m_data->m_raycastHits.clear(); m_data->m_raycastHits.clear();
b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++) for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
{ {
m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]); m_data->m_raycastHits.push_back(rayHits[i]);
} }
break; break;
} }
@@ -1664,6 +1665,7 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
static int sequence = 0; static int sequence = 0;
m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; 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]; return &m_data->m_testBlock1->m_clientCommands[0];
} }
@@ -1683,6 +1685,10 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
return false; return false;
} }
char* PhysicsClientSharedMemory::getSharedMemoryStreamBuffer() {
return m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
}
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) {
btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) {

1
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -54,6 +54,7 @@ public:
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len); virtual void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual int getNumDebugLines() const; virtual int getNumDebugLines() const;

8
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -106,6 +106,7 @@ PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool pas
m_data = new PhysicsDirectInternalData; m_data = new PhysicsDirectInternalData;
m_data->m_commandProcessor = physSdk; m_data->m_commandProcessor = physSdk;
m_data->m_ownsCommandProcessor = passSdkOwnership; m_data->m_ownsCommandProcessor = passSdkOwnership;
m_data->m_command.m_client = this;
} }
PhysicsDirect::~PhysicsDirect() PhysicsDirect::~PhysicsDirect()
@@ -712,9 +713,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
b3Printf("Raycast completed"); b3Printf("Raycast completed");
} }
m_data->m_raycastHits.clear(); m_data->m_raycastHits.clear();
b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_bulletStreamDataServerToClient;
for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++) for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
{ {
m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]); m_data->m_raycastHits.push_back(rayHits[i]);
} }
break; break;
} }
@@ -1323,6 +1325,10 @@ void PhysicsDirect::setSharedMemoryKey(int key)
//m_data->m_physicsClient->setSharedMemoryKey(key); //m_data->m_physicsClient->setSharedMemoryKey(key);
} }
char* PhysicsDirect::getSharedMemoryStreamBuffer() {
return m_data->m_bulletStreamDataServerToClient;
}
void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
{ {
if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)

1
examples/SharedMemory/PhysicsDirect.h
View File

@@ -78,6 +78,7 @@ public:
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len); void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual int getNumDebugLines() const; virtual int getNumDebugLines() const;

4
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -140,6 +140,10 @@ void PhysicsLoopBack::setSharedMemoryKey(int key)
m_data->m_physicsClient->setSharedMemoryKey(key); m_data->m_physicsClient->setSharedMemoryKey(key);
} }
char* PhysicsLoopBack::getSharedMemoryStreamBuffer() {
return m_data->m_physicsClient->getSharedMemoryStreamBuffer();
}
void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len) void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len)
{ {
m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);

1
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -58,6 +58,7 @@ public:
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len); void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual int getNumDebugLines() const; virtual int getNumDebugLines() const;

171
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1780,6 +1780,13 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
if (btGetTaskScheduler() == 0) {
btITaskScheduler *scheduler = btCreateDefaultTaskScheduler();
if (scheduler == 0) {
scheduler = btGetSequentialTaskScheduler();
}
btSetTaskScheduler(scheduler);
}
} }
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -4681,31 +4688,87 @@ bool PhysicsServerCommandProcessor::processRequestKeyboardEventsCommand(const st
return hasStatus; return hasStatus;
} }
bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
for (int ray=0;ray<clientCmd.m_requestRaycastIntersections.m_numRays;ray++) #if __cplusplus >= 201103L
#include <atomic>
struct CastSyncInfo {
std::atomic<int> m_nextTaskNumber;
CastSyncInfo() : m_nextTaskNumber(0) {}
inline int getNextTask() {
return m_nextTaskNumber++;
}
};
#else // __cplusplus >= 201103L
struct CastSyncInfo {
volatile int m_nextTaskNumber;
btSpinMutex m_taskLock;
CastSyncInfo() : m_nextTaskNumber(0) {}
inline int getNextTask() {
m_taskLock.lock();
const int taskNr = m_nextTaskNumber++;
m_taskLock.unlock();
return taskNr;
}
};
#endif // __cplusplus >= 201103L
struct BatchRayCaster : public btIParallelForBody
{
CastSyncInfo *m_syncInfo;
const btCollisionWorld *m_world;
const b3RayData *m_rayInputBuffer;
b3RayHitInfo *m_hitInfoOutputBuffer;
int m_numRays;
BatchRayCaster(const btCollisionWorld* world, const b3RayData *rayInputBuffer, b3RayHitInfo *hitInfoOutputBuffer, int numRays)
: m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays) {
m_syncInfo = new CastSyncInfo;
}
~BatchRayCaster() {
delete m_syncInfo;
}
void castRays(int numWorkers) {
#if BT_THREADSAFE
btParallelFor(0, numWorkers, 1, *this);
#else // BT_THREADSAFE
for (int i = 0; i < m_numRays; i++) {
processRay(i);
}
#endif // BT_THREADSAFE
}
void forLoop( int iBegin, int iEnd ) const
{ {
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][0], while(true) {
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][1], const int taskNr = m_syncInfo->getNextTask();
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][2]); if (taskNr >= m_numRays)
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][0], return;
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][1], processRay(taskNr);
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][2]); }
}
void processRay(int ray) const {
const double *from = m_rayInputBuffer[ray].m_rayFromPosition;
const double *to = m_rayInputBuffer[ray].m_rayToPosition;
btVector3 rayFromWorld(from[0], from[1], from[2]);
btVector3 rayToWorld(to[0], to[1], to[2]);
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld); btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); m_world->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits;
b3RayHitInfo& hit = m_hitInfoOutputBuffer[ray];
if (rayResultCallback.hasHit()) if (rayResultCallback.hasHit())
{ {
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction hit.m_hitFraction = rayResultCallback.m_closestHitFraction;
= rayResultCallback.m_closestHitFraction;
int objectUniqueId = -1; int objectUniqueId = -1;
int linkIndex = -1; int linkIndex = -1;
@@ -4724,39 +4787,61 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
} }
} }
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId hit.m_hitObjectUniqueId = objectUniqueId;
= objectUniqueId; hit.m_hitObjectLinkIndex = linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex
= linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] hit.m_hitPositionWorld[0] = rayResultCallback.m_hitPointWorld[0];
= rayResultCallback.m_hitPointWorld[0]; hit.m_hitPositionWorld[1] = rayResultCallback.m_hitPointWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] hit.m_hitPositionWorld[2] = rayResultCallback.m_hitPointWorld[2];
= rayResultCallback.m_hitPointWorld[1]; hit.m_hitNormalWorld[0] = rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] hit.m_hitNormalWorld[1] = rayResultCallback.m_hitNormalWorld[1];
= rayResultCallback.m_hitPointWorld[2]; hit.m_hitNormalWorld[2] = rayResultCallback.m_hitNormalWorld[2];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0]
= rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1]
= rayResultCallback.m_hitNormalWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2]
= rayResultCallback.m_hitNormalWorld[2];
} else } else
{ {
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1; hit.m_hitFraction = 1;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1; hit.m_hitObjectUniqueId = -1;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1; hit.m_hitObjectLinkIndex = -1;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; hit.m_hitPositionWorld[0] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0; hit.m_hitPositionWorld[1] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0; hit.m_hitPositionWorld[2] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0; hit.m_hitNormalWorld[0] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0; hit.m_hitNormalWorld[1] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0; hit.m_hitNormalWorld[2] = 0;
} }
serverStatusOut.m_raycastHits.m_numRaycastHits++;
} }
};
bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
const int numRays = clientCmd.m_requestRaycastIntersections.m_numRays;
const int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads;
b3RayData *rayInputBuffer = (b3RayData *)malloc(sizeof(b3RayData) * numRays);
memcpy(rayInputBuffer, bufferServerToClient, sizeof(b3RayData) * numRays);
BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, rayInputBuffer, (b3RayHitInfo *)bufferServerToClient, numRays);
if (numThreads == 0) {
// When 0 is specified, Bullet can decide how many threads to use.
// About 16 rays per thread seems to work reasonably well.
batchRayCaster.castRays(numRays / 16);
} else if (numThreads == 1) {
// Sequentially trace all rays:
for (int i = 0; i < numRays; i++) {
batchRayCaster.processRay(i);
}
} else {
// Otherwise, just use the user-specified number of threads. This is
// still limited by the number of virtual cores on the machine.
batchRayCaster.castRays(numThreads);
}
free(rayInputBuffer);
serverStatusOut.m_raycastHits.m_numRaycastHits = numRays;
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
return hasStatus; return hasStatus;
} }

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -23,12 +23,6 @@
typedef unsigned long long int smUint64_t; typedef unsigned long long int smUint64_t;
#endif #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 SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 128 #define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256 #define MAX_NUM_SENSORS 256
@@ -38,6 +32,7 @@
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH
struct TmpFloat3 struct TmpFloat3
{ {
float m_x; float m_x;
@@ -281,15 +276,19 @@ enum EnumRequestContactDataUpdateFlags
struct RequestRaycastIntersections struct RequestRaycastIntersections
{ {
// The number of threads that Bullet may use to perform the ray casts.
// 0: Let Bullet decide
// 1: Use a single thread (i.e. no multi-threading)
// 2 or more: Number of threads to use.
int m_numThreads;
int m_numRays; int m_numRays;
double m_rayFromPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3]; // Actual ray data stored in m_bulletStreamDataServerToClientRefactor.
double m_rayToPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
}; };
struct SendRaycastHits struct SendRaycastHits
{ {
int m_numRaycastHits; int m_numRaycastHits;
b3RayHitInfo m_rayHits[MAX_RAY_INTERSECTION_BATCH_SIZE]; // Actual ray data stored in m_bulletStreamDataServerToClientRefactor.
}; };
struct RequestContactDataArgs struct RequestContactDataArgs
@@ -1012,6 +1011,7 @@ struct SharedMemoryCommand
int m_type; int m_type;
smUint64_t m_timeStamp; smUint64_t m_timeStamp;
int m_sequenceNumber; int m_sequenceNumber;
struct PhysicsClient *m_client;
//m_updateFlags is a bit fields to tell which parameters need updating //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; //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;

23
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,7 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev ///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201806020 #define SHARED_MEMORY_MAGIC_NUMBER 201806150
//#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801170
//#define SHARED_MEMORY_MAGIC_NUMBER 201801080 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080
//#define SHARED_MEMORY_MAGIC_NUMBER 201801010 //#define SHARED_MEMORY_MAGIC_NUMBER 201801010
@@ -17,6 +17,11 @@
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024 //#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 enum EnumSharedMemoryClientCommand
{ {
@@ -417,8 +422,7 @@ enum b3VREventType
#define MAX_VR_BUTTONS 64 #define MAX_VR_BUTTONS 64
#define MAX_VR_CONTROLLERS 8 #define MAX_VR_CONTROLLERS 8
#define MAX_RAY_INTERSECTION_BATCH_SIZE 256
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define MAX_KEYBOARD_EVENTS 256 #define MAX_KEYBOARD_EVENTS 256
#define MAX_MOUSE_EVENTS 256 #define MAX_MOUSE_EVENTS 256
@@ -553,6 +557,12 @@ struct b3ContactInformation
struct b3ContactPointData* m_contactPointData; struct b3ContactPointData* m_contactPointData;
}; };
struct b3RayData
{
double m_rayFromPosition[3];
double m_rayToPosition[3];
};
struct b3RayHitInfo struct b3RayHitInfo
{ {
double m_hitFraction; double m_hitFraction;
@@ -568,7 +578,12 @@ struct b3RaycastInformation
struct b3RayHitInfo* m_rayHits; struct b3RayHitInfo* m_rayHits;
}; };
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_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define VISUAL_SHAPE_MAX_PATH_LEN 1024 #define VISUAL_SHAPE_MAX_PATH_LEN 1024
struct b3VisualShapeData struct b3VisualShapeData

15
examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py
View File

@@ -40,6 +40,9 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
return r return r
def _isDone(self):
return self._alive < 0
def move_robot(self, init_x, init_y, init_z): def move_robot(self, init_x, init_y, init_z):
"Used by multiplayer stadium to move sideways, to another running lane." "Used by multiplayer stadium to move sideways, to another running lane."
self.cpp_robot.query_position() self.cpp_robot.query_position()
@@ -60,8 +63,8 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
state = self.robot.calc_state() # also calculates self.joints_at_limit state = self.robot.calc_state() # also calculates self.joints_at_limit
alive = float(self.robot.alive_bonus(state[0]+self.robot.initial_z, self.robot.body_rpy[1])) # state[0] is body height above ground, body_rpy[1] is pitch self._alive = float(self.robot.alive_bonus(state[0]+self.robot.initial_z, self.robot.body_rpy[1])) # state[0] is body height above ground, body_rpy[1] is pitch
done = alive < 0 done = self._isDone()
if not np.isfinite(state).all(): if not np.isfinite(state).all():
print("~INF~", state) print("~INF~", state)
done = True done = True
@@ -89,7 +92,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
debugmode=0 debugmode=0
if(debugmode): if(debugmode):
print("alive=") print("alive=")
print(alive) print(self._alive)
print("progress") print("progress")
print(progress) print(progress)
print("electricity_cost") print("electricity_cost")
@@ -100,7 +103,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
print(feet_collision_cost) print(feet_collision_cost)
self.rewards = [ self.rewards = [
alive, self._alive,
progress, progress,
electricity_cost, electricity_cost,
joints_at_limit_cost, joints_at_limit_cost,
@@ -136,6 +139,9 @@ class HalfCheetahBulletEnv(WalkerBaseBulletEnv):
self.robot = HalfCheetah() self.robot = HalfCheetah()
WalkerBaseBulletEnv.__init__(self, self.robot) WalkerBaseBulletEnv.__init__(self, self.robot)
def _isDone(self):
return False
class AntBulletEnv(WalkerBaseBulletEnv): class AntBulletEnv(WalkerBaseBulletEnv):
def __init__(self): def __init__(self):
self.robot = Ant() self.robot = Ant()
@@ -172,4 +178,3 @@ class HumanoidFlagrunHarderBulletEnv(HumanoidBulletEnv):
s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client) s = HumanoidBulletEnv.create_single_player_scene(self, bullet_client)
s.zero_at_running_strip_start_line = False s.zero_at_running_strip_start_line = False
return s return s

14
examples/pybullet/pybullet.c
View File

@@ -4683,16 +4683,17 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
int statusType; int statusType;
PyObject* rayFromObjList = 0; PyObject* rayFromObjList = 0;
PyObject* rayToObjList = 0; PyObject* rayToObjList = 0;
int numThreads = 1;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int sizeFrom = 0; int sizeFrom = 0;
int sizeTo = 0; int sizeTo = 0;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "physicsClientId", NULL}; static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "physicsClientId", NULL};
int physicsClientId = 0; int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ii", kwlist,
&rayFromObjList, &rayToObjList, &physicsClientId)) &rayFromObjList, &rayToObjList, &numThreads, &physicsClientId))
return NULL; return NULL;
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
@@ -4704,6 +4705,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
commandHandle = b3CreateRaycastBatchCommandInit(sm); commandHandle = b3CreateRaycastBatchCommandInit(sm);
b3RaycastBatchSetNumThreads(commandHandle, numThreads);
if (rayFromObjList) if (rayFromObjList)
@@ -4725,7 +4727,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
{ {
int i; int i;
if (lenFrom >= MAX_RAY_INTERSECTION_BATCH_SIZE) if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE)
{ {
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size."); PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
Py_DECREF(seqRayFromObj); Py_DECREF(seqRayFromObj);
@@ -9404,7 +9406,9 @@ static PyMethodDef SpamMethods[] = {
{"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS, {"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS,
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) " "Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"}, "Takes two required arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates) "
"and one optional argument numThreads to specify 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."},
{ "loadPlugin", (PyCFunction)pybullet_loadPlugin, METH_VARARGS | METH_KEYWORDS, { "loadPlugin", (PyCFunction)pybullet_loadPlugin, METH_VARARGS | METH_KEYWORDS,
"Load a plugin, could implement custom commands etc." }, "Load a plugin, could implement custom commands etc." },