コード例 #1
0
ファイル: OSVRWrapper.cpp プロジェクト: NGenesis/OSVRWrapper
// Retrieve Current Orientation From HMD
OSVRWRAPPER_API void OSVR_GetHMDRotation(double *w, double *x, double *y, double *z)
{
	*w = osvrQuatGetW(&CurrentRotation);
	*x = osvrQuatGetX(&CurrentRotation);
	*y = osvrQuatGetY(&CurrentRotation);
	*z = osvrQuatGetZ(&CurrentRotation);

}
コード例 #2
0
ファイル: TrackerCallback.cpp プロジェクト: 6DB/OSVR-Core
void myOrientationCallback(void * /*userdata*/,
                           const OSVR_TimeValue * /*timestamp*/,
                           const OSVR_OrientationReport *report) {
    std::cout << "Got ORIENTATION report: Orientation = ("
              << osvrQuatGetW(&(report->rotation)) << ", ("
              << osvrQuatGetX(&(report->rotation)) << ", "
              << osvrQuatGetY(&(report->rotation)) << ", "
              << osvrQuatGetZ(&(report->rotation)) << "))" << std::endl;
}
コード例 #3
0
void myTrackerCallback(void * /*userdata*/,
                       const OSVR_TimeValue * /*timestamp*/,
                       const OSVR_PoseReport *report) {
    std::cout << "Got POSE report: Position = ("
              << report->pose.translation.data[0] << ", "
              << report->pose.translation.data[1] << ", "
              << report->pose.translation.data[2] << "), orientation = ("
              << osvrQuatGetW(&(report->pose.rotation)) << ", ("
              << osvrQuatGetX(&(report->pose.rotation)) << ", "
              << osvrQuatGetY(&(report->pose.rotation)) << ", "
              << osvrQuatGetZ(&(report->pose.rotation)) << "))" << std::endl;
}
コード例 #4
0
ファイル: OSVRWrapper.cpp プロジェクト: NGenesis/OSVRWrapper
// Extract Roll Angle From Quaternion
double osvrQuatGetRoll(const OSVR_Quaternion *q)
{
	return atan2(2.0f * osvrQuatGetX(q) * osvrQuatGetW(q) + 2.0f * osvrQuatGetZ(q) * osvrQuatGetY(q), 1.0f - 2.0f * (osvrQuatGetY(q) * osvrQuatGetY(q) + osvrQuatGetW(q) * osvrQuatGetW(q)));

}
コード例 #5
0
ファイル: OSVRWrapper.cpp プロジェクト: NGenesis/OSVRWrapper
// OSVR Helper Functions
// Extract Pitch Angle From Quaternion
double osvrQuatGetPitch(const OSVR_Quaternion *q)
{
	return asin(2.0f * (osvrQuatGetX(q) * osvrQuatGetY(q) - osvrQuatGetW(q) * osvrQuatGetZ(q)));

}
コード例 #6
0
void myOrientationCallback(void * /*userdata*/,
                           const OSVR_TimeValue * /*timestamp*/,
                           const OSVR_OrientationReport *report) {

	//Get updated values here
	now_t = clock();
	float diff_t = now_t - last_t;
	// read values every 200ms
	if (diff_t > 200)
	{
		std::cout << diff_t << std::endl;
		last_t = now_t;

		std::ostringstream  strserial;

		
		qtn.setQuaternion((float)osvrQuatGetW(&(report->rotation)), (float)osvrQuatGetX(&(report->rotation)),
			(float)osvrQuatGetY(&(report->rotation)), (float)osvrQuatGetZ(&(report->rotation)));

		/*
		std::cout << osvrQuatGetW(&(report->rotation)) << "w"
			<< osvrQuatGetX(&(report->rotation)) << "x"
			<< osvrQuatGetY(&(report->rotation)) << "y"
			<< osvrQuatGetZ(&(report->rotation)) << "z" << std::endl;
			*/
		//qtn.Normalize();
		EulerAnglesStruct eaStruct = qtn.getEulerAngles();
		
		float norm_yaw = eaStruct.yaw;
		if (eaStruct.yaw < 0)
		{
			norm_yaw = 180 + eaStruct.yaw + 180;

		}
		else
		{
			norm_yaw = eaStruct.yaw;
		}

		if (startangle == 9999)
		{
			//don't move camera, just record this as the point of origin

			startangle = eaStruct.yaw;

		
		}
		else
		{

			//sets up angle diff
			float diff_angle = 0;
			//
			diff_angle = eaStruct.yaw - startangle;

			diff_angle = (int)diff_angle % 360;

			if (diff_angle > 0)
			{
				if (diff_angle > 90)
				{
					diff_angle = 0;
				}
				else
				{
					diff_angle = diff_angle > 180 ? 360 - diff_angle : diff_angle;
				}
			}
			else
			{
				diff_angle = diff_angle < -180 ? +360 + diff_angle : diff_angle;
			}
			//sends serial info to arduino to get parsed to gimbal action
			strserial << ceil(eaStruct.pitch) << "x" << diff_angle << "g";
			
			const std::string tmp = strserial.str();
			const char* cstr = tmp.c_str();//converts C++ string to c style string for sending via serial lib
			if (serial_setup_success)
			{
				ssend.sendStuff(cstr);
			}
			else
				std::cout << "com fail "<<std::endl;

			
			std::cout <<strserial.str() << std::endl;

		}

	}
}
コード例 #7
0
ファイル: OSVR-Tracker.cpp プロジェクト: leiwen83/Perception
/**
* Handle OSVR tracking.
***/
void* OSVR_Tracker::Provoke(void* pThis, int eD3D, int eD3DInterface, int eD3DMethod, DWORD dwNumberConnected, int& nProvokerIndex)
{
	// update game timer
	m_cGameTimer.Tick();

	static UINT unFrameSkip = 200;
	if (unFrameSkip > 0)
	{
		unFrameSkip--;
		return nullptr;
	}

	if ((!m_psOSVR_ClientContext) || (!m_psOSVR_ClientInterface))
	{
		// create client context handle
		m_psOSVR_ClientContext =
			osvrClientInit("com.mtbs3d.vireio.osvr.tracker", 0);

		// get client interface
		osvrClientGetInterface(m_psOSVR_ClientContext, "/me/head", &m_psOSVR_ClientInterface);
	}
	else
	{
		// update the client context
		osvrClientUpdate(m_psOSVR_ClientContext);

		// let's read the tracker state.
		OSVR_ReturnCode cRet =
			osvrGetPoseState(m_psOSVR_ClientInterface, &m_sTimestamp, &m_sState);
		if (cRet != OSVR_RETURN_SUCCESS)
		{
			OutputDebugStringA("No pose state!\n");
		}
		else
		{
			m_bControlUpdate = true;

			// backup old euler angles and velocity
			float afEulerOld[3];
			float afEulerVelocityOld[3];
			memcpy(&afEulerOld[0], &m_afEuler[0], sizeof(float)* 3);
			memcpy(&afEulerVelocityOld[0], &m_afEulerVelocity[0], sizeof(float)* 3);

			// quaternion -> euler angles
			const float w = (float)m_sState.rotation.data[0];
			const float x = (float)m_sState.rotation.data[1];
			const float y = (float)m_sState.rotation.data[2];
			const float z = (float)m_sState.rotation.data[3];

			float sqw = w*w;
			float sqx = x*x;
			float sqy = y*y;
			float sqz = z*z;

			float unit = sqx + sqy + sqz + sqw;
			float test = x*y + z*w;

			if (test > 0.499*unit)
			{
				// singularity at north pole
				m_afEuler[1] = 2 * atan2(x, w);
				m_afEuler[2] = FLOAT_PI / 2;
				m_afEuler[0] = 0;
			}
			else if (test < -0.499*unit)
			{
				// singularity at south pole
				m_afEuler[1] = -2 * atan2(x, w);
				m_afEuler[2] = -FLOAT_PI / 2;
				m_afEuler[0] = 0;
			}
			else
			{
				m_afEuler[1] = atan2(2 * y*w - 2 * x*z, sqx - sqy - sqz + sqw);
				m_afEuler[2] = asin(2 * test / unit);
				m_afEuler[0] = atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw);
			}

			// PITCH = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
			// ROLL = atan2(2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z);

			// get euler velocity + acceleration
			float afEulerAcceleration[3];
			for (UINT unI = 0; unI < 3; unI++)
			{
				// get the velocity
				m_afEulerVelocity[unI] = (m_afEuler[unI] - afEulerOld[unI]) / (float)m_cGameTimer.DeltaTime();

				// get the acceleration
				afEulerAcceleration[unI] = (m_afEulerVelocity[unI] - afEulerVelocityOld[unI]) / (float)m_cGameTimer.DeltaTime();
			}

			// get predicted euler
			for (UINT unI = 0; unI < 3; unI++)
			{
				// compute predicted euler
				m_afEulerPredicted[unI] = (0.5f * afEulerAcceleration[unI] * ((float)m_cGameTimer.DeltaTime() * (float)m_cGameTimer.DeltaTime())) + (m_afEulerVelocity[unI] * (float)m_cGameTimer.DeltaTime()) + m_afEuler[unI];
			}

			// set position
			m_afPosition[0] = (float)m_sState.translation.data[0];
			m_afPosition[1] = (float)m_sState.translation.data[1];
			m_afPosition[2] = (float)m_sState.translation.data[2];

#ifdef _DEBUG
			// output debug data
			std::wstringstream szPose;
			szPose << L"Got POSE state: Position = ("
				<< m_sState.translation.data[0] << L", "
				<< m_sState.translation.data[1] << L", "
				<< m_sState.translation.data[2] << L"), orientation = ("
				<< osvrQuatGetW(&(m_sState.rotation)) << L", "
				<< osvrQuatGetX(&(m_sState.rotation)) << L", "
				<< osvrQuatGetY(&(m_sState.rotation)) << L", "
				<< osvrQuatGetZ(&(m_sState.rotation)) << L")";
			OutputDebugString(szPose.str().c_str());
#endif
		}
	}

	return nullptr;
}