C++ SensorFusion::GetOrientation方法代码示例

void InputTestApp::OnIdle()
{
    double curtime = pPlatform->GetAppTime();
 //   float  dt      = float(LastUpdate - curtime);
    LastUpdate     = curtime;
    
    if (pBox)
    {
        Quatf q = SFusion.GetOrientation();
        pBox->SetOrientation(q);

   // Test Euler conversion, alternative to the above:
   //     Vector3f euler;
   //     SFusion.GetOrientation().GetEulerABC<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&euler.y, &euler.x, &euler.z);
   //     Matrix4f mat = Matrix4f::RotationY(euler.y) * Matrix4f::RotationX(euler.x) * Matrix4f::RotationZ(euler.z);
   //  pBox->SetMatrix(mat);    

        // Update titlebar every 20th of a second.
        if ((curtime - LastTitleUpdate) > 0.05f)
        {
            char                          titleBuffer[512];
            SensorDevice::CoordinateFrame coord = SensorDevice::Coord_Sensor;
            if (pSensor)
                coord = pSensor->GetCoordinateFrame();

            OVR_sprintf(titleBuffer, 512, "OVR SensorBox %s %s  Ang: %0.3f",
                        (SFusion.IsGravityEnabled() ?  "" : "[Grav Off]"),
                        (coord == SensorDevice::Coord_HMD) ? "[HMD Coord]" : "",
                        CalcDownAngleDegrees(q));
            pPlatform->SetWindowTitle(titleBuffer);
            LastTitleUpdate = curtime;
        }
    }

    if (pBox2)
    {
        pBox2->SetOrientation(SFusion2.GetOrientation());
    }

    // Render
    int w, h;
    pPlatform->GetWindowSize(&w, &h);

    pRender->SetViewport(0, 0, w, h);

    pRender->Clear();
    pRender->BeginScene();

    pRender->SetProjection(Proj);
    pRender->SetDepthMode(1,1);
    
    Sc.Render(pRender, View);

    pRender->Present();

}

JNIEXPORT void JNICALL Java_de_fruitfly_ovr_OculusRift_pollSubsystem(JNIEnv *, jobject) {
	if (!Initialized) return;
	if (!pSensor) return;

	quaternion = FusionResult.GetOrientation();
	quaternion.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
}

extern "C" RIFTAPI_API int GetSensorSample()
{
   
	SFusion.GetOrientation().GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&Heading, &Pitch, &Roll);
	

	return 1;
}

//periodic procedure for sending messages to the controller
double OculusRiftService::onAction(){
  
     IPv4 ip;
     std::string str ="";
    str = getMyIP(&ip);
    if( str == "" )
        {
          std::cout <<"Theres been an error !" << std::endl;
      
        }
    // else
    //    std::cout << "IPv4 address stored in struct: ";
    //    std::cout << ip.b1<<"."<<ip.b2<<"."<<ip.b3<<"."<<ip.b4<< std::endl;
    
	float r_yaw, r_pitch, r_roll;

	Quatf q = pFusionResult->GetOrientation();
	Matrix4f bodyFrameMatrix(q);
	q.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&r_yaw, &r_pitch, &r_roll);

	std::vector<std::string> names;
	names = this->getAllConnectedEntitiesName();
	int entSize = names.size();

   // std::cout << "IPv4 Address:  " << str << std::endl;
   // std::cout << " entity " <<  entSize << std::endl;


	for(int i = 0; i < entSize; i++) {
    std::string msg = "ORS_DATA ";	
	    msg += str;
		msg += DoubleToString(r_yaw);
		msg += DoubleToString(r_pitch);
		msg += DoubleToString(r_roll);

		this->sendMsgToCtr(names[i], msg);
    std::cout << msg << std::endl;
	}


	return 0.1;  //time period
}

void InputTestApp::OnKey(KeyCode key, int chr, bool down, int modifiers)
{
    OVR_UNUSED2(chr, modifiers);

    switch (key)
    {
    case Key_Q:
        if (!down)
            pPlatform->Exit(0);
        break;

    case Key_F1:
        CurrentView = View_Perspective;
        SetView(CurrentView);
        //UpdateWindowTitle();
        break;
    case Key_F2:
        CurrentView = View_XY_DownZ;
        SetView(CurrentView);
        break;
    case Key_F3:
        CurrentView = View_XZ_UpY;
        SetView(CurrentView);
        break;

    case Key_R:
        if (down)
        {
            SFusion.Reset();
            SFusion2.Reset();
        }
        break;

    case Key_H:
        if (down && pSensor)
        {
            SensorDevice::CoordinateFrame coord = pSensor->GetCoordinateFrame();
            pSensor->SetCoordinateFrame(
                (coord == SensorDevice::Coord_Sensor) ?
                SensorDevice::Coord_HMD : SensorDevice::Coord_Sensor);
            SFusion.Reset();
            SFusion2.Reset();
        }
        break;

    case Key_G:
        if (down)
        {
            SFusion.SetGravityEnabled(!SFusion.IsGravityEnabled());
            SFusion2.SetGravityEnabled(SFusion.IsGravityEnabled());
        }
        break;

    case Key_A:

        if (down)
        {
            if (!pSensor2)
            {
                LogText("Angle: %2.3f\n", CalcDownAngleDegrees(SFusion.GetOrientation()));
            }
            else
            {
                LogText("Angle: %2.3f Secondary Sensor Angle: %2.3f\n",
                        CalcDownAngleDegrees(SFusion.GetOrientation()),
                        CalcDownAngleDegrees(SFusion2.GetOrientation()));
            }                        
        }
        break;

        /*
    case Key_End:
        if (!down)
        {
            OriAdjust = OriSensor.Conj();
            Sc.ViewPoint.SetOrientation(Quatf());
        }
        break; */
        default:
            break;
    }
}

void InputTestApp::OnIdle()
{
    double curtime = pPlatform->GetAppTime();
	time_t t = time(0);   // get time now
	struct tm * now = localtime(&t);

 //   float  dt      = float(LastUpdate - curtime);
    LastUpdate     = curtime;
    
    if (pBox)
    {
		Vector3f acceldata = SFusion.GetAcceleration();
		Vector3f gyrodata = SFusion.GetAngularVelocity();
		Vector3f magdata = SFusion.GetMagnetometer();	

        Quatf q = SFusion.GetOrientation();
        pBox->SetOrientation(q);

		//fstream outFile;
		//outFile.open("C://Users//Barrett//Documents//oculus_sensor_data.txt");
		// Output the sensor data to the text file
		ofstream outFile("C://Users//Barrett//Documents//oculus_sensor_data.csv", ios::app);
		outFile << \
			now->tm_sec << "," << \
			curtime << "," << \
			acceldata.x << "," << acceldata.y << "," << acceldata.z << "," << \
			gyrodata.x << "," << gyrodata.y << "," << gyrodata.z << "," << \
			magdata.x << "," << magdata.y << "," << magdata.z << "," << \
			q.x << "," << q.y << "," << q.z << q.w << "\n";
		
   // Test Euler conversion, alternative to the above:
   //     Vector3f euler;
   //     SFusion.GetOrientation().GetEulerABC<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&euler.y, &euler.x, &euler.z);
   //     Matrix4f mat = Matrix4f::RotationY(euler.y) * Matrix4f::RotationX(euler.x) * Matrix4f::RotationZ(euler.z);
   //  pBox->SetMatrix(mat);    

        // Update titlebar every 20th of a second.
        if ((curtime - LastTitleUpdate) > 0.05f)
        {
            char                          titleBuffer[512];
            SensorDevice::CoordinateFrame coord = SensorDevice::Coord_Sensor;
            if (pSensor)
                coord = pSensor->GetCoordinateFrame();

            OVR_sprintf(titleBuffer, 512, "OVR SensorBox %s %s  Ang: %0.3f",
                        (SFusion.IsGravityEnabled() ?  "" : "[Grav Off]"),
                        (coord == SensorDevice::Coord_HMD) ? "[HMD Coord]" : "",
                        CalcDownAngleDegrees(q));
            pPlatform->SetWindowTitle(titleBuffer);
            LastTitleUpdate = curtime;
        }
    }

    if (pBox2)
    {
        pBox2->SetOrientation(SFusion2.GetOrientation());
    }

    // Render
    int w, h;
    pPlatform->GetWindowSize(&w, &h);

    pRender->SetViewport(0, 0, w, h);

    pRender->Clear();
    pRender->BeginScene();

    pRender->SetProjection(Proj);
    pRender->SetDepthMode(1,1);
    
    Sc.Render(pRender, View);

    pRender->Present();

}

//.........这里部分代码省略.........
	for(int i = 0; i < MAX_OBJECTS; i++){
		objects[i] = smgr->addCubeSceneNode(2);
		objects[i]->setMaterialFlag(EMF_LIGHTING, false);
		objects[i]->setPosition( vector3df( (rand() % 30) - 5, (rand() % 30) - 5, rand() % 80) );
	}

	//device->setInputReceivingSceneManager(smgr);
	
	//použivane pro 
	vector3df tempRot; 
	irr::core::quaternion tempQ;
	irr::core::matrix4 tempM;

	float round = 0;

    while(device->run())
    {
		round += 0.01;
        driver->beginScene(true, true, SColor(255,100,101,140));
		
		for(int i = 0; i < MAX_OBJECTS; i++){
			vector3df tmpPos = objects[i]->getPosition();
			if(tmpPos.Z > pos.Z) continue;
			
			objects[i]->setPosition( vector3df( (rand() % 30) - 15, (rand() % 30) - 15, rand() % 80 + pos.Z) );
		}
		
	#ifndef OCCULUS
			tempM.setRotationDegrees(vector3df(sin(round*0.5)*360-180, sin(round)*360-180, cos(round*0.8)*360-180));
			
			// transform forward vector of camera
			irr::core::vector3df frv = irr::core::vector3df (0.0f, 0.0f, 1.0f);
			tempM.transformVect(frv);
    
			// transform upvector of camera
		    irr::core::vector3df upv = irr::core::vector3df (0.0f, 1.0f, 0.0f);
			tempM.transformVect(upv);

		    camera->setUpVector(upv); //set up vector of camera
			camera->setTarget(frv); //set target of camera (look at point) (thx Zeuss for correcting it)

	#endif

		
		if(pSensor){
			Quatf quaternion = FusionResult.GetOrientation();

		   ICameraSceneNode* camera = smgr->getActiveCamera();
   
		   tempQ.set(-quaternion.z,quaternion.y,-quaternion.x, quaternion.w);
		   tempQ.normalize();
		   tempQ.toEuler(tempRot);
    
		   
			tempM.setRotationDegrees(tempRot);

			// transform forward vector of camera
			irr::core::vector3df frv = irr::core::vector3df (0.0f, 0.0f, 1.0f);
			tempM.transformVect(frv);
    
			// transform upvector of camera
		    irr::core::vector3df upv = irr::core::vector3df (0.0f, 1.0f, 0.0f);
			tempM.transformVect(upv);

		    camera->setUpVector(upv); //set up vector of camera
			camera->setTarget(frv); //set target of camera (look at point) (thx Zeuss for correcting it)

			// update absolute position
			camera->updateAbsolutePosition();



			float yaw, pitch, roll;
			quaternion.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
			camera->getParent()->setRotation( vector3df(RadToDegree(pitch),RadToDegree(yaw),RadToDegree(roll)));
			//camera->setRotation( vector3df(RadToDegree(-pitch),RadToDegree(-yaw),RadToDegree(roll)));
			//camera->setProjectionMatrix(ToMatrix(quaternion));
			cout << " Yaw: " << RadToDegree(yaw) << 
				", Pitch: " << RadToDegree(pitch) << 
				", Roll: " << RadToDegree(roll) << endl;
		
			if (_kbhit()) exit(0);
		}

		#ifdef OCCULUS
			renderer.drawAll(smgr); 
		#else
			smgr->drawAll();
		#endif

        guienv->drawAll();

        driver->endScene();
    }


    device->drop();
	Clear();
    return 0;
}

void Output()
{
	cout << "----- Oculus Console -----" << endl;

	if (pHMD)
	{
		cout << " [x] HMD Found" << endl;
	}
	else
	{
		cout << " [ ] HMD Not Found" << endl;
	}

	if (pSensor)
	{
		cout << " [x] Sensor Found" << endl;
	}
	else
	{
		cout << " [ ] Sensor Not Found" << endl;
	}

	cout << "--------------------------" << endl;

	if (InfoLoaded)
        {
		cout << " DisplayDeviceName: " << Info.DisplayDeviceName << endl;
		cout << " ProductName: " << Info.ProductName << endl;
		cout << " Manufacturer: " << Info.Manufacturer << endl;
		cout << " Version: " << Info.Version << endl;
		cout << " HResolution: " << Info.HResolution<< endl;
		cout << " VResolution: " << Info.VResolution<< endl;
		cout << " HScreenSize: " << Info.HScreenSize<< endl;
		cout << " VScreenSize: " << Info.VScreenSize<< endl;
		cout << " VScreenCenter: " << Info.VScreenCenter<< endl;
		cout << " EyeToScreenDistance: " << Info.EyeToScreenDistance << endl;
		cout << " LensSeparationDistance: " << Info.LensSeparationDistance << endl;
		cout << " InterpupillaryDistance: " << Info.InterpupillaryDistance << endl;
		cout << " DistortionK[0]: " << Info.DistortionK[0] << endl;
		cout << " DistortionK[1]: " << Info.DistortionK[1] << endl;
		cout << " DistortionK[2]: " << Info.DistortionK[2] << endl;
		cout << "--------------------------" << endl;
        }

	cout << endl << " Press ENTER to continue" << endl;

	cin.get();

	while(pSensor)
	{
		Vector3f acc = FusionResult.GetAcceleration();
		Quatf quaternion = FusionResult.GetOrientation();

		float yaw, pitch, roll;
		quaternion.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);

		cout << "Yaw=" << RadToDegree(yaw) << 
			" Pitch=" << RadToDegree(pitch) << 
			" Roll=" << RadToDegree(roll) <<
			" X=" << acc.x / STD_GRAV << 
			" Y=" << acc.y / STD_GRAV << 
			" Z=" << acc.z / STD_GRAV << endl;

		Sleep(50);

		if (_kbhit()) exit(0);
	}
}