diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 9b493d0f4..b9be5dfdf 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -434,9 +434,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; - btVector3* linesFrom = (btVector3*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; - btVector3* linesTo = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); - btVector3* linesColor = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + 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); @@ -444,9 +444,13 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt for (int i=0;im_debugLinesFrom[i] = linesFrom[i]; - m_data->m_debugLinesTo[i] = linesTo[i]; - m_data->m_debugLinesColor[i] = linesColor[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; } @@ -566,4 +570,4 @@ const btVector3* PhysicsClientSharedMemory::getDebugLinesColor() const int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 190fb6f50..ca83b90c0 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -107,7 +107,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"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; diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 665fe6694..e90b04676 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -549,6 +549,7 @@ void PhysicsServerSharedMemory::processClientCommands() { int curFlags =m_data->m_debugDrawer->getDebugMode(); int debugMode = btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; + m_data->m_debugDrawer->m_lines.resize(0); //|btIDebugDraw::DBG_DrawAabb| // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; m_data->m_debugDrawer->setDebugMode(debugMode); @@ -560,16 +561,24 @@ void PhysicsServerSharedMemory::processClientCommands() if (memRequirementsm_testBlock1->m_bulletStreamDataServerToClient[0]; - btVector3* linesTo = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); - btVector3* linesColor = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + 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)); for (int i=0;im_debugDrawer->m_lines[i].m_from; - linesTo[i] = m_data->m_debugDrawer->m_lines[i].m_to; - linesColor[i] = m_data->m_debugDrawer->m_lines[i].m_color; - } + linesFrom[i*4] = m_data->m_debugDrawer->m_lines[i].m_from.x(); + linesTo[i*4] = m_data->m_debugDrawer->m_lines[i].m_to.x(); + linesColor[i*4] = m_data->m_debugDrawer->m_lines[i].m_color.x(); + + linesFrom[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_from.y(); + linesTo[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_to.y(); + linesColor[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_color.y(); + + linesFrom[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_from.z(); + linesTo[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_to.z(); + linesColor[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_color.z(); +} SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); status.m_sendDebugLinesArgs.m_numDebugLines = numLines; m_data->submitServerStatus(status);