Exemplo n.º 1
0
	virtual void	physicsDebugDraw(int debugFlags)
	{
		if (m_options==eCLIENTEXAMPLE_SERVER)
		{
			m_physicsServer.physicsDebugDraw(debugFlags);
		}
	}
Exemplo n.º 2
0
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();
		}
	}
}
Exemplo n.º 4
0
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);
			}
		}
	}
}
Exemplo n.º 5
0
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;
	}
Exemplo n.º 7
0
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");
	}

}
Exemplo n.º 8
0
	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);
			}
		}
	}
Exemplo n.º 9
0
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);
  
}
Exemplo n.º 10
0
    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;
    };
Exemplo n.º 11
0
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);
}
Exemplo n.º 12
0
	void replayFromLogFile()
	{
		m_replay = true;
		m_physicsServer.replayFromLogFile("BulletPhysicsCommandLog.bin");
	}
Exemplo n.º 13
0
void    PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
	///debug rendering
	m_physicsServer.physicsDebugDraw(debugDrawFlags);

}
Exemplo n.º 14
0
    void enableCommandLogging()
	{
		m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
	}
Exemplo n.º 15
0
	virtual void	renderScene()
	{
		m_physicsServer.renderScene();
	}
Exemplo n.º 16
0
	virtual void	physicsDebugDraw(int debugFlags)
	{
		m_physicsServer.physicsDebugDraw(debugFlags);
		
	}
Exemplo n.º 17
0
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);
				}

			}
		}
	}
}
Exemplo n.º 18
0
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();
}
Exemplo n.º 20
0
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();
}
Exemplo n.º 21
0
void	PhysicsServerExample::stepSimulation(float deltaTime)
{
    m_physicsServer.processClientCommands();
}
Exemplo n.º 22
0
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();
		}
	}
}
Exemplo n.º 23
0
	virtual void setSharedMemoryKey(int key)
	{
		m_physicsServer.setSharedMemoryKey(key);
	}
Exemplo n.º 24
0
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");
	}

}
Exemplo n.º 25
0
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);
            }
        }
    }


}
Exemplo n.º 26
0
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();
}
Exemplo n.º 27
0
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);
				}