bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
bool allowCreation = true;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
{
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
} else
{
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;
}
bool PhysicsClientSharedMemory::connect()
{
///server always has to create and initialize shared memory
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;
}
SimpleOpenGL2App::SimpleOpenGL2App(const char* title, int width, int height)
{
gApp2 = this;
m_data = new SimpleOpenGL2AppInternalData;
m_window = new b3gDefaultOpenGLWindow();
b3gWindowConstructionInfo ci;
ci.m_title = title;
ci.m_openglVersion = 2;
ci.m_width = width;
ci.m_height = height;
m_window->createWindow(ci);
m_window->setWindowTitle(title);
#ifndef __APPLE__
#ifndef _WIN32
//some Linux implementations need the 'glewExperimental' to be true
glewExperimental = GL_TRUE;
#endif
if (glewInit() != GLEW_OK)
{
b3Error("glewInit failed");
exit(1);
}
if (!GLEW_VERSION_2_1) // check that the machine supports the 2.1 API.
{
b3Error("GLEW_VERSION_2_1 needs to support 2_1");
exit(1); // or handle the error in a nicer way
}
#endif
glGetError();//don't remove this call, it is needed for Ubuntu
glClearColor(0.9,0.9,1,1);
b3Assert(glGetError() ==GL_NO_ERROR);
//m_primRenderer = new GLPrimitiveRenderer(width,height);
m_parameterInterface = 0;
b3Assert(glGetError() ==GL_NO_ERROR);
//m_instancingRenderer = new GLInstancingRenderer(128*1024,32*1024*1024);
//m_instancingRenderer->init();
//m_instancingRenderer->resize(width,height);
b3Assert(glGetError() ==GL_NO_ERROR);
//m_instancingRenderer->InitShaders();
m_window->setMouseMoveCallback(Simple2MouseMoveCallback);
m_window->setMouseButtonCallback(Simple2MouseButtonCallback);
m_window->setKeyboardCallback(Simple2KeyboardCallback);
m_window->setWheelCallback(Simple2WheelCallback);
m_window->setResizeCallback(Simple2ResizeCallback);
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
bool allowCreation = true;
if (m_data->m_isConnected)
{
b3Warning("connectSharedMemory, while already connected");
return m_data->m_isConnected;
}
int counter = 0;
do
{
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
int magicId =m_data->m_testBlock1->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_isConnected = true;
} else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
m_data->m_isConnected = false;
}
} else
{
b3Error("Cannot connect to shared memory");
m_data->m_isConnected = false;
}
} while (counter++ < 10 && !m_data->m_isConnected);
if (!m_data->m_isConnected)
{
b3Error("Server cannot connect to shared memory.\n");
}
return m_data->m_isConnected;
}
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
#ifdef TEST_SHARED_MEMORY
{
btSharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_sharedMemoryPtr;
}
}
int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
int id = shmget((key_t) key, (size_t) size,flags);
if (id < 0)
{
b3Error("shmget error");
} else
{
btPointerCaster result;
result.ptr = shmat(id,0,0);
if (result.integer == -1)
{
b3Error("shmat returned -1");
} else
{
btSharedMemorySegment seg;
seg.m_key = key;
seg.m_createdSharedMemory = allowCreation;
seg.m_sharedMemoryId = id;
seg.m_sharedMemoryPtr = result.ptr;
m_internalData->m_segments.push_back(seg);
return result.ptr;
}
}
#else
//not implemented yet
btAssert(0);
#endif
return 0;
}
void PosixSharedMemory::releaseSharedMemory(int key, int size)
{
#ifdef TEST_SHARED_MEMORY
btSharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (0==seg)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
return;
}
if (seg->m_sharedMemoryId < 0)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
} else
{
if (seg->m_createdSharedMemory)
{
int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0);
if (result == -1)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
} else
{
b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
}
seg->m_createdSharedMemory = false;
seg->m_sharedMemoryId = -1;
}
if (seg->m_sharedMemoryPtr)
{
shmdt(seg->m_sharedMemoryPtr);
seg->m_sharedMemoryPtr = 0;
b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
}
}
m_internalData->m_segments.removeAtIndex(i);
#endif
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClient* cl = (PhysicsClient*) userPtr;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
cl->submitCommand(CMD_LOAD_URDF);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
break;
}
case CMD_SHUTDOWN:
{
cl->submitCommand(CMD_SHUTDOWN);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void PhysicsServer::initPhysics()
{
createEmptyDynamicsWorld();
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
// btAssert(m_testBlock1);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
{
b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n");
}
m_testBlock1->m_numClientCommands = 0;
m_testBlock1->m_numServerCommands = 0;
m_testBlock1->m_numProcessedClientCommands=0;
m_testBlock1->m_numProcessedServerCommands=0;
m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
b3Printf("Shared memory succesfully allocated\n");
} else
{
b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n");
}
}
void Win32SharedMemory::releaseSharedMemory(int key, int size)
{
Win32SharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg==0)
{
b3Error("Couldn't find shared memory segment");
return;
}
if (seg->m_buf)
{
UnmapViewOfFile(seg->m_buf);
seg->m_buf=0;
}
if (seg->m_hMapFile)
{
CloseHandle(seg->m_hMapFile);
seg->m_hMapFile = 0;
}
m_internalData->m_segments.removeAtIndex(i);
}
void LuaPhysicsSetup::initPhysics()
{
m_guiHelper->setUpAxis(upaxis);
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* f=0;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],sLuaFileName);
f = fopen(relativeFileName,"rb");
}
if (f)
{
fclose(f);
lua_State *L = luaL_newstate();
luaopen_io(L); // provides io.*
luaopen_base(L);
luaopen_table(L);
luaopen_string(L);
luaopen_math(L);
//luaopen_package(L);
luaL_openlibs(L);
// make my_function() available to Lua programs
lua_register(L, "createDefaultDynamicsWorld", gCreateDefaultDynamicsWorld);
lua_register(L, "deleteDynamicsWorld", gDeleteDynamicsWorld);
lua_register(L, "createCubeShape", gCreateCubeShape);
lua_register(L, "createSphereShape", gCreateSphereShape);
lua_register(L, "loadMultiBodyFromUrdf",gLoadMultiBodyFromUrdf);
lua_register(L, "createRigidBody", gCreateRigidBody);
lua_register(L, "setBodyPosition", gSetBodyPosition);
lua_register(L, "setBodyOrientation", gSetBodyOrientation);
int s = luaL_loadfile(L, relativeFileName);
if ( s==0 ) {
// execute Lua program
s = lua_pcall(L, 0, LUA_MULTRET, 0);
}
report_errors(L, s);
lua_close(L);
} else
{
b3Error("Cannot find Lua file%s\n",sLuaFileName);
}
}
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)
{
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >= 201705023))
{
b3Error("Error: physics server version mismatch (expected %d got %d)\n", SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
}
else
{
b3Error("Error connecting to shared memory: 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;
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
m_data->m_guiHelper = guiHelper;
bool allowCreation = true;
bool allowConnectToExistingSharedMemory = false;
if (m_data->m_isConnected)
{
b3Warning("connectSharedMemory, while already connected");
return m_data->m_isConnected;
}
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
int magicId =m_data->m_testBlock1->m_magicId;
b3Printf("magicId = %d\n", magicId);
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block\n");
m_data->m_isConnected = true;
} else
{
b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n");
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
m_data->m_isConnected = false;
}
} else
{
b3Error("Cannot connect to shared memory");
m_data->m_isConnected = false;
}
return m_data->m_isConnected;
}
bool PhysicsClientSharedMemory::connect() {
/// server always has to create and initialize shared memory
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 {
b3Warning("Cannot connect to shared memory");
return false;
}
#if 0
if (m_data->m_isConnected)
{
//get all existing bodies and body info...
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37;
submitClientCommand(command);
int timeout = 1024 * 1024 * 1024;
const SharedMemoryStatus* status = 0;
while ((status == 0) && (timeout-- > 0))
{
status = processServerStatus();
}
//submitClientCommand(command);
}
#endif
return true;
}
int b3CpuNarrowPhase::allocateCollidable()
{
int curSize = m_data->m_collidablesCPU.size();
if (curSize < m_data->m_config.m_maxConvexShapes)
{
m_data->m_collidablesCPU.expand();
return curSize;
}
else
{
b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
}
return -1;
}
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
} else
{
b3Printf("Shared Memory status is OK\n");
}
} else
{
m_wantsTermination = true;
}
}
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
b3RigidBodyData body;
int bodyIndex = m_data->m_rigidBodies.size();
body.m_invMass = mass ? 1.f/mass : 0.f;
body.m_angVel.setValue(0,0,0);
body.m_collidableIdx = collidableIndex;
body.m_frictionCoeff = 0.3f;
body.m_linVel.setValue(0,0,0);
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_restituitionCoeff = 0.f;
m_data->m_rigidBodies.push_back(body);
if (collidableIndex>=0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
b3Scalar margin = 0.01f;
b3Transform t;
t.setIdentity();
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
return bodyIndex;
}
int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
int index = gfxObj->m_numGraphicsInstances + gfxObj->m_instanceOffset;
int maxElements = m_data->m_instance_positions_ptr.size();
if (index*4<maxElements)
{
m_data->m_instance_positions_ptr[index*4]=position[0];
m_data->m_instance_positions_ptr[index*4+1]=position[1];
m_data->m_instance_positions_ptr[index*4+2]=position[2];
m_data->m_instance_positions_ptr[index*4+3]=1;
m_data->m_instance_quaternion_ptr[index*4]=quaternion[0];
m_data->m_instance_quaternion_ptr[index*4+1]=quaternion[1];
m_data->m_instance_quaternion_ptr[index*4+2]=quaternion[2];
m_data->m_instance_quaternion_ptr[index*4+3]=quaternion[3];
m_data->m_instance_colors_ptr[index*4]=color[0];
m_data->m_instance_colors_ptr[index*4+1]=color[1];
m_data->m_instance_colors_ptr[index*4+2]=color[2];
m_data->m_instance_colors_ptr[index*4+3]=color[3];
m_data->m_instance_scale_ptr[index*3] = scaling[0];
m_data->m_instance_scale_ptr[index*3+1] = scaling[1];
m_data->m_instance_scale_ptr[index*3+2] = scaling[2];
gfxObj->m_numGraphicsInstances++;
m_data->m_totalNumInstances++;
} else
{
b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
return -1;
}
return index;//gfxObj->m_numGraphicsInstances;
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClient* cl = (PhysicsClient*) userPtr;
switch (buttonId)
{
case CMD_LOAD_URDF:
case CMD_CREATE_BOX_COLLISION_SHAPE:
case CMD_REQUEST_ACTUAL_STATE:
case CMD_STEP_FORWARD_SIMULATION:
case CMD_SHUTDOWN:
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->submitCommand(buttonId);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
bool PhysicsServerSharedMemory::connectSharedMemory(struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
bool allowCreation = true;
bool allConnected = false;
int numConnected = 0;
int counter = 0;
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_areConnected[block])
{
allConnected = true;
numConnected++;
b3Warning("connectSharedMemory, while already connected");
continue;
}
do
{
m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlocks[block])
{
int magicId = m_data->m_testBlocks[block]->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlocks[block]->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlocks[block]);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_areConnected[block] = true;
numConnected++;
}
else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE);
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
}
else
{
//b3Error("Cannot connect to shared memory");
m_data->m_areConnected[block] = false;
}
} while (counter++ < 10 && !m_data->m_areConnected[block]);
if (!m_data->m_areConnected[block])
{
b3Error("Server cannot connect to shared memory.\n");
}
}
allConnected = (numConnected == MAX_SHARED_MEMORY_BLOCKS);
return allConnected;
}
void GLInstancingRenderer::writeTransforms()
{
b3Assert(glGetError() ==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
glFlush();
b3Assert(glGetError() ==GL_NO_ERROR);
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
if (orgBase)
{
int totalNumInstances= 0;
for (int k=0;k<m_graphicsInstances.size();k++)
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
totalNumInstances+=gfxObj->m_numGraphicsInstances;
}
m_data->m_totalNumInstances = totalNumInstances;
for (int k=0;k<m_graphicsInstances.size();k++)
{
//int k=0;
b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
char* base = orgBase;
float* positions = (float*)(base+m_data->m_maxShapeCapacityInBytes);
float* orientations = (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE);
float* colors= (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE);
float* scaling= (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE);
//static int offset=0;
//offset++;
for (int i=0;i<gfxObj->m_numGraphicsInstances;i++)
{
int srcIndex=i+gfxObj->m_instanceOffset;
positions[srcIndex*4] = m_data->m_instance_positions_ptr[srcIndex*4];
positions[srcIndex*4+1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
positions[srcIndex*4+2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
positions[srcIndex*4+3] = m_data->m_instance_positions_ptr[srcIndex*4+3];
orientations[srcIndex*4]=m_data->m_instance_quaternion_ptr[srcIndex*4];
orientations[srcIndex*4+1]=m_data->m_instance_quaternion_ptr[srcIndex*4+1];
orientations[srcIndex*4+2]=m_data->m_instance_quaternion_ptr[srcIndex*4+2];
orientations[srcIndex*4+3]=m_data->m_instance_quaternion_ptr[srcIndex*4+3];
colors[srcIndex*4]=m_data->m_instance_colors_ptr[srcIndex*4];
colors[srcIndex*4+1]=m_data->m_instance_colors_ptr[srcIndex*4+1];
colors[srcIndex*4+2]=m_data->m_instance_colors_ptr[srcIndex*4+2];
colors[srcIndex*4+3]=m_data->m_instance_colors_ptr[srcIndex*4+3];
scaling[srcIndex*3]=m_data->m_instance_scale_ptr[srcIndex*3];
scaling[srcIndex*3+1]=m_data->m_instance_scale_ptr[srcIndex*3+1];
scaling[srcIndex*3+2]=m_data->m_instance_scale_ptr[srcIndex*3+2];
}
}
} else
{
b3Error("ERROR glMapBuffer failed\n");
}
b3Assert(glGetError() ==GL_NO_ERROR);
glUnmapBuffer( GL_ARRAY_BUFFER);
//if this glFinish is removed, the animation is not always working/blocks
//@todo: figure out why
glFlush();
glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
b3Assert(glGetError() ==GL_NO_ERROR);
}
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{
bool hasStatus = false;
if (!m_data->m_testBlock1)
{
serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
return true;
}
if (!m_data->m_waitingForServer)
{
serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND;
return true;
}
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true;
serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command
switch (serverCmd.m_type)
{
case CMD_CLIENT_COMMAND_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server completed command");
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
m_data->m_serverLoadUrdfOK = true;
if (m_data->m_verboseOutput)
{
b3Printf("Server loading the URDF OK\n");
}
if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0)
{
bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA();
bf->parse(false);
m_data->m_robotMultiBodyData.push_back(bf);
for (int i=0; i<bf->m_multiBodies.size(); i++)
{
int flag = bf->getFlags();
int qOffset = 7;
int uOffset=6;
if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
{
Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
if (mb->m_baseName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
}
}
for (int link=0; link<mb->m_numLinks; link++)
{
{
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
info.m_linkIndex = link;
if (mb->m_links[link].m_linkName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
}
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
} else
{
Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
if (mb->m_baseName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
}
}
for (int link=0; link<mb->m_numLinks; link++)
{
{
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
}
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
}
}
if (bf->ok())
{
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server received desired state");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server completed step simulation");
}
break;
}
case CMD_URDF_LOADING_FAILED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server failed loading the URDF...\n");
}
m_data->m_serverLoadUrdfOK = false;
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server received bullet data stream OK\n");
}
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server failed receiving bullet data stream\n");
}
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Received actual state\n");
}
SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
if (m_data->m_verboseOutput)
{
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
}
char msg[1024];
{
sprintf(msg,"Q=[");
for (int i=0; i<numQ; i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
}
if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
{
sprintf(msg,"U=[");
for (int i=0; i<numU; i++)
{
if (i<numU-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
}
}
sprintf(msg,"%s]",msg);
}
if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
if (m_data->m_verboseOutput)
{
b3Printf("\n");
}
break;
}
case CMD_DEBUG_LINES_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
}
int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0];
btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3));
btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3));
m_data->m_debugLinesFrom.resize(numLines);
m_data->m_debugLinesTo.resize(numLines);
m_data->m_debugLinesColor.resize(numLines);
for (int i=0; i<numLines; i++)
{
btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]);
btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]);
btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]);
m_data->m_debugLinesFrom[i] = from;
m_data->m_debugLinesTo[i] = to;
m_data->m_debugLinesColor[i] = color;
}
break;
}
case CMD_DEBUG_LINES_OVERFLOW_FAILED:
{
b3Warning("Error receiving debug lines");
m_data->m_debugLinesFrom.resize(0);
m_data->m_debugLinesTo.resize(0);
m_data->m_debugLinesColor.resize(0);
break;
}
default:
{
b3Error("Unknown server status\n");
btAssert(0);
}
};
m_data->m_testBlock1->m_numProcessedServerCommands++;
//we don't have more than 1 command outstanding (in total, either server or client)
btAssert(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;
}
} else
{
if (m_data->m_verboseOutput)
{
b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
m_data->m_testBlock1->m_numProcessedServerCommands);
}
}
return hasStatus;
}
inline int b3GpuPgsContactSolver::sortConstraintByBatch3( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies, int* batchSizes)
{
B3_PROFILE("sortConstraintByBatch3");
static int maxSwaps = 0;
int numSwaps = 0;
curUsed.resize(2*simdWidth);
static int maxNumConstraints = 0;
if (maxNumConstraints<numConstraints)
{
maxNumConstraints = numConstraints;
//printf("maxNumConstraints = %d\n",maxNumConstraints );
}
int numUsedArray = numBodies/32+1;
bodyUsed.resize(numUsedArray);
for (int q=0;q<numUsedArray;q++)
bodyUsed[q]=0;
int curBodyUsed = 0;
int numIter = 0;
m_data->m_sortData.resize(0);
m_data->m_idxBuffer.resize(0);
m_data->m_old.resize(0);
#if defined(_DEBUG)
for(int i=0; i<numConstraints; i++)
cs[i].getBatchIdx() = -1;
#endif
int numValidConstraints = 0;
int unprocessedConstraintIndex = 0;
int batchIdx = 0;
{
B3_PROFILE("cpu batch innerloop");
while( numValidConstraints < numConstraints)
{
numIter++;
int nCurrentBatch = 0;
batchSizes[batchIdx] = 0;
// clear flag
for(int i=0; i<curBodyUsed; i++)
bodyUsed[curUsed[i]/32] = 0;
curBodyUsed = 0;
for(int i=numValidConstraints; i<numConstraints; i++)
{
int idx = i;
b3Assert( idx < numConstraints );
// check if it can go
int bodyAS = cs[idx].m_bodyAPtrAndSignBit;
int bodyBS = cs[idx].m_bodyBPtrAndSignBit;
int bodyA = abs(bodyAS);
int bodyB = abs(bodyBS);
bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;
bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;
int aUnavailable = 0;
int bUnavailable = 0;
if (!aIsStatic)
{
aUnavailable = bodyUsed[ bodyA/32 ] & (1<<(bodyA&31));
}
if (!aUnavailable)
if (!bIsStatic)
{
bUnavailable = bodyUsed[ bodyB/32 ] & (1<<(bodyB&31));
}
if( aUnavailable==0 && bUnavailable==0 ) // ok
{
if (!aIsStatic)
{
bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31));
curUsed[curBodyUsed++]=bodyA;
}
if (!bIsStatic)
{
bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31));
curUsed[curBodyUsed++]=bodyB;
}
cs[idx].getBatchIdx() = batchIdx;
if (i!=numValidConstraints)
{
b3Swap(cs[i],cs[numValidConstraints]);
numSwaps++;
}
numValidConstraints++;
{
nCurrentBatch++;
if( nCurrentBatch == simdWidth )
{
batchSizes[batchIdx] += simdWidth;
nCurrentBatch = 0;
for(int i=0; i<curBodyUsed; i++)
bodyUsed[curUsed[i]/32] = 0;
curBodyUsed = 0;
}
}
}
}
if (batchIdx>=B3_MAX_NUM_BATCHES)
{
b3Error("batchIdx>=B3_MAX_NUM_BATCHES");
b3Assert(0);
break;
}
batchSizes[batchIdx] += nCurrentBatch;
batchIdx ++;
}
}
#if defined(_DEBUG)
// debugPrintf( "nBatches: %d\n", batchIdx );
for(int i=0; i<numConstraints; i++)
{
b3Assert( cs[i].getBatchIdx() != -1 );
}
#endif
batchSizes[batchIdx] =0;
if (maxSwaps<numSwaps)
{
maxSwaps = numSwaps;
//printf("maxSwaps = %d\n", maxSwaps);
}
return batchIdx;
}
virtual void reportError(const char* error)
{
m_numErrors++;
b3Error(error);
}
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
{
constraint->setUserConstraintPtr(0);
m_constraints.push_back(constraint);
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
int rbA = p->getRigidBodyA().getUserIndex();
int rbB = p->getRigidBodyB().getUserIndex();
btVector3 pivotInB = p->getPivotInB();
if (rbB<=0)
{
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(p2p);
m_rigidBodyPipeline->addConstraint(p2p);
} else
{
b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
}
break;
}
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
int rbA = dof2->getRigidBodyA().getUserIndex();
int rbB = dof2->getRigidBodyB().getUserIndex();
btTransform frameInA = dof2->getFrameOffsetB();
btTransform frameInB = dof2->getFrameOffsetB();
if (rbA<=0)
{
frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
rbA = m_staticBody->getUserIndex();
}
if (rbB<=0)
{
frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
{
btVector3 limit(0,0,0);
dof2->getLinearLowerLimit(limit);
dof3->setLinearLowerLimit((const b3Vector3&)limit);
dof2->setLinearUpperLimit(limit);
dof3->setLinearUpperLimit((const b3Vector3&)limit);
dof2->setAngularLowerLimit(limit);
dof3->setAngularLowerLimit((const b3Vector3&)limit);
dof2->setAngularUpperLimit(limit);
dof3->setAngularUpperLimit((const b3Vector3&)limit);
/* for (int i=0;i<6;i++)
{
dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
}
*/
dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
}
// p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(dof3);
m_rigidBodyPipeline->addConstraint(dof3);
} else
{
b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
}
// b3Generic6DofConstraint
break;
}
default:
b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
};
}
virtual void reportError(const char* error)
{
b3Error(error);
}
void PhysicsServer::stepSimulation(float deltaTime)
{
bool wantsShutdown = false;
if (m_testBlock1)
{
///we ignore overflow of integer for now
if (m_testBlock1->m_numClientCommands> m_testBlock1->m_numProcessedClientCommands)
{
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1);
const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0];
m_testBlock1->m_numProcessedClientCommands++;
//consume the command
switch (clientCmd.m_type)
{
case CMD_LOAD_URDF:
{
b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (completedOk)
{
serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
} else
{
serverCmd.m_type =CMD_URDF_LOADING_FAILED;
}
m_testBlock1->m_numServerCommands++;
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3Printf("Step simulation request");
double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
m_dynamicsWorld->stepSimulation(timeStep);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++;
break;
}
case CMD_SHUTDOWN:
{
wantsShutdown = true;
break;
}
default:
{
b3Error("Unsupported command encountered");
btAssert(0);
}
};
//process the command right now
}
}
if (wantsShutdown)
{
m_wantsShutdown = true;
releaseSharedMemory();
}
}
void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
{
B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
if (0)
{
calculateOverlappingPairsHost(maxPairs);
/*
b3AlignedObjectArray<b3Int4> cpuPairs;
m_gpuPairs.copyToHost(cpuPairs);
printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
for (int i=0;i<m_gpuPairs.size();i++)
{
printf("host pair %d = %d,%d\n",i,cpuPairs[i].x,cpuPairs[i].y);
}
*/
return;
}
int numSmallAabbs = m_smallAabbsMappingGPU.size();
b3OpenCLArray<int> pairCount(m_context,m_queue);
pairCount.push_back(0);
m_gpuPairs.resize(maxPairs);//numSmallAabbs*maxPairsPerBody);
{
int numLargeAabbs = m_largeAabbsMappingGPU.size();
if (numLargeAabbs && numSmallAabbs)
{
B3_PROFILE("sap2Kernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( m_allAabbsGPU1.getBufferCL() ),
b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ),
b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ),
b3BufferInfoCL( m_gpuPairs.getBufferCL() ),
b3BufferInfoCL(pairCount.getBufferCL())};
b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( numLargeAabbs );
launcher.setConst( numSmallAabbs);
launcher.setConst( 0 );//axis is not used
launcher.setConst( maxPairs );
//@todo: use actual maximum work item sizes of the device instead of hardcoded values
launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64);
int numPairs = pairCount.at(0);
if (numPairs >maxPairs)
{
b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
numPairs =maxPairs;
}
}
}
if (numSmallAabbs)
{
B3_PROFILE("gridKernel");
m_hashGpu.resize(numSmallAabbs);
{
B3_PROFILE("kCalcHashAABB");
b3LauncherCL launch(m_queue,kCalcHashAABB,"kCalcHashAABB");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_allAabbsGPU1.getBufferCL());
launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
launch.setBuffer(m_hashGpu.getBufferCL());
launch.setBuffer(this->m_paramsGPU.getBufferCL());
launch.launch1D(numSmallAabbs);
}
m_sorter->execute(m_hashGpu);
int numCells = this->m_paramsCPU.m_gridSize[0]*this->m_paramsCPU.m_gridSize[1]*this->m_paramsCPU.m_gridSize[2];
m_cellStartGpu.resize(numCells);
//b3AlignedObjectArray<int > cellStartCpu;
{
B3_PROFILE("kClearCellStart");
b3LauncherCL launch(m_queue,kClearCellStart,"kClearCellStart");
launch.setConst(numCells);
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.launch1D(numCells);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindCellStart");
b3LauncherCL launch(m_queue,kFindCellStart,"kFindCellStart");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_hashGpu.getBufferCL());
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.launch1D(numSmallAabbs);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindOverlappingPairs");
b3LauncherCL launch(m_queue,kFindOverlappingPairs,"kFindOverlappingPairs");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_allAabbsGPU1.getBufferCL());
launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
launch.setBuffer(m_hashGpu.getBufferCL());
launch.setBuffer(m_cellStartGpu.getBufferCL());
launch.setBuffer(m_paramsGPU.getBufferCL());
//launch.setBuffer(0);
launch.setBuffer(pairCount.getBufferCL());
launch.setBuffer(m_gpuPairs.getBufferCL());
launch.setConst(maxPairs);
launch.launch1D(numSmallAabbs);
int numPairs = pairCount.at(0);
if (numPairs >maxPairs)
{
b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
numPairs =maxPairs;
}
m_gpuPairs.resize(numPairs);
if (0)
{
b3AlignedObjectArray<b3Int4> pairsCpu;
m_gpuPairs.copyToHost(pairsCpu);
printf("m_gpuPairs.size()=%d\n",m_gpuPairs.size());
for (int i=0;i<m_gpuPairs.size();i++)
{
printf("pair %d = %d,%d\n",i,pairsCpu[i].x,pairsCpu[i].y);
}
printf("?!?\n");
}
}
}
//calculateOverlappingPairsHost(maxPairs);
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
command.m_updateFlags =
URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE;
command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = 0;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_DESIRED_STATE:
{
command.m_type =CMD_SEND_DESIRED_STATE;
cl->prepareControlCommand(command);
cl->enqueueCommand(command);
break;
}
case CMD_RESET_SIMULATION:
{
command.m_type = CMD_RESET_SIMULATION;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
command.m_type = buttonId;
cl->enqueueCommand(command);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
{
Win32SharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_buf;
}
}
Win32SharedMemorySegment seg;
seg.m_key = key;
#ifdef UNICODE
swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key);
#else
sprintf(seg.m_szName,"MyFileMappingObject%d",key);
#endif
seg.m_hMapFile = OpenFileMapping(
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
seg.m_szName); // name of mapping object
if (seg.m_hMapFile==NULL)
{
if (allowCreation)
{
seg.m_hMapFile = CreateFileMapping(
INVALID_HANDLE_VALUE, // use paging file
NULL, // default security
PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD)
seg.m_szName); // name of mapping object
} else
{
b3Warning("Could not create file mapping object (%d).\n",GetLastError());
return 0;
}
}
seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission
0,
0,
size);
if (seg.m_buf == NULL)
{
b3Warning("Could not map view of file (%d).\n",GetLastError());
CloseHandle(seg.m_hMapFile);
return 0;
}
m_internalData->m_segments.push_back(seg);
return seg.m_buf;
}
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
RobotControlExample* cl = (RobotControlExample*) userPtr;
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION;
sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//r2d2.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
//#ifdef USE_C_API
b3InitPhysicsParamCommand(&command);
b3PhysicsParamSetGravity(&command, 1,1,-10);
// #else
//
// command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
// command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
// command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
// command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
// command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
// #endif // USE_C_API
cl->enqueueCommand(command);
break;
};
case CMD_INIT_POSE:
{
///@todo: implement this
command.m_type = CMD_INIT_POSE;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_DESIRED_STATE:
{
command.m_type =CMD_SEND_DESIRED_STATE;
cl->prepareControlCommand(command);
cl->enqueueCommand(command);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
command.m_type = buttonId;
sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet");
command.m_dataStreamArguments.m_streamChunkLength = 0;
cl->enqueueCommand(command);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}