// 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);
}
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;
}
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;
}
// 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)));
}
// 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)));
}
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;
}
}
}
/**
* 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;
}
