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"); } }