virtual void physicsDebugDraw(int debugFlags) { if (m_options==eCLIENTEXAMPLE_SERVER) { m_physicsServer.physicsDebugDraw(debugFlags); } }
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) { drawUserDebugLines(); ///debug rendering m_physicsServer.physicsDebugDraw(debugDrawFlags); }
void PhysicsServerExample::stepSimulation(float deltaTime) { if (m_replay) { for (int i=0;i<100;i++) m_physicsServer.processClientCommands(); } else { btClock rtc; btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); while (rtc.getTimeMilliseconds()<endTime) { m_physicsServer.processClientCommands(); } } }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; } } } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); SharedMemoryCommand& cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); m_physicsClient.submitClientCommand(cmd); } } } }
void PhysicsServerExample::stepSimulation(float deltaTime) { btClock rtc; btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); while (rtc.getTimeMilliseconds()<endTime) { m_physicsServer.processClientCommands(); } }
virtual bool mouseButtonCallback(int button, int state, float x, float y) { if (m_replay) return false; CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); if (!renderer) { btAssert(0); return false; } CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window; if (state==1) { if(button==0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL) )) { btVector3 camPos; renderer->getActiveCamera()->getCameraPosition(camPos); btVector3 rayFrom = camPos; btVector3 rayTo = getRayTo(int(x),int(y)); m_physicsServer.pickBody(rayFrom, rayTo); } } else { if (button==0) { m_physicsServer.removePickingConstraint(); //remove p2p } } //printf("button=%d, state=%d\n",button,state); return false; }
void RobotControlExample::initPhysics() { ///for this testing we use Z-axis up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); btVector3 grav(0,0,0); grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); bool allowSharedMemoryInitialization = true; m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); 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); createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger); createButton("Init Pose",CMD_INIT_POSE,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); */ } if (!m_physicsClient.connect()) { b3Warning("Cannot eonnect to physics client"); } }
virtual void renderScene() { if (m_options == eCLIENTEXAMPLE_SERVER) { m_physicsServer.renderScene(); } b3DebugLines debugLines; b3GetDebugLines(m_physicsClientHandle,&debugLines); int numLines = debugLines.m_numDebugLines; int lineWidth = 1; if (1) { btAlignedObjectArray<btVector3FloatData> points; points.resize(numLines*2); btAlignedObjectArray<unsigned int> indices; indices.resize(numLines*2); for (int i=0;i<numLines;i++) { points[i*2].m_floats[0] = debugLines.m_linesFrom[i*3+0]; points[i*2].m_floats[1] = debugLines.m_linesFrom[i*3+1]; points[i*2].m_floats[2] = debugLines.m_linesFrom[i*3+2]; points[i*2+1].m_floats[0] = debugLines.m_linesTo[i*3+0]; points[i*2+1].m_floats[1] = debugLines.m_linesTo[i*3+1]; points[i*2+1].m_floats[2] = debugLines.m_linesTo[i*3+2]; indices[i*2] = i*2; indices[i*2+1] = i*2+1; } float color[4] = {0.2,0.2,1,1}; if (points.size() && indices.size()) { m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],color,points.size(),sizeof(btVector3FloatData),&indices[0],indices.size(),lineWidth); } } else { for (int i=0;i<numLines;i++) { m_guiHelper->getRenderInterface()->drawLine(debugLines.m_linesFrom,debugLines.m_linesTo,debugLines.m_linesColor,lineWidth); } } }
void PhysicsServerExample::initPhysics() { ///for this testing we use Z-axis up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); btVector3 grav(0,0,0); grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); }
virtual bool mouseMoveCallback(float x,float y) { CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); if (!renderer) { btAssert(0); return false; } btVector3 rayTo = getRayTo(int(x), int(y)); btVector3 rayFrom; renderer->getActiveCamera()->getCameraPosition(rayFrom); m_physicsServer.movePickedBody(rayFrom,rayTo); return false; };
void PhysicsServerExample::initPhysics() { ///for this testing we use Z-axis up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS); for (int i=0;i<m_threadSupport->getNumTasks();i++) { MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i); b3Assert(storage); storage->threadId = i; //storage->m_sharedMem = data->m_sharedMem; } for (int w=0;w<MAX_MOTION_NUM_THREADS;w++) { m_args[w].m_cs = m_threadSupport->createCriticalSection(); m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized); int numMoving = 0; m_args[w].m_positions.resize(numMoving); m_args[w].m_physicsServerPtr = &m_physicsServer; int index = 0; m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w); while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized) { b3Clock::usleep(1000); } } m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs); m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper); }
void replayFromLogFile() { m_replay = true; m_physicsServer.replayFromLogFile("BulletPhysicsCommandLog.bin"); }
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) { ///debug rendering m_physicsServer.physicsDebugDraw(debugDrawFlags); }
void enableCommandLogging() { m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin"); }
virtual void renderScene() { m_physicsServer.renderScene(); }
virtual void physicsDebugDraw(int debugFlags) { m_physicsServer.physicsDebugDraw(debugFlags); }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //update sensor feedback: joint force/torque data and measured joint positions for (int i=0;i<m_numMotors;i++) { int jointIndex = m_motorTargetState[i].m_jointIndex; int positionIndex = m_motorTargetState[i].m_posIndex; int velocityIndex = m_motorTargetState[i].m_uIndex; m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex]; m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex]; m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]); m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]); if (m_motorTargetState[i].m_measuredJointPosition>0.1) { m_motorTargetState[i].m_velTarget = -1.5; } else { m_motorTargetState[i].m_velTarget = 1.5; } b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z()); b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z()); } } if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { SharedMemoryCommand sensorCommand; sensorCommand.m_type = CMD_CREATE_SENSOR; sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0; for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++) { b3JointInfo info; m_physicsClient.getJointInfo(jointIndex,info); if (m_verboseOutput) { b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { switch (m_option) { case ROBOT_VELOCITY_CONTROL: { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; break; } case ROBOT_PD_CONTROL: { char motorName[1024]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_kp = 1; motorInfo->m_kd = 0; { sprintf(motorName,"%s kp", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kp); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-SIMD_PI; slider.m_maxVal=SIMD_PI; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s kd", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kd); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q'", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-10; slider.m_maxVal=10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; break; } case ROBOT_PING_PONG_JOINT_FEEDBACK: { if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_jointIndex = jointIndex; sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex; sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true; sensorCommand.m_createSensorArguments.m_numJointSensorChanges++; m_numMotors++; } } break; } default: { b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); } }; } } } if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges) { enqueueCommand(sensorCommand); } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { if (m_verboseOutput) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); } SharedMemoryCommand cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); if (cmd.m_type == CMD_CREATE_SENSOR) { b3Printf("CMD_CREATE_SENSOR!\n"); } if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM) { char relativeFileName[1024]; bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024); if (fileFound) { FILE *fp = fopen(relativeFileName, "rb"); if (fp) { fseek(fp, 0L, SEEK_END); int mFileLen = ftell(fp); fseek(fp, 0L, SEEK_SET); if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { char* data = (char*)malloc(mFileLen); fread(data, mFileLen, 1, fp); fclose(fp); cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen; m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen); if (m_verboseOutput) { b3Printf("Loaded bullet data chunks into shared memory\n"); } free(data); } else { b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } } else { b3Warning("Cannot open file %s\n", relativeFileName); } } else { b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName); } } m_physicsClient.submitClientCommand(cmd); } else { if (m_numMotors) { SharedMemoryCommand command; command.m_type =CMD_SEND_DESIRED_STATE; prepareControlCommand(command); enqueueCommand(command); command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); command.m_type = CMD_REQUEST_ACTUAL_STATE; enqueueCommand(command); } } } } }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); if (m_verboseOutput) { b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { switch (m_option) { case ROBOT_VELOCITY_CONTROL: { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; break; } case ROBOT_PD_CONTROL: { char motorName[1024]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_kp = 1; motorInfo->m_kd = 0; { sprintf(motorName,"%s kp", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kp); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-SIMD_PI; slider.m_maxVal=SIMD_PI; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s kd", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kd); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q'", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-10; slider.m_maxVal=10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; break; } default: { b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); } }; } } } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { if (m_verboseOutput) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); } SharedMemoryCommand cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM) { char relativeFileName[1024]; bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024); if (fileFound) { FILE *fp = fopen(relativeFileName, "rb"); if (fp) { fseek(fp, 0L, SEEK_END); int mFileLen = ftell(fp); fseek(fp, 0L, SEEK_SET); if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { char* data = (char*)malloc(mFileLen); fread(data, mFileLen, 1, fp); fclose(fp); cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen; m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen); if (m_verboseOutput) { b3Printf("Loaded bullet data chunks into shared memory\n"); } free(data); } else { b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } } else { b3Warning("Cannot open file %s\n", relativeFileName); } } else { b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName); } } m_physicsClient.submitClientCommand(cmd); } else { if (m_numMotors) { SharedMemoryCommand command; command.m_type =CMD_SEND_DESIRED_STATE; prepareControlCommand(command); enqueueCommand(command); command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); } } } } }
void PhysicsServerExample::renderScene() { ///debug rendering m_physicsServer.renderScene(); }
void PhysicsServerExample::renderScene() { #if 0 ///little VR test to follow/drive Husky vehicle if (gHuskyId >= 0) { gVRTeleportPos1 = huskyTr.getOrigin(); gVRTeleportOrn = huskyTr.getRotation(); } #endif B3_PROFILE("PhysicsServerExample::RenderScene"); drawUserDebugLines(); if (gEnableRealTimeSimVR) { static int frameCount=0; static btScalar prevTime = m_clock.getTimeSeconds(); frameCount++; static btScalar worseFps = 1000000; int numFrames = 200; static int count = 0; count++; #if 0 if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); btScalar fps = 1. / (curTime - prevTime); prevTime = curTime; if (fps < worseFps) { worseFps = fps; } if (count > numFrames) { count = 0; sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2); sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep); gDroppedSimulationSteps = 0; worseFps = 1000000; } } #endif #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) { ComboBoxParams comboParams; comboParams.m_comboboxId = 0; comboParams.m_numItems = 0; comboParams.m_startItem = 0; comboParams.m_callback = 0;//MyComboBoxCallback; comboParams.m_userPointer = 0;//this; m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()); m_tinyVrGui->init(); } if (m_tinyVrGui) { b3Transform tr;tr.setIdentity(); tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2])); tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3])); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); static char line0[1024]; static char line1[1024]; m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); m_tinyVrGui->tick(dt,tr); } #endif//BT_ENABLE_VR } ///debug rendering //m_args[0].m_cs->lock(); //gVRTeleportPos[0] += 0.01; btTransform tr2a, tr2; tr2a.setIdentity(); tr2.setIdentity(); tr2.setOrigin(gVRTeleportPos1); tr2a.setRotation(gVRTeleportOrn); btTransform trTotal = tr2*tr2a; btTransform trInv = trTotal.inverse(); btMatrix3x3 vrOffsetRot; vrOffsetRot.setRotation(trInv.getRotation()); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { vrOffset[i + 4 * j] = vrOffsetRot[i][j]; } } vrOffset[12]= trInv.getOrigin()[0]; vrOffset[13]= trInv.getOrigin()[1]; vrOffset[14]= trInv.getOrigin()[2]; this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> getActiveCamera()->setVRCameraOffsetTransform(vrOffset); m_physicsServer.renderScene(); for (int i=0;i<MAX_VR_CONTROLLERS;i++) { if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i]) { btVector3 from = m_args[0].m_vrControllerPos[i]; btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]); btVector3 toX = from+mat.getColumn(0); btVector3 toY = from+mat.getColumn(1); btVector3 toZ = from+mat.getColumn(2); int width = 2; btVector4 color; color=btVector4(1,0,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); color=btVector4(0,1,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); color=btVector4(0,0,1,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); } } if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { if (!gEnableRealTimeSimVR) { gEnableRealTimeSimVR = true; m_physicsServer.enableRealTimeSimulation(1); } } //m_args[0].m_cs->unlock(); }
void PhysicsServerExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); }
void PhysicsServerExample::stepSimulation(float deltaTime) { //this->m_physicsServer.processClientCommands(); //check if any graphics related tasks are requested switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) { case eGUIHelperCreateCollisionShapeGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperCreateCollisionObjectGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj, m_multiThreadedHelper->m_color2); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperCreateRigidBodyGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperRegisterTexture: { m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels, m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperRegisterGraphicsShape: { m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape( m_multiThreadedHelper->m_vertices, m_multiThreadedHelper->m_numvertices, m_multiThreadedHelper->m_indices, m_multiThreadedHelper->m_numIndices, m_multiThreadedHelper->m_primitiveType, m_multiThreadedHelper->m_textureId); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperRegisterGraphicsInstance: { m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance( m_multiThreadedHelper->m_shapeIndex, m_multiThreadedHelper->m_position, m_multiThreadedHelper->m_quaternion, m_multiThreadedHelper->m_color, m_multiThreadedHelper->m_scaling); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperRemoveAllGraphicsInstances: { m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); b3Assert(numRenderInstances==0); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperCopyCameraImageData: { m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix, m_multiThreadedHelper->m_projectionMatrix, m_multiThreadedHelper->m_pixelsRGBA, m_multiThreadedHelper->m_rgbaBufferSizeInPixels, m_multiThreadedHelper->m_depthBuffer, m_multiThreadedHelper->m_depthBufferSizeInPixels, m_multiThreadedHelper->m_segmentationMaskBuffer, m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, m_multiThreadedHelper->m_startPixelIndex, m_multiThreadedHelper->m_destinationWidth, m_multiThreadedHelper->m_destinationHeight, m_multiThreadedHelper->m_numPixelsCopied); m_multiThreadedHelper->getCriticalSection()->lock(); m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->getCriticalSection()->unlock(); break; } case eGUIHelperIdle: default: { } } #if 0 if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) { btClock rtc; btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); while (rtc.getTimeMilliseconds()<endTime) { m_physicsServer.processClientCommands(); } } else { //for (int i=0;i<10;i++) m_physicsServer.processClientCommands(); } #endif { if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) { m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); } } }
virtual void setSharedMemoryKey(int key) { m_physicsServer.setSharedMemoryKey(key); }
void PhysicsClientExample::initPhysics() { if (m_guiHelper && m_guiHelper->getParameterInterface()) { int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createButtons(); } else { MyCallback(CMD_LOAD_URDF, true, this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_RESET_SIMULATION,true,this); } m_selectedBody = -1; m_prevSelectedBody = -1; if (m_options == eCLIENTEXAMPLE_SERVER) { m_canvas = m_guiHelper->get2dCanvasInterface(); if (m_canvas) { m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight); for (int i=0;i<camVisualizerWidth;i++) { for (int j=0;j<camVisualizerHeight;j++) { unsigned char red=255; unsigned char green=255; unsigned char blue=255; unsigned char alpha=255; if (i==j) { red = 0; green=0; blue=0; } m_canvas->setPixel(m_canvasIndex,i,j,red,green,blue,alpha); } } m_canvas->refreshImageData(m_canvasIndex); } m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); //m_physicsClientHandle = b3ConnectPhysicsDirect(); if (!b3CanSubmitCommand(m_physicsClientHandle)) { b3Warning("Cannot connect to physics client"); } }
void PhysicsClientExample::stepSimulation(float deltaTime) { if (m_options == eCLIENTEXAMPLE_SERVER) { for (int i=0;i<100;i++) { m_physicsServer.processClientCommands(); } } if (m_prevSelectedBody != m_selectedBody) { createButtons(); m_prevSelectedBody = m_selectedBody; } //while (!b3CanSubmitCommand(m_physicsClientHandle)) { b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { int statusType = b3GetStatusType(status); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //b3Printf("bla\n"); } if (statusType ==CMD_CAMERA_IMAGE_COMPLETED) { static int counter=0; char msg[1024]; sprintf(msg,"Camera image %d OK\n",counter++); b3CameraImageData imageData; b3GetCameraImageData(m_physicsClientHandle,&imageData); if (m_canvas && m_canvasIndex >=0) { for (int i=0;i<imageData.m_pixelWidth;i++) { for (int j=0;j<imageData.m_pixelHeight;j++) { int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth))); int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight))); btClamp(yIndex,0,imageData.m_pixelHeight); btClamp(xIndex,0,imageData.m_pixelWidth); int bytesPerPixel = 4; int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel; m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], imageData.m_rgbColorData[pixelIndex+2], 255); // imageData.m_rgbColorData[pixelIndex+3]); } } m_canvas->refreshImageData(m_canvasIndex); } b3Printf(msg); } if (statusType == CMD_CAMERA_IMAGE_FAILED) { b3Printf("Camera image FAILED\n"); } if (statusType == CMD_URDF_LOADING_COMPLETED) { int bodyIndex = b3GetStatusBodyIndex(status); if (bodyIndex>=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); for (int i=0;i<numJoints;i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } ComboBoxParams comboParams; comboParams.m_comboboxId = bodyIndex; comboParams.m_numItems = 1; comboParams.m_startItem = 0; comboParams.m_callback = MyComboBoxCallback; comboParams.m_userPointer = this; const char* bla = "bla"; const char* blarray[1]; blarray[0] = bla; comboParams.m_items=blarray;//{&bla}; m_guiHelper->getParameterInterface()->registerComboBox(comboParams); } } } } if (b3CanSubmitCommand(m_physicsClientHandle)) { if (m_userCommandRequests.size()) { //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); int commandId = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders if (commandId ==CMD_RESET_SIMULATION) { m_selectedBody = -1; m_numMotors=0; createButtons(); b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); if (m_options == eCLIENTEXAMPLE_SERVER) { b3SubmitClientCommand(m_physicsClientHandle, commandHandle); while (!b3CanSubmitCommand(m_physicsClientHandle)) { m_physicsServer.processClientCommands(); b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { int statusType = b3GetStatusType(status); b3Printf("Status after reset: %d",statusType); } } } else { prepareAndSubmitCommand(commandId); } } else { prepareAndSubmitCommand(commandId); } } else { if (m_numMotors) { enqueueCommand(CMD_SEND_DESIRED_STATE); enqueueCommand(CMD_STEP_FORWARD_SIMULATION); if (m_options != eCLIENTEXAMPLE_SERVER) { enqueueCommand(CMD_REQUEST_DEBUG_LINES); } //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } } } }
void PhysicsServerExample::renderScene() { B3_PROFILE("PhysicsServerExample::RenderScene"); static char line0[1024]; static char line1[1024]; if (gEnableRealTimeSimVR) { static int frameCount=0; static btScalar prevTime = m_clock.getTimeSeconds(); frameCount++; static btScalar worseFps = 1000000; int numFrames = 200; static int count = 0; count++; if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); btScalar fps = 1. / (curTime - prevTime); prevTime = curTime; if (fps < worseFps) { worseFps = fps; } if (count > numFrames) { count = 0; sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2); sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep); gDroppedSimulationSteps = 0; worseFps = 1000000; } } #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) { ComboBoxParams comboParams; comboParams.m_comboboxId = 0; comboParams.m_numItems = 0; comboParams.m_startItem = 0; comboParams.m_callback = 0;//MyComboBoxCallback; comboParams.m_userPointer = 0;//this; m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()); m_tinyVrGui->init(); } if (m_tinyVrGui) { b3Transform tr;tr.setIdentity(); tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2])); tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3])); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); m_tinyVrGui->tick(dt,tr); } #endif//BT_ENABLE_VR } ///debug rendering //m_args[0].m_cs->lock(); //gVRTeleportPos[0] += 0.01; vrOffset[12]=-gVRTeleportPos[0]; vrOffset[13]=-gVRTeleportPos[1]; vrOffset[14]=-gVRTeleportPos[2]; this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> getActiveCamera()->setVRCameraOffsetTransform(vrOffset); m_physicsServer.renderScene(); for (int i=0;i<MAX_VR_CONTROLLERS;i++) { if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i]) { btVector3 from = m_args[0].m_vrControllerPos[i]; btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]); btVector3 toX = from+mat.getColumn(0); btVector3 toY = from+mat.getColumn(1); btVector3 toZ = from+mat.getColumn(2); int width = 2; btVector4 color; color=btVector4(1,0,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); color=btVector4(0,1,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); color=btVector4(0,0,1,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); } } if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { gEnableRealTimeSimVR = true; } if (gDebugRenderToggle) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { B3_PROFILE("Draw Debug HUD"); //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); pos[0]+=gVRTeleportPos[0]; pos[1]+=gVRTeleportPos[1]; pos[2]+=gVRTeleportPos[2]; btTransform viewTr; btScalar m[16]; float mf[16]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf); for (int i=0;i<16;i++) { m[i] = mf[i]; } m[12]=+gVRTeleportPos[0]; m[13]=+gVRTeleportPos[1]; m[14]=+gVRTeleportPos[2]; viewTr.setFromOpenGLMatrix(m); btTransform viewTrInv = viewTr.inverse(); btVector3 side = viewTrInv.getBasis().getColumn(0); btVector3 up = viewTrInv.getBasis().getColumn(1); btVector3 fwd = viewTrInv.getBasis().getColumn(2); float upMag = 0; float sideMag = 2.2; float fwdMag = -4; m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1); //btVector3 fwd = viewTrInv.getBasis().getColumn(2); up = viewTrInv.getBasis().getColumn(1); upMag = -0.3; m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1); } //m_args[0].m_cs->unlock(); }
void PhysicsClientExample::stepSimulation(float deltaTime) { if (m_options == eCLIENTEXAMPLE_SERVER) { for (int i = 0; i < 100; i++) { m_physicsServer.processClientCommands(); } } if (m_prevSelectedBody != m_selectedBody) { createButtons(); m_prevSelectedBody = m_selectedBody; } //while (!b3CanSubmitCommand(m_physicsClientHandle)) { b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { int statusType = b3GetStatusType(status); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //b3Printf("bla\n"); } if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { // static int counter=0; // char msg[1024]; // sprintf(msg,"Camera image %d OK\n",counter++); b3CameraImageData imageData; b3GetCameraImageData(m_physicsClientHandle, &imageData); if (m_canvas) { //compute depth image range float minDepthValue = 1e20f; float maxDepthValue = -1e20f; for (int i = 0; i < camVisualizerWidth; i++) { for (int j = 0; j < camVisualizerHeight; j++) { int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth))); int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight))); btClamp(xIndex, 0, imageData.m_pixelWidth); btClamp(yIndex, 0, imageData.m_pixelHeight); if (m_canvasDepthIndex >= 0) { int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth); float depthValue = imageData.m_depthValues[depthPixelIndex]; //todo: rescale the depthValue to [0..255] if (depthValue > -1e20) { maxDepthValue = btMax(maxDepthValue, depthValue); minDepthValue = btMin(minDepthValue, depthValue); } } } } for (int i = 0; i < camVisualizerWidth; i++) { for (int j = 0; j < camVisualizerHeight; j++) { int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth))); int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight))); btClamp(yIndex, 0, imageData.m_pixelHeight); btClamp(xIndex, 0, imageData.m_pixelWidth); int bytesPerPixel = 4; //RGBA if (m_canvasRGBIndex >= 0) { int rgbPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth) * bytesPerPixel; m_canvas->setPixel(m_canvasRGBIndex, i, j, imageData.m_rgbColorData[rgbPixelIndex], imageData.m_rgbColorData[rgbPixelIndex + 1], imageData.m_rgbColorData[rgbPixelIndex + 2], 255); //alpha set to 255 } if (m_canvasDepthIndex >= 0) { int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth); float depthValue = imageData.m_depthValues[depthPixelIndex]; //todo: rescale the depthValue to [0..255] if (depthValue > -1e20) { int rgb = 0; if (maxDepthValue != minDepthValue) { rgb = (depthValue - minDepthValue) * (255. / (btFabs(maxDepthValue - minDepthValue))); if (rgb < 0 || rgb > 255) { //printf("rgb=%d\n",rgb); } } m_canvas->setPixel(m_canvasDepthIndex, i, j, rgb, rgb, 255, 255); //alpha set to 255 } else { m_canvas->setPixel(m_canvasDepthIndex, i, j, 0, 0, 0, 255); //alpha set to 255 } } if (m_canvasSegMaskIndex >= 0 && (0 != imageData.m_segmentationMaskValues)) { int segmentationMaskPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth); int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex]; btVector4 palette[4] = {btVector4(32, 255, 32, 255), btVector4(32, 32, 255, 255), btVector4(255, 255, 32, 255), btVector4(32, 255, 255, 255)}; if (segmentationMask >= 0) { int obIndex = segmentationMask & ((1 << 24) - 1); int linkIndex = (segmentationMask >> 24) - 1; btVector4 rgb = palette[(obIndex + linkIndex) & 3]; m_canvas->setPixel(m_canvasSegMaskIndex, i, j, rgb.x(), rgb.y(), rgb.z(), 255); //alpha set to 255 } else { m_canvas->setPixel(m_canvasSegMaskIndex, i, j, 0, 0, 0, 255); //alpha set to 255 } } } } if (m_canvasRGBIndex >= 0) m_canvas->refreshImageData(m_canvasRGBIndex); if (m_canvasDepthIndex >= 0) m_canvas->refreshImageData(m_canvasDepthIndex); if (m_canvasSegMaskIndex >= 0) m_canvas->refreshImageData(m_canvasSegMaskIndex); }