void AppStage_ComputeTrackerPoses::render()
{
switch (m_menuState)
{
case eMenuState::inactive:
break;
case eMenuState::pendingControllerListRequest:
case eMenuState::pendingControllerStartRequest:
case eMenuState::pendingTrackerListRequest:
case eMenuState::pendingTrackerStartRequest:
break;
case eMenuState::failedControllerListRequest:
case eMenuState::failedControllerStartRequest:
case eMenuState::failedTrackerListRequest:
case eMenuState::failedTrackerStartRequest:
break;
case eMenuState::verifyTrackers:
{
render_tracker_video();
} break;
case eMenuState::calibrateWithMat:
m_pCalibrateWithMat->render();
break;
case eMenuState::testTracking:
{
// Draw the chaperone origin axes
drawTransformedAxes(glm::mat4(1.0f), 100.f);
// Draw the frustum for each tracking camera.
// The frustums are defined in PSMove tracking space.
// We need to transform them into chaperone space to display them along side the HMD.
for (t_tracker_state_map_iterator tracker_iter = m_trackerViews.begin(); tracker_iter != m_trackerViews.end(); ++tracker_iter)
{
const ClientTrackerView *trackerView = tracker_iter->second.trackerView;
const PSMovePose trackerPose = trackerView->getTrackerPose();
const glm::mat4 trackerMat4 = psmove_pose_to_glm_mat4(trackerPose);
PSMoveFrustum frustum = trackerView->getTrackerFrustum();
// use color depending on tracking status
glm::vec3 color= does_tracker_see_any_controller(trackerView) ? k_psmove_frustum_color : k_psmove_frustum_color_no_track;
drawTextAtWorldPosition(glm::mat4(1.f), psmove_position_to_glm_vec3(trackerPose.Position), "#%d", trackerView->getTrackerId());
drawTransformedFrustum(glm::mat4(1.f), &frustum, color);
drawTransformedAxes(trackerMat4, 20.f);
}
// Draw the psmove model
for (t_controller_state_map_iterator controller_iter = m_controllerViews.begin(); controller_iter != m_controllerViews.end(); ++controller_iter)
{
const ClientControllerView *controllerView = controller_iter->second.controllerView;
const PSMoveTrackingColorType trackingColorType= controller_iter->second.trackingColorType;
PSMovePose controllerPose = controllerView->GetPose();
glm::mat4 controllerMat4 = psmove_pose_to_glm_mat4(controllerPose);
if (m_controllerViews.size() > 1)
{
drawTextAtWorldPosition(glm::mat4(1.f), psmove_position_to_glm_vec3(controllerPose.Position), "#%d", controllerView->GetControllerID());
}
drawController(controllerView, controllerMat4, trackingColorType);
drawTransformedAxes(controllerMat4, 10.f);
// Draw the acceleration and velocity arrows
{
const PSMovePhysicsData &physicsData = controllerView->GetPhysicsData();
const glm::mat4 originMat4= glm::translate(glm::mat4(1.f), psmove_position_to_glm_vec3(controllerPose.Position));
const glm::vec3 vel_endpoint = psmove_float_vector3_to_glm_vec3(physicsData.VelocityCmPerSec);
const glm::vec3 acc_endpoint = psmove_float_vector3_to_glm_vec3(physicsData.AccelerationCmPerSecSqr)*k_centimeters_to_meters;
const float vel= glm::length(vel_endpoint);
if (vel > k_positional_epsilon)
{
drawArrow(originMat4, glm::vec3(0.f), vel_endpoint, 0.1f, glm::vec3(0.f, 1.f, 1.f));
//drawTextAtWorldPosition(originMat4, vel_endpoint, "v=%.2fcm/s", vel);
}
const float acc = glm::length(acc_endpoint);
if (acc > k_positional_epsilon)
{
drawArrow(originMat4, glm::vec3(0.f), acc_endpoint, 0.1f, glm::vec3(1.f, 1.f, 0.f));
//drawTextAtWorldPosition(originMat4, acc_endpoint, "a=%.2fm/s^2", acc);
}
}
}
} break;
case eMenuState::showTrackerVideo:
{
render_tracker_video();
} break;
case eMenuState::calibrateStepFailed:
break;
default:
assert(0 && "unreachable");
}
}
void AppStage_ComputeTrackerPoses::render()
{
switch (m_menuState)
{
case eMenuState::inactive:
break;
case eMenuState::pendingControllerListRequest:
case eMenuState::pendingControllerStartRequest:
case eMenuState::pendingTrackerListRequest:
case eMenuState::pendingTrackerStartRequest:
break;
case eMenuState::failedControllerListRequest:
case eMenuState::failedControllerStartRequest:
case eMenuState::failedTrackerListRequest:
case eMenuState::failedTrackerStartRequest:
break;
case eMenuState::verifyHMD:
{
if (m_hmdView != nullptr)
{
PSMovePose pose = m_hmdView->getDisplayHmdPose();
glm::quat orientation(pose.Orientation.w, pose.Orientation.x, pose.Orientation.y, pose.Orientation.z);
glm::vec3 position(pose.Position.x, pose.Position.y, pose.Position.z);
glm::mat4 rot = glm::mat4_cast(orientation);
glm::mat4 trans = glm::translate(glm::mat4(1.0f), position);
glm::mat4 transform = trans * rot;
drawDK2Model(transform);
drawTransformedAxes(transform, 10.f);
}
{
PSMoveVolume volume;
if (m_app->getOpenVRContext()->getHMDTrackingVolume(volume))
{
drawTransformedVolume(glm::mat4(1.f), &volume, glm::vec3(0.f, 1.f, 1.f));
}
}
} break;
case eMenuState::verifyTrackers:
{
render_tracker_video();
} break;
case eMenuState::selectCalibrationType:
break;
case eMenuState::calibrateWithHMD:
m_pCalibrateWithHMD->render();
break;
case eMenuState::calibrateWithMat:
m_pCalibrateWithMat->render();
break;
case eMenuState::testTracking:
{
// Draw the origin axes
drawTransformedAxes(glm::mat4(1.0f), 100.f);
// Draw the HMD and tracking volume
if (m_hmdView != nullptr)
{
// Compute a transform that goes from HMD tracking space to PSMove tracking space
PSMovePose hmd_pose_at_origin = m_app->getOpenVRContext()->getHMDPoseAtPSMoveTrackingSpaceOrigin();
glm::mat4 tracking_space_transform = psmove_pose_to_glm_mat4(hmd_pose_at_origin);
glm::mat4 tracking_space_inv_transform = glm::inverse(tracking_space_transform);
// Put the HMD transform in PSMove tracking space
PSMovePose hmd_pose = m_hmdView->getDisplayHmdPose();
glm::mat4 hmd_transform = tracking_space_inv_transform * psmove_pose_to_glm_mat4(hmd_pose);
drawDK2Model(hmd_transform);
drawTransformedAxes(hmd_transform, 10.f);
PSMoveVolume volume;
if (m_app->getOpenVRContext()->getHMDTrackingVolume(volume))
{
drawTransformedVolume(tracking_space_inv_transform, &volume, glm::vec3(0.f, 1.f, 1.f));
}
}
// Draw the frustum for each tracking camera
for (t_tracker_state_map_iterator iter = m_trackerViews.begin(); iter != m_trackerViews.end(); ++iter)
{
const ClientTrackerView *trackerView = iter->second.trackerView;
{
PSMoveFrustum frustum = trackerView->getTrackerFrustum();
drawFrustum(&frustum, k_psmove_frustum_color);
}
{
PSMovePose pose = trackerView->getTrackerPose();
glm::mat4 cameraTransform = psmove_pose_to_glm_mat4(pose);
drawTransformedAxes(cameraTransform, 20.f);
}
}
// Draw the psmove model
{
PSMovePose pose = m_controllerView->GetPSMoveView().GetPose();
glm::mat4 worldTransform = psmove_pose_to_glm_mat4(pose);
drawPSMoveModel(worldTransform, glm::vec3(1.f, 1.f, 1.f));
drawTransformedAxes(worldTransform, 10.f);
}
} break;
case eMenuState::calibrateStepFailed:
break;
default:
assert(0 && "unreachable");
}
}
void AppStage_MagnetometerCalibration::render()
{
const float modelScale = 18.f;
glm::mat4 scaleAndRotateModelX90=
glm::rotate(
glm::scale(glm::mat4(1.f), glm::vec3(modelScale, modelScale, modelScale)),
90.f, glm::vec3(1.f, 0.f, 0.f));
PSMoveIntVector3 rawSampleExtents = (m_maxSampleExtent - m_minSampleExtent).unsafe_divide(2);
glm::vec3 boxMin = psmove_float_vector3_to_glm_vec3(m_minSampleExtent.castToFloatVector3());
glm::vec3 boxMax = psmove_float_vector3_to_glm_vec3(m_maxSampleExtent.castToFloatVector3());
glm::vec3 boxCenter = (boxMax + boxMin) * 0.5f;
glm::vec3 boxExtents = (boxMax - boxMin) * 0.5f;
glm::mat4 recenterMatrix =
glm::translate(glm::mat4(1.f), -eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.center));
switch (m_menuState)
{
case eCalibrationMenuState::waitingForStreamStartResponse:
{
} break;
case eCalibrationMenuState::failedStreamStart:
case eCalibrationMenuState::failedBadCalibration:
{
} break;
case eCalibrationMenuState::measureBExtents:
{
float r= clampf01(static_cast<float>(m_led_color_r) / 255.f);
float g= clampf01(static_cast<float>(m_led_color_g) / 255.f);
float basis= clampf01(static_cast<float>(m_led_color_b) / 255.f);
// Draw the psmove model in the middle
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(r, g, basis));
// Draw the sample point cloud around the origin
drawPointCloud(
recenterMatrix,
glm::vec3(1.f, 1.f, 1.f),
reinterpret_cast<float *>(&m_alignedSamples->magnetometerEigenSamples[0]),
m_sampleCount);
// Draw the sample bounding box
// Label the min and max corners with the min and max magnetometer readings
drawTransformedBox(recenterMatrix, boxMin, boxMax, glm::vec3(1.f, 1.f, 1.f));
drawTextAtWorldPosition(recenterMatrix, boxMin, "%d,%d,%d",
m_minSampleExtent.i, m_minSampleExtent.j, m_minSampleExtent.k);
drawTextAtWorldPosition(recenterMatrix, boxMax, "%d,%d,%d",
m_maxSampleExtent.i, m_maxSampleExtent.j, m_maxSampleExtent.k);
// Draw and label the extent axes
drawTransformedAxes(glm::mat4(1.f), boxExtents.x, boxExtents.y, boxExtents.z);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(boxExtents.x, 0.f, 0.f), "%d", rawSampleExtents.i);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(0.f, boxExtents.y, 0.f), "%d", rawSampleExtents.j);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(0.f, 0.f, boxExtents.z), "%d", rawSampleExtents.k);
// Draw the best fit ellipsoid
{
glm::mat3 basis = eigen_matrix3f_to_glm_mat3(m_sampleFitEllipsoid.basis);
glm::vec3 center = eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.center);
glm::vec3 extents = eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.extents);
drawEllipsoid(
recenterMatrix,
glm::vec3(0.f, 0.4f, 1.f),
basis, center, extents);
drawTextAtWorldPosition(
recenterMatrix,
center - basis[0]*extents.x,
"E:%.1f", m_sampleFitEllipsoid.error);
}
// Draw the current magnetometer direction
{
glm::vec3 m_start= boxCenter;
glm::vec3 m_end= psmove_float_vector3_to_glm_vec3(m_lastMagnetometer.castToFloatVector3());
drawArrow(recenterMatrix, m_start, m_end, 0.1f, glm::vec3(1.f, 0.f, 0.f));
drawTextAtWorldPosition(recenterMatrix, m_end, "M");
}
} break;
case eCalibrationMenuState::waitForGravityAlignment:
{
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(1.f, 1.f, 1.f));
// Draw the current direction of gravity
{
const float renderScale = 200.f;
glm::mat4 renderScaleMatrix =
glm::scale(glm::mat4(1.f), glm::vec3(renderScale, renderScale, renderScale));
glm::vec3 g= psmove_float_vector3_to_glm_vec3(m_lastAccelerometer);
drawArrow(
renderScaleMatrix,
glm::vec3(), g,
0.1f,
glm::vec3(0.f, 1.f, 0.f));
drawTextAtWorldPosition(renderScaleMatrix, g, "G");
}
} break;
case eCalibrationMenuState::measureBDirection:
{
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(1.f, 1.f, 1.f));
// Draw the current magnetometer direction
{
glm::vec3 m_start = boxCenter;
glm::vec3 m_end = psmove_float_vector3_to_glm_vec3(m_lastMagnetometer.castToFloatVector3());
drawArrow(recenterMatrix, m_start, m_end, 0.1f, glm::vec3(1.f, 0.f, 0.f));
drawTextAtWorldPosition(recenterMatrix, m_end, "M");
}
} break;
case eCalibrationMenuState::waitForSetCalibrationResponse:
{
} break;
case eCalibrationMenuState::failedSetCalibration:
{
} break;
case eCalibrationMenuState::complete:
{
// Get the orientation of the controller in world space (OpenGL Coordinate System)
glm::quat q= psmove_quaternion_to_glm_quat(m_controllerView->GetPSMoveView().GetOrientation());
glm::mat4 worldSpaceOrientation= glm::mat4_cast(q);
glm::mat4 worldTransform = glm::scale(worldSpaceOrientation, glm::vec3(modelScale, modelScale, modelScale));
drawPSMoveModel(worldTransform, glm::vec3(1.f, 1.f, 1.f));
drawTransformedAxes(glm::mat4(1.f), 200.f);
} break;
case eCalibrationMenuState::pendingExit:
{
} break;
default:
assert(0 && "unreachable");
}
}
void AppStage_GyroscopeCalibration::render()
{
const float bigModelScale = 10.f;
glm::mat4 scaleAndRotateModelX90=
glm::rotate(
glm::scale(glm::mat4(1.f), glm::vec3(bigModelScale, bigModelScale, bigModelScale)),
90.f, glm::vec3(1.f, 0.f, 0.f));
switch (m_menuState)
{
case eCalibrationMenuState::pendingTrackingSpaceSettings:
case eCalibrationMenuState::waitingForStreamStartResponse:
case eCalibrationMenuState::failedStreamStart:
case eCalibrationMenuState::failedTrackingSpaceSettings:
{
} break;
case eCalibrationMenuState::waitForStable:
{
drawController(m_controllerView, scaleAndRotateModelX90);
// Draw the current direction of gravity
{
const float renderScale = 200.f;
glm::mat4 renderScaleMatrix =
glm::scale(glm::mat4(1.f), glm::vec3(renderScale, renderScale, renderScale));
glm::vec3 g= -psm_vector3f_to_glm_vec3(m_lastCalibratedAccelerometer);
drawArrow(
renderScaleMatrix,
glm::vec3(), g,
0.1f,
glm::vec3(0.f, 1.f, 0.f));
drawTextAtWorldPosition(renderScaleMatrix, g, "G");
}
} break;
case eCalibrationMenuState::measureBiasAndDrift:
{
drawController(m_controllerView, scaleAndRotateModelX90);
} break;
case eCalibrationMenuState::measureComplete:
{
drawController(m_controllerView, scaleAndRotateModelX90);
} break;
case eCalibrationMenuState::test:
{
// Get the orientation of the controller in world space (OpenGL Coordinate System)
PSMQuatf controllerQuat;
if (PSM_GetControllerOrientation(m_controllerView->ControllerID, &controllerQuat) == PSMResult_Success)
{
glm::quat q= psm_quatf_to_glm_quat(controllerQuat);
glm::mat4 worldSpaceOrientation= glm::mat4_cast(q);
glm::mat4 worldTransform = glm::scale(worldSpaceOrientation, glm::vec3(1.f));
drawController(m_controllerView, worldTransform);
drawTransformedAxes(worldSpaceOrientation, 200.f);
drawTransformedAxes(glm::mat4(1.f), 200.f);
}
} break;
default:
assert(0 && "unreachable");
}
}
