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

work-in-progress

Browse Source 
add UDP network connection for physics client <-> server.
also set spinning friction in rolling friction demo (otherwise objects may keep on spinning forever)
This commit is contained in:
erwincoumans
2016-11-04 13:15:10 -07:00
parent c2ca88bf44
commit 9708392322
52 changed files with 2104 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -58,21 +58,25 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len<MAX_URDF_FILENAME_LENGTH)
if (cl->canSubmitCommand())
{
strcpy(command->m_urdfArguments.m_urdfFileName,urdfFileName);
} else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
}
else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle)command;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
return 0;
}
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
@@ -116,35 +120,52 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_FIXED_BASE;
command->m_urdfArguments.m_useFixedBase = useFixedBase;
return 0;
if (command && (command->m_type == CMD_LOAD_URDF))
{
command->m_updateFlags |= URDF_ARGS_USE_FIXED_BASE;
command->m_urdfArguments.m_useFixedBase = useFixedBase;
return 0;
}
return -1;
}
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_urdfArguments.m_initialPosition[0] = startPosX;
command->m_urdfArguments.m_initialPosition[1] = startPosY;
command->m_urdfArguments.m_initialPosition[2] = startPosZ;
command->m_updateFlags|=URDF_ARGS_INITIAL_POSITION;
return 0;
if (command)
{
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command->m_type == CMD_LOAD_URDF)
{
command->m_urdfArguments.m_initialPosition[0] = startPosX;
command->m_urdfArguments.m_initialPosition[1] = startPosY;
command->m_urdfArguments.m_initialPosition[2] = startPosZ;
command->m_updateFlags |= URDF_ARGS_INITIAL_POSITION;
}
return 0;
}
return -1;
}
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_urdfArguments.m_initialOrientation[0] = startOrnX;
command->m_urdfArguments.m_initialOrientation[1] = startOrnY;
command->m_urdfArguments.m_initialOrientation[2] = startOrnZ;
command->m_urdfArguments.m_initialOrientation[3] = startOrnW;
command->m_updateFlags|=URDF_ARGS_INITIAL_ORIENTATION;
return 0;
if (command)
{
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command->m_type == CMD_LOAD_URDF)
{
command->m_urdfArguments.m_initialOrientation[0] = startOrnX;
command->m_urdfArguments.m_initialOrientation[1] = startOrnY;
command->m_urdfArguments.m_initialOrientation[2] = startOrnZ;
command->m_urdfArguments.m_initialOrientation[3] = startOrnW;
command->m_updateFlags |= URDF_ARGS_INITIAL_ORIENTATION;
}
return 0;
}
return -1;
}
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
@@ -174,7 +195,7 @@ int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandH
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_allowRealTimeSimulation = enableRealTimeSimulation;
command->m_physSimParamArgs.m_allowRealTimeSimulation = (enableRealTimeSimulation!=0);
command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
return 0;
}
@@ -621,20 +642,11 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in
}
b3PhysicsClientHandle b3ConnectSharedMemory(int key)
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
///client should never create shared memory, only the server does
cl->setSharedMemoryKey(key);
cl->connect();
return (b3PhysicsClientHandle ) cl;
}
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->disconnectSharedMemory();
delete cl;
}
@@ -772,22 +784,34 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
PhysicsClient* cl = (PhysicsClient* ) physClient;
return (int)cl->submitClientCommand(*command);
b3Assert(command);
b3Assert(cl);
if (command && cl)
{
return (int)cl->submitClientCommand(*command);
}
return -1;
}
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
int timeout = 1024*1024*1024;
b3SharedMemoryStatusHandle statusHandle=0;
b3SubmitClientCommand(physClient,commandHandle);
while ((statusHandle==0) && (timeout-- > 0))
{
statusHandle =b3ProcessServerStatus(physClient);
}
return (b3SharedMemoryStatusHandle) statusHandle;
int timeout = 1024 * 1024 * 1024;
b3SharedMemoryStatusHandle statusHandle = 0;
b3Assert(commandHandle);
b3Assert(physClient);
if (physClient && commandHandle)
{
b3SubmitClientCommand(physClient, commandHandle);
while ((statusHandle == 0) && (timeout-- > 0))
{
statusHandle = b3ProcessServerStatus(physClient);
}
return (b3SharedMemoryStatusHandle)statusHandle;
}
return 0;
}
@@ -1546,4 +1570,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
}
return true;
}
}

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -11,14 +11,18 @@ B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle);
B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
///There are several connection methods, see following header files:
#include "PhysicsClientSharedMemory_C_API.h"
#include "PhysicsClientSharedMemory2_C_API.h"
#include "PhysicsDirectC_API.h"
#include "PhysicsClientUDP_C_API.h"
#include "SharedMemoryInProcessPhysicsC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
///b3ConnectSharedMemory will connect to a physics server over shared memory, so
///make sure to start the server first.
///and a way to spawn an OpenGL 3D GUI physics server and connect (b3CreateInProcessPhysicsServerAndConnect)
b3PhysicsClientHandle b3ConnectSharedMemory(int key);
///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory.
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);

14
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -124,8 +124,11 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
return true;
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
{
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
}
return false;
}
@@ -210,7 +213,7 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
serverCmd.m_numDataStreamBytes);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
@@ -291,16 +294,17 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_URDF_LOADING_COMPLETED: {
if (m_data->m_verboseOutput) {
b3Printf("Server loading the URDF OK\n");
}
if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) {
if (serverCmd.m_numDataStreamBytes > 0) {
bParse::btBulletFile bf(
this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
serverCmd.m_dataStreamArguments.m_streamChunkLength);
serverCmd.m_numDataStreamBytes);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -12,7 +12,7 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsClientSharedMemory();

27
examples/SharedMemory/PhysicsClientSharedMemory2.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include "PhysicsClientSharedMemory2.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3Scalar.h"
#include "SharedMemoryCommandProcessor.h"
PhysicsClientSharedMemory2::PhysicsClientSharedMemory2(SharedMemoryCommandProcessor* proc)
:PhysicsDirect(proc)
{
m_proc = proc;
}
PhysicsClientSharedMemory2::~PhysicsClientSharedMemory2()
{
}
void PhysicsClientSharedMemory2::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
{
if (m_proc)
{
m_proc->setSharedMemoryInterface(sharedMem);
}
}

18
examples/SharedMemory/PhysicsClientSharedMemory2.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef PHYSICS_CLIENT_SHARED_MEMORY2_H
#define PHYSICS_CLIENT_SHARED_MEMORY2_H
#include "PhysicsDirect.h"
class PhysicsClientSharedMemory2 : public PhysicsDirect
{
class SharedMemoryCommandProcessor* m_proc;
public:
PhysicsClientSharedMemory2(SharedMemoryCommandProcessor* proc);
virtual ~PhysicsClientSharedMemory2();
void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
};
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H

19
examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include "PhysicsClientSharedMemory2_C_API.h"
#include "PhysicsDirect.h"
#include "SharedMemoryCommandProcessor.h"
b3PhysicsClientHandle b3ConnectSharedMemory2(int key)
{
SharedMemoryCommandProcessor* cmdProc = new SharedMemoryCommandProcessor();
cmdProc->setSharedMemoryKey(key);
PhysicsDirect* cl = new PhysicsDirect(cmdProc);
cl->setSharedMemoryKey(key);
cl->connect();
return (b3PhysicsClientHandle)cl;
}

18
examples/SharedMemory/PhysicsClientSharedMemory2_C_API.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef PHYSICS_CLIENT_SHARED_MEMORY2_H
#define PHYSICS_CLIENT_SHARED_MEMORY2_H
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
b3PhysicsClientHandle b3ConnectSharedMemory2(int key);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H

11
examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp Normal file
View File

@@ -0,0 +1,11 @@
#include "PhysicsClientSharedMemory_C_API.h"
#include "PhysicsClientSharedMemory.h"
b3PhysicsClientHandle b3ConnectSharedMemory(int key)
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
cl->setSharedMemoryKey(key);
cl->connect();
return (b3PhysicsClientHandle)cl;
}

16
examples/SharedMemory/PhysicsClientSharedMemory_C_API.h Normal file
View File

@@ -0,0 +1,16 @@
#ifndef PHYSICS_CLIENT_SHARED_MEMORY_H
#define PHYSICS_CLIENT_SHARED_MEMORY_H
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
b3PhysicsClientHandle b3ConnectSharedMemory(int key);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_SHARED_MEMORY_H

577
examples/SharedMemory/PhysicsClientUDP.cpp Normal file
View File

@@ -0,0 +1,577 @@
#include "PhysicsClientUDP.h"
#include <enet/enet.h>
#include <stdio.h>
#include <string.h>
#include "../Utils/b3Clock.h"
#include "PhysicsClient.h"
//#include "LinearMath/btVector3.h"
#include "SharedMemoryCommands.h"
#include <string>
#include "Bullet3Common/b3Logging.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
void UDPThreadFunc(void* userPtr, void* lsMemory);
void* UDPlsMemoryFunc();
#ifndef _WIN32
#include "../MultiThreading/b3PosixThreadSupport.h"
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("UDPThread",
UDPThreadFunc,
UDPlsMemoryFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
return threadSupport;
}
#elif defined( _WIN32)
#include "../MultiThreading/b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc, numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
}
#endif
struct UDPThreadLocalStorage
{
int threadId;
};
unsigned int b3DeserializeInt(const unsigned char* input)
{
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
return tmp;
}
struct UdpNetworkedInternalData
{
ENetHost* m_client;
ENetAddress m_address;
ENetPeer* m_peer;
ENetEvent m_event;
bool m_isConnected;
b3ThreadSupportInterface* m_threadSupport;
b3CriticalSection* m_cs;
UdpNetworkedInternalData* m_udpInternalData;
SharedMemoryCommand m_clientCmd;
bool m_hasCommand;
bool m_hasStatus;
SharedMemoryStatus m_lastStatus;
b3AlignedObjectArray<char> m_stream;
std::string m_hostName;
int m_port;
UdpNetworkedInternalData()
:m_client(0),
m_peer(0),
m_isConnected(false),
m_threadSupport(0),
m_hasCommand(false),
m_hasStatus(false)
{
}
bool connectUDP()
{
if (m_isConnected)
return true;
if (enet_initialize() != 0)
{
fprintf(stderr, "Error initialising enet");
exit(EXIT_FAILURE);
}
m_client = enet_host_create(NULL, /* create a client host */
1, /* number of clients */
2, /* number of channels */
57600 / 8, /* incoming bandwith */
14400 / 8); /* outgoing bandwith */
if (m_client == NULL) {
fprintf(stderr, "Could not create client host");
return false;
}
enet_address_set_host(&m_address, m_hostName.c_str());
m_address.port = m_port;
m_peer = enet_host_connect(m_client,
&m_address, /* address to connect to */
2, /* number of channels */
0); /* user data supplied to
the receiving host */
if (m_peer == NULL) {
fprintf(stderr, "No available peers for initiating an ENet "
"connection.\n");
return false;
}
/* Try to connect to server within 5 seconds */
if (enet_host_service(m_client, &m_event, 5000) > 0 &&
m_event.type == ENET_EVENT_TYPE_CONNECT)
{
puts("Connection to server succeeded.");
}
else
{
/* Either the 5 seconds are up or a disconnect event was */
/* received. Reset the peer in the event the 5 seconds */
/* had run out without any significant event. */
enet_peer_reset(m_peer);
fprintf(stderr, "Connection to server failed.");
return false;
}
int serviceResult = enet_host_service(m_client, &m_event, 0);
if (serviceResult > 0)
{
switch (m_event.type)
{
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
m_event.peer->data = (void*)"New User";
break;
case ENET_EVENT_TYPE_RECEIVE:
printf("A packet of length %u containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
m_event.packet->data,
m_event.peer->data,
m_event.channelID);
/* Clean up the packet now that we're done using it.
> */
enet_packet_destroy(m_event.packet);
break;
case ENET_EVENT_TYPE_DISCONNECT:
printf("%s disconected.\n", m_event.peer->data);
break;
}
}
else if (serviceResult > 0)
{
puts("Error with servicing the client");
return false;
}
m_isConnected = true;
return m_isConnected;
}
bool checkData()
{
bool hasStatus = false;
int serviceResult = enet_host_service(m_client, &m_event, 100);
if (serviceResult > 0)
{
switch (m_event.type)
{
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
m_event.peer->data = (void*)"New User";
break;
case ENET_EVENT_TYPE_RECEIVE:
{
printf("A packet of length %u containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
m_event.packet->data,
m_event.peer->data,
m_event.channelID);
int packetSizeInBytes = b3DeserializeInt(m_event.packet->data);
if (packetSizeInBytes == m_event.packet->dataLength)
{
SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&m_event.packet->data[4];
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
m_stream.resize(numStreamBytes);
for (int i = 0; i < numStreamBytes; i++)
{
m_stream[i] = m_event.packet->data[i + streamOffsetInBytes];
}
}
else
{
printf("unknown status message received\n");
}
enet_packet_destroy(m_event.packet);
hasStatus = true;
break;
}
case ENET_EVENT_TYPE_DISCONNECT:
{
printf("%s disconected.\n", m_event.peer->data);
break;
}
}
}
else if (serviceResult > 0)
{
puts("Error with servicing the client");
}
return hasStatus;
}
};
enum UDPThreadEnums
{
eUDPRequestTerminate = 13,
eUDPIsUnInitialized,
eUDPIsInitialized,
eUDPInitializationFailed,
eUDPHasTerminated
};
enum UDPCommandEnums
{
eUDPIdle = 13,
eUDP_ConnectRequest,
eUDP_Connected,
eUDP_ConnectionFailed,
eUDP_DisconnectRequest,
eUDP_Disconnected,
};
void UDPThreadFunc(void* userPtr, void* lsMemory)
{
printf("UDPThreadFunc thread started\n");
UDPThreadLocalStorage* localStorage = (UDPThreadLocalStorage*)lsMemory;
UdpNetworkedInternalData* args = (UdpNetworkedInternalData*)userPtr;
int workLeft = true;
b3Clock clock;
clock.reset();
bool init = true;
if (init)
{
args->m_cs->lock();
args->m_cs->setSharedParam(0, eUDPIsInitialized);
args->m_cs->unlock();
double deltaTimeInSeconds = 0;
do
{
deltaTimeInSeconds += double(clock.getTimeMicroseconds()) / 1000000.;
if (deltaTimeInSeconds<(1. / 5000.))
{
// b3Clock::usleep(250);
}
else
{
clock.reset();
deltaTimeInSeconds = 0.f;
switch (args->m_cs->getSharedParam(1))
{
case eUDP_ConnectRequest:
{
bool connected = args->connectUDP();
if (connected)
{
args->m_cs->setSharedParam(1, eUDP_Connected);
}
else
{
args->m_cs->setSharedParam(1, eUDP_ConnectionFailed);
}
break;
}
default:
{
}
};
if (args->m_isConnected)
{
args->m_cs->lock();
bool hasCommand = args->m_hasCommand;
args->m_cs->unlock();
if (hasCommand)
{
int sz = sizeof(SharedMemoryCommand);
ENetPacket *packet = enet_packet_create(&args->m_clientCmd, sz, ENET_PACKET_FLAG_RELIABLE);
int res = enet_peer_send(args->m_peer, 0, packet);
args->m_cs->lock();
args->m_hasCommand = false;
args->m_cs->unlock();
}
bool hasNewStatus = args->checkData();
if (hasNewStatus)
{
if (args->m_hasStatus)
{
//overflow: last status hasn't been processed yet
b3Assert(0);
printf("Error: received new status but previous status not processed yet");
}
else
{
args->m_cs->lock();
args->m_hasStatus = hasNewStatus;
args->m_cs->unlock();
}
}
}
}
} while (args->m_cs->getSharedParam(0) != eUDPRequestTerminate);
}
else
{
args->m_cs->lock();
args->m_cs->setSharedParam(0, eUDPInitializationFailed);
args->m_cs->unlock();
}
printf("finished\n");
}
void* UDPlsMemoryFunc()
{
//don't create local store memory, just return 0
return new UDPThreadLocalStorage;
}
UdpNetworkedPhysicsProcessor::UdpNetworkedPhysicsProcessor(const char* hostName, int port)
{
m_data = new UdpNetworkedInternalData;
if (hostName)
{
m_data->m_hostName = hostName;
}
m_data->m_port = port;
}
UdpNetworkedPhysicsProcessor::~UdpNetworkedPhysicsProcessor()
{
disconnect();
delete m_data;
}
bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
int sz = sizeof(SharedMemoryCommand);
int timeout = 1024 * 1024 * 1024;
m_data->m_cs->lock();
m_data->m_clientCmd = clientCmd;
m_data->m_hasCommand = true;
m_data->m_cs->unlock();
while (m_data->m_hasCommand && (timeout-- > 0))
{
// b3Clock::usleep(100);
}
#if 0
timeout = 1024 * 1024 * 1024;
bool hasStatus = false;
const SharedMemoryStatus* stat = 0;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = receiveStatus(serverStatusOut, bufferServerToClient, bufferSizeInBytes);
b3Clock::usleep(100);
}
return hasStatus;
#endif
return false;
}
bool UdpNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = false;
if (m_data->m_hasStatus)
{
hasStatus = true;
serverStatusOut = m_data->m_lastStatus;
int numStreamBytes = m_data->m_stream.size();
if (numStreamBytes < bufferSizeInBytes)
{
for (int i = 0; i < numStreamBytes; i++)
{
bufferServerToClient[i] = m_data->m_stream[i];
}
}
else
{
printf("Error: steam buffer overflow\n");
}
m_data->m_cs->lock();
m_data->m_hasStatus = false;
m_data->m_cs->unlock();
}
return hasStatus;
}
void UdpNetworkedPhysicsProcessor::renderScene()
{
}
void UdpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
void UdpNetworkedPhysicsProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
}
bool UdpNetworkedPhysicsProcessor::isConnected() const
{
return m_data->m_isConnected;
}
bool UdpNetworkedPhysicsProcessor::connect()
{
if (m_data->m_threadSupport==0)
{
m_data->m_threadSupport = createUDPThreadSupport(1);
m_data->m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_cs->setSharedParam(0, eUDPIsUnInitialized);
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) m_data, 0);
while (m_data->m_cs->getSharedParam(0) == eUDPIsUnInitialized)
{
b3Clock::usleep(1000);
}
m_data->m_cs->lock();
m_data->m_cs->setSharedParam(1, eUDP_ConnectRequest);
m_data->m_cs->unlock();
while (m_data->m_cs->getSharedParam(1) == eUDP_ConnectRequest)
{
b3Clock::usleep(1000);
}
}
return true;
}
void UdpNetworkedPhysicsProcessor::disconnect()
{
if (m_data->m_threadSupport)
{
m_data->m_cs->lock();
m_data->m_cs->setSharedParam(0, eUDPRequestTerminate);
m_data->m_cs->unlock();
int numActiveThreads = 1;
while (numActiveThreads)
{
int arg0, arg1;
if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n", numActiveThreads);
}
else
{
b3Clock::usleep(1000);
}
};
printf("stopping threads\n");
delete m_data->m_threadSupport;
m_data->m_threadSupport = 0;
}
}

37
examples/SharedMemory/PhysicsClientUDP.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef PHYSICS_CLIENT_UDP_H
#define PHYSICS_CLIENT_UDP_H
#include "PhysicsDirect.h"
#include "PhysicsServerCommandProcessor.h"
class UdpNetworkedPhysicsProcessor : public PhysicsCommandProcessorInterface
{
struct UdpNetworkedInternalData* m_data;
public:
UdpNetworkedPhysicsProcessor(const char* hostName, int port);
virtual ~UdpNetworkedPhysicsProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
};
#endif //PHYSICS_CLIENT_UDP_H

21
examples/SharedMemory/PhysicsClientUDP_C_API.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "PhysicsClientUDP_C_API.h"
#include "PhysicsClientUDP.h"
#include "PhysicsDirect.h"
#include <stdio.h>
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
{
UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port);
PhysicsDirect* direct = new PhysicsDirect(udp);
bool connected = direct->connect();
printf("direct!\n");
return (b3PhysicsClientHandle)direct;
}

19
examples/SharedMemory/PhysicsClientUDP_C_API.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef PHYSICS_CLIENT_UDP_C_API_H
#define PHYSICS_CLIENT_UDP_C_API_H
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
///send physics commands using UDP networking
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_UDP_C_API_H

27
examples/SharedMemory/PhysicsCommandProcessorInterface.h Normal file
View File

@@ -0,0 +1,27 @@
#ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
#define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
class PhysicsCommandProcessorInterface
{
public:
virtual ~PhysicsCommandProcessorInterface() {}
virtual bool connect()=0;
virtual void disconnect() = 0;
virtual bool isConnected() const = 0;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) = 0;
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) = 0;
virtual void renderScene() = 0;
virtual void physicsDebugDraw(int debugDrawFlags) = 0;
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) = 0;
};
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

264
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -3,7 +3,9 @@
#include "PhysicsClientSharedMemory.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommands.h"
#include "PhysicsServerCommandProcessor.h"
#include "PhysicsCommandProcessorInterface.h"
#include "LinearMath/btHashMap.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
@@ -44,26 +46,34 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsCommandProcessorInterface* m_commandProcessor;
bool m_ownsCommandProcessor;
PhysicsDirectInternalData()
:m_hasStatus(false),
m_verboseOutput(false)
m_verboseOutput(false),
m_ownsCommandProcessor(false)
{
}
};
PhysicsDirect::PhysicsDirect()
PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk)
{
m_data = new PhysicsDirectInternalData;
m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
m_data->m_commandProcessor = physSdk;
m_data->m_ownsCommandProcessor = false;
}
PhysicsDirect::~PhysicsDirect()
{
delete m_data->m_commandProcessor;
if (m_data->m_commandProcessor->isConnected())
{
m_data->m_commandProcessor->disconnect();
}
if (m_data->m_ownsCommandProcessor)
{
delete m_data->m_commandProcessor;
}
delete m_data;
}
@@ -71,23 +81,26 @@ PhysicsDirect::~PhysicsDirect()
// return true if connection succesfull, can also check 'isConnected'
bool PhysicsDirect::connect()
{
bool connected = m_data->m_commandProcessor->connect();
m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
return true;
return connected;
}
// return true if connection succesfull, can also check 'isConnected'
bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
{
bool connected = m_data->m_commandProcessor->connect();
m_data->m_commandProcessor->setGuiHelper(guiHelper);
return true;
return connected;
}
void PhysicsDirect::renderScene()
{
m_data->m_commandProcessor->renderScene();
}
void PhysicsDirect::debugDraw(int debugDrawMode)
{
m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
@@ -96,21 +109,31 @@ void PhysicsDirect::debugDraw(int debugDrawMode)
////todo: rename to 'disconnect'
void PhysicsDirect::disconnectSharedMemory()
{
m_data->m_commandProcessor->disconnect();
m_data->m_commandProcessor->setGuiHelper(0);
}
bool PhysicsDirect::isConnected() const
{
return true;
return m_data->m_commandProcessor->isConnected();
}
// return non-null if there is a status, nullptr otherwise
const SharedMemoryStatus* PhysicsDirect::processServerStatus()
{
if (!m_data->m_hasStatus)
{
m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
SharedMemoryStatus* stat = 0;
if (m_data->m_hasStatus)
{
stat = &m_data->m_serverStatus;
postProcessStatus(m_data->m_serverStatus);
m_data->m_hasStatus = false;
}
return stat;
@@ -136,7 +159,19 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
btAssert(m_data->m_serverStatus.m_type == CMD_DEBUG_LINES_COMPLETED);
@@ -184,6 +219,8 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)
{
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_startingLineIndex =
serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
@@ -204,6 +241,17 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
@@ -223,6 +271,8 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
{
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId;
@@ -243,7 +293,19 @@ bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& or
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
@@ -264,7 +326,10 @@ bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& or
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
@@ -289,6 +354,18 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
@@ -340,6 +417,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
{
m_data->m_hasStatus = false;
// continue requesting remaining pixels
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
@@ -365,7 +443,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
{
bParse::btBulletFile bf(
&m_data->m_bulletStreamDataServerToClient[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
serverCmd.m_numDataStreamBytes);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
@@ -402,6 +480,87 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
}
}
void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd)
{
switch (serverCmd.m_type)
{
case CMD_RESET_SIMULATION_COMPLETED:
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i = 0; i<m_data->m_bodyJointMap.size(); i++)
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j = 0; j<bodyJoints->m_jointInfo.size(); j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i<numBodies; i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
if (hasStatus)
{
processBodyJointInfo(bodyUniqueId, infoStatus);
}
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
if (serverCmd.m_numDataStreamBytes > 0)
{
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyIndex, serverCmd);
}
break;
}
default:
{
// b3Error("Unknown server status type");
}
};
}
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -427,78 +586,7 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
switch (m_data->m_serverStatus.m_type)
{
case CMD_RESET_SIMULATION_COMPLETED:
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (hasStatus)
{
processBodyJointInfo(bodyUniqueId, infoStatus);
}
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0)
{
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyIndex,serverCmd);
}
break;
}
default:
{
// b3Error("Unknown server status type");
}
};
postProcessStatus(m_data->m_serverStatus);
}
return hasStatus;
}
@@ -549,7 +637,7 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
if (jointIndex < bodyJoints->m_jointInfo.size())
if ((jointIndex >=0) && (jointIndex < bodyJoints->m_jointInfo.size()))
{
info = bodyJoints->m_jointInfo[jointIndex];
return true;

7
examples/SharedMemory/PhysicsDirect.h
View File

@@ -7,9 +7,6 @@
#include "PhysicsClient.h"
#include "LinearMath/btVector3.h"
///todo: the PhysicsClient API was designed with shared memory in mind,
///now it become more general we need to move out the shared memory specifics away
///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ]
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class PhysicsDirect : public PhysicsClient
{
@@ -27,9 +24,11 @@ protected:
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
public:
PhysicsDirect();
PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk);
virtual ~PhysicsDirect();

10
examples/SharedMemory/PhysicsDirectC_API.cpp
View File

@@ -2,13 +2,21 @@
#include "PhysicsDirect.h"
#include "PhysicsServerCommandProcessor.h"
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsDirect()
{
PhysicsDirect* direct = new PhysicsDirect();
PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor;
PhysicsDirect* direct = new PhysicsDirect(sdk);
bool connected = direct->connect();
return (b3PhysicsClientHandle )direct;
}
//

21
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1187,6 +1187,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_dataStream = 0;
//consume the command
switch (clientCmd.m_type)
@@ -1244,7 +1246,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
//9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color'
int maxNumLines = bufferSizeInBytes/(sizeof(float)*9)-1;
int bytesPerLine = (sizeof(float) * 9);
int maxNumLines = bufferSizeInBytes/bytesPerLine-1;
if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size())
{
b3Warning("m_startingLineIndex exceeds total number of debug lines");
@@ -1277,7 +1280,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED;
serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine;
serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines;
serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex;
serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines);
@@ -1327,6 +1330,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8);
serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
{
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
@@ -1368,6 +1373,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
@@ -1386,7 +1392,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes;
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
hasStatus = true;
break;
}
@@ -1606,11 +1613,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0;
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
hasStatus = true;
@@ -2583,7 +2590,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
hasStatus = true;
break;
@@ -3006,7 +3013,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
hasStatus = true;
break;

38
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -3,6 +3,8 @@
#include "LinearMath/btVector3.h"
#include "PhysicsCommandProcessorInterface.h"
struct SharedMemLines
{
btVector3 m_from;
@@ -10,8 +12,10 @@ struct SharedMemLines
btVector3 m_color;
};
///todo: naming. Perhaps PhysicsSdkCommandprocessor?
class PhysicsServerCommandProcessor
class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
{
struct PhysicsServerCommandProcessorInternalData* m_data;
@@ -22,15 +26,15 @@ class PhysicsServerCommandProcessor
protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
@@ -39,12 +43,30 @@ public:
virtual ~PhysicsServerCommandProcessor();
void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld();
virtual void deleteDynamicsWorld();
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
virtual bool connect()
{
return true;
};
virtual void disconnect() {}
virtual bool isConnected() const
{
return true;
}
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
};
virtual void renderScene();
virtual void physicsDebugDraw(int debugDrawFlags);

216
examples/SharedMemory/SharedMemoryCommandProcessor.cpp Normal file
View File

@@ -0,0 +1,216 @@
#include "SharedMemoryCommandProcessor.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3Scalar.h"
#include "SharedMemoryBlock.h"
struct SharedMemoryCommandProcessorInternalData
{
int m_sharedMemoryKey;
bool m_isConnected;
SharedMemoryInterface* m_sharedMemory;
bool m_ownsSharedMemory;
bool m_verboseOutput;
bool m_waitingForServer;
SharedMemoryStatus m_lastServerStatus;
SharedMemoryBlock* m_testBlock1;
SharedMemoryCommandProcessorInternalData()
:m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_isConnected(false),
m_sharedMemory(0),
m_ownsSharedMemory(false),
m_verboseOutput(false),
m_waitingForServer(false),
m_testBlock1(0)
{
}
};
SharedMemoryCommandProcessor::SharedMemoryCommandProcessor()
{
m_data = new SharedMemoryCommandProcessorInternalData;
m_data->m_sharedMemoryKey = SHARED_MEMORY_KEY;
#ifdef _WIN32
m_data->m_sharedMemory = new Win32SharedMemoryClient();
#else
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
m_data->m_ownsSharedMemory = true;
}
SharedMemoryCommandProcessor::~SharedMemoryCommandProcessor()
{
if (m_data->m_isConnected)
{
disconnect();
}
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
delete m_data;
}
bool SharedMemoryCommandProcessor::connect()
{
if (m_data->m_isConnected)
return true;
bool allowCreation = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
else {
if (m_data->m_verboseOutput) {
b3Printf("Connected to existing shared memory, status OK.\n");
}
m_data->m_isConnected = true;
}
}
else {
b3Error("Cannot connect to shared memory");
return false;
}
return true;
}
void SharedMemoryCommandProcessor::disconnect()
{
if (m_data->m_isConnected && m_data->m_sharedMemory)
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
}
m_data->m_isConnected = false;
}
bool SharedMemoryCommandProcessor::isConnected() const
{
return m_data->m_isConnected;
}
bool SharedMemoryCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if (!m_data->m_waitingForServer) {
if (&m_data->m_testBlock1->m_clientCommands[0] != &clientCmd) {
m_data->m_testBlock1->m_clientCommands[0] = clientCmd;
}
m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true;
}
return false;
}
bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
SharedMemoryStatus* stat = 0;
m_data->m_lastServerStatus.m_dataStream = 0;
m_data->m_lastServerStatus.m_numDataStreamBytes = 0;
if (!m_data->m_testBlock1)
{
//m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
//return &m_data->m_lastServerStatus;
//serverStatusOut = m_data->m_lastServerStatus;
return false;
}
if (!m_data->m_waitingForServer) {
return false;
}
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
{
//m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
//return &m_data->m_lastServerStatus;
return false;
}
if (m_data->m_testBlock1->m_numServerCommands >
m_data->m_testBlock1->m_numProcessedServerCommands)
{
b3Assert(m_data->m_testBlock1->m_numServerCommands ==
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
m_data->m_lastServerStatus = serverCmd;
m_data->m_lastServerStatus.m_dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < m_data->m_lastServerStatus.m_numDataStreamBytes; i++)
{
bufferServerToClient[i] = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i];
}
m_data->m_testBlock1->m_numProcessedServerCommands++;
// we don't have more than 1 command outstanding (in total, either server or client)
b3Assert(m_data->m_testBlock1->m_numProcessedServerCommands ==
m_data->m_testBlock1->m_numServerCommands);
if (m_data->m_testBlock1->m_numServerCommands ==
m_data->m_testBlock1->m_numProcessedServerCommands) {
m_data->m_waitingForServer = false;
}
else {
m_data->m_waitingForServer = true;
}
serverStatusOut = m_data->m_lastServerStatus;
return true;
}
return false;
}
void SharedMemoryCommandProcessor::renderScene()
{
}
void SharedMemoryCommandProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
void SharedMemoryCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
}
void SharedMemoryCommandProcessor::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
{
if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
m_data->m_ownsSharedMemory = false;
m_data->m_sharedMemory = sharedMem;
}
void SharedMemoryCommandProcessor::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
}

37
examples/SharedMemory/SharedMemoryCommandProcessor.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef SHARED_MEMORY_COMMAND_PROCESSOR_H
#define SHARED_MEMORY_COMMAND_PROCESSOR_H
#include "PhysicsCommandProcessorInterface.h"
class SharedMemoryCommandProcessor : public PhysicsCommandProcessorInterface
{
struct SharedMemoryCommandProcessorInternalData* m_data;
public:
SharedMemoryCommandProcessor();
virtual ~SharedMemoryCommandProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void setSharedMemoryKey(int key);
};
#endif //SHARED_MEMORY_COMMAND_PROCESSOR_H

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -90,7 +90,6 @@ struct UrdfArgs
struct BulletDataStreamArgs
{
char m_bulletFileName[MAX_FILENAME_LENGTH];
int m_streamChunkLength;
int m_bodyUniqueId;
};
@@ -508,6 +507,10 @@ struct SharedMemoryStatus
smUint64_t m_timeStamp;
int m_sequenceNumber;
//m_streamBytes is only for internal purposes
int m_numDataStreamBytes;
char* m_dataStream;
union
{
struct BulletDataStreamArgs m_dataStreamArguments;

13
examples/SharedMemory/premake4.lua
View File

@@ -42,6 +42,14 @@ myfiles =
"PhysicsLoopBack.h",
"PhysicsLoopBackC_API.cpp",
"PhysicsLoopBackC_API.h",
"PhysicsClientSharedMemory_C_API.cpp",
"PhysicsClientSharedMemory_C_API.h",
"PhysicsClientSharedMemory2_C_API.cpp",
"PhysicsClientSharedMemory2_C_API.h",
"PhysicsClientSharedMemory2.cpp",
"PhysicsClientSharedMemory2.h",
"SharedMemoryCommandProcessor.cpp",
"SharedMemoryCommandProcessor.h",
"PhysicsServerCommandProcessor.cpp",
"PhysicsServerCommandProcessor.h",
"TinyRendererVisualShapeConverter.cpp",
@@ -288,4 +296,7 @@ if os.is("Windows") then
end
end
end
include "udp"

187
examples/SharedMemory/udp/main.cpp Normal file
View File

@@ -0,0 +1,187 @@
/* server.cpp */
#include <stdio.h>
#include <enet/enet.h>
#include "SharedMemoryCommandProcessor.h"
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "PhysicsServerCommandProcessor.h"
void MySerializeInt(unsigned int sz, unsigned char* output)
{
unsigned int tmp = sz;
output[0] = tmp & 255;
tmp = tmp >> 8;
output[1] = tmp & 255;
tmp = tmp >> 8;
output[2] = tmp & 255;
tmp = tmp >> 8;
output[3] = tmp & 255;
}
int main(int argc, char *argv[])
{
unsigned char buf[4];
SharedMemoryCommandProcessor* sm = new SharedMemoryCommandProcessor;
// PhysicsDirect* sm = new PhysicsDirect(sdk);
//PhysicsClientSharedMemory* sm = new PhysicsClientSharedMemory();
bool isPhysicsClientConnected = sm->connect();
if (isPhysicsClientConnected)
{
ENetAddress address;
ENetHost *server;
ENetEvent event;
int serviceResult;
puts("Starting server");
if (enet_initialize() != 0)
{
puts("Error initialising enet");
exit(EXIT_FAILURE);
}
/* Bind the server to the default localhost. */
/* A specific host address can be specified by */
/* enet_address_set_host (& address, "x.x.x.x"); */
address.host = ENET_HOST_ANY;
/* Bind the server to port 1234. */
address.port = 1234;
server = enet_host_create(&address,
32, /* number of clients */
2, /* number of channels */
0, /* Any incoming bandwith */
0); /* Any outgoing bandwith */
if (server == NULL)
{
puts("Could not create server host");
exit(EXIT_FAILURE);
}
while (true)
{
serviceResult = 1;
/* Keep doing host_service until no events are left */
while (serviceResult > 0)
{
/* Wait up to 1000 milliseconds for an event. */
serviceResult = enet_host_service(server, &event, 0);
if (serviceResult > 0)
{
switch (event.type)
{
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
event.peer->address.host,
event.peer->address.port);
/* Store any relevant client information here. */
event.peer->data = (void*)"Client information";
break;
case ENET_EVENT_TYPE_RECEIVE:
printf("A packet of length %u containing '%s' was "
"received from %s on channel %u.\n",
event.packet->dataLength,
event.packet->data,
event.peer->data,
event.channelID);
if (event.packet->dataLength == sizeof(SharedMemoryCommand))
{
SharedMemoryCommand* cmdPtr = (SharedMemoryCommand*)event.packet->data;
SharedMemoryStatus serverStatus;
b3AlignedObjectArray<char> buffer;
buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
bool hasStatus = sm->processCommand(*cmdPtr,serverStatus, &buffer[0], buffer.size());
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = sm->receiveStatus(serverStatus, &buffer[0], buffer.size());
}
printf("buffer.size = %d\n", buffer.size());
printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
if (hasStatus)
{
//create packetData with [int packetSizeInBytes, status, streamBytes)
unsigned char* statBytes = (unsigned char*)&serverStatus;
b3AlignedObjectArray<unsigned char> packetData;
packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes);
int sz = packetData.size();
int curPos = 0;
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < sizeof(SharedMemoryStatus); i++)
{
packetData[i + curPos] = statBytes[i];
}
curPos += sizeof(SharedMemoryStatus);
for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++)
{
packetData[i + curPos] = serverStatus.m_dataStream[i];
}
ENetPacket *packet = enet_packet_create(&packetData[0], packetData.size() , ENET_PACKET_FLAG_RELIABLE);
//enet_peer_send(peer, 0, packet);
enet_host_broadcast(server, 0, packet);
}
}
else
{
printf("received packet with unknown contents\n");
}
/* Tell all clients about this message */
//enet_host_broadcast(server, 0, event.packet);
break;
case ENET_EVENT_TYPE_DISCONNECT:
printf("%s disconnected.\n", event.peer->data);
/* Reset the peer's client information. */
event.peer->data = NULL;
break;
}
}
else if (serviceResult > 0)
{
puts("Error with servicing the server");
exit(EXIT_FAILURE);
}
}
}
enet_host_destroy(server);
enet_deinitialize();
}
delete sm;
return 0;
}

46
examples/SharedMemory/udp/premake4.lua Normal file
View File

@@ -0,0 +1,46 @@
project ("App_PhysicsServerUDP")
language "C++"
kind "ConsoleApp"
includedirs {"../../ThirdPartyLibs/enet/include","../../../src",".."}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
end
if os.is("MacOSX") then
end
links {
"enet",
"BulletFileLoader",
"Bullet3Common",
"LinearMath"
}
files {
"main.cpp",
"../PhysicsClient.cpp",
"../PhysicsClient.h",
"../PhysicsDirect.cpp",
"../PhysicsDirect.h",
"../PhysicsCommandProcessorInterface.h",
"../SharedMemoryCommandProcessor.cpp",
"../SharedMemoryCommandProcessor.h",
"../PhysicsClientC_API.cpp",
"../PhysicsClientC_API.h",
"../Win32SharedMemory.cpp",
"../Win32SharedMemory.h",
"../PosixSharedMemory.cpp",
"../PosixSharedMemory.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3ResourcePath.h",
}