PSMoveMatrix3x3 psmove_quaternion_to_psmove_matrix3x3(const PSMoveQuaternion &q) { glm::quat glm_quat= psmove_quaternion_to_glm_quat(q); glm::mat3 glm_mat3 = glm::mat3_cast(glm_quat); PSMoveMatrix3x3 result = glm_mat3_to_psmove_matrix3x3(glm_mat3); return result; }
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"); } }