OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
OSVR_OrientationState orientation_x;
OSVR_OrientationState orientation_y;
OSVR_OrientationState orientation_z;
OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x);
OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y);
OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z);
OSVR_Vec3 rpy_x;
OSVR_Vec3 rpy_y;
OSVR_Vec3 rpy_z;
rpyFromQuaternion(&orientation_x, &rpy_x);
rpyFromQuaternion(&orientation_y, &rpy_y);
rpyFromQuaternion(&orientation_z, &rpy_z);
OSVR_Vec3 rpy;
osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x));
osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y));
osvrVec3SetZ(&rpy, osvrVec3GetZ(&rpy_z));
quaternionFromRPY(&rpy, orientation);
return OSVR_RETURN_SUCCESS;
}
OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position) {
OSVR_PositionState position_x;
OSVR_PositionState position_y;
OSVR_PositionState position_z;
OSVR_TimeValue timestamp;
OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], ×tamp, &position_x);
OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], ×tamp, &position_y);
OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], ×tamp, &position_z);
if (xret == OSVR_RETURN_SUCCESS) {
osvrVec3SetX(position, osvrVec3GetX(&position_x));
}
if (yret == OSVR_RETURN_SUCCESS) {
osvrVec3SetY(position, osvrVec3GetY(&position_y));
}
if (zret == OSVR_RETURN_SUCCESS) {
osvrVec3SetZ(position, osvrVec3GetZ(&position_z));
}
return OSVR_RETURN_SUCCESS;
}