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