diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index ca44372a..c296ca3b 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -3751,7 +3751,19 @@ void renderLoopClosure( static_cast(pointClouds[idx].render_color[2] * 255.f), 255 }; scan_renderer.drawCachedWithTransform( - static_cast(idx), delta, c, static_cast(pointClouds[idx].point_size), ScanColorMode::Flat); + static_cast(idx), + delta, + c, + static_cast(pointClouds[idx].point_size), + ScanColorMode::Flat, + 0.f, + 1.f, + Eigen::Vector3d::Zero(), + 1.f, + point_clouds_container.xz_intersection, + point_clouds_container.yz_intersection, + point_clouds_container.xy_intersection, + static_cast(point_clouds_container.intersection_width)); } } } diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index f792808f..8e2da0be 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -113,8 +113,9 @@ class ScanRenderer // useInitialPose draws every scan at its m_initial_pose instead of its // current m_pose (ports the legacy PointCloud::render()'s // show_with_initial_pose branch). The cached GPU buffers stay baked at - // m_pose; the per-scan delta (m_initial_pose * m_pose^-1) is folded into - // the MVP, the same trick drawCachedWithTransform() uses to preview a pose. + // m_pose; the per-scan delta (m_initial_pose * m_pose^-1) is applied in + // the vertex shader, the same trick drawCachedWithTransform() uses to + // preview a pose, so the slab filter and gradients follow the shown pose. void draw( const std::vector& pointClouds, float pointSize, @@ -198,8 +199,9 @@ class ScanRenderer // re-upload -- displaced by an extra transform on top of the pose it // was cached at. extraTransform is the delta from the scan's current // m_pose to the desired preview pose (i.e. previewPose * m_pose.inverse()); - // it's folded into the MVP matrix for this draw call rather than applied - // to the points themselves. For overlays that need to preview a scan at + // it's applied in the vertex shader for this draw call rather than to the + // cached points themselves, ahead of the slab filter and Elevation/Distance + // gradients, so both follow the preview pose. For overlays that need to preview a scan at // a pose other than its stored one without paying for a CPU retransform // every frame (e.g. a loop-closure edge's in-progress relative pose, or a // gizmo-drag preview). colorMode selects the shader's gradient exactly as @@ -297,6 +299,7 @@ class ScanRenderer Shader shader_{}; bool shaderValid_ = false; int locMVP_ = -1; + int locModel_ = -1; int locPointSize_ = -1; int locColor_ = -1; int locColorMode_ = -1; diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 3a0a2110..070cdb8f 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -49,6 +49,7 @@ void ScanRenderer::init() if (shaderValid_) { locMVP_ = rlGetLocationUniform(shader_.id, "mvp"); + locModel_ = rlGetLocationUniform(shader_.id, "model"); locPointSize_ = rlGetLocationUniform(shader_.id, "pointSize"); locColor_ = rlGetLocationUniform(shader_.id, "pointColor"); locColorMode_ = rlGetLocationUniform(shader_.id, "colorMode"); @@ -198,8 +199,8 @@ void ScanRenderer::syncPoses(const std::vector& pointClouds) namespace { // Defined lower down with the other file-local helpers; forward declared - // here so draw()/drawTrajectories() can fold a per-scan initial-pose delta - // into the MVP. + // here so draw()/drawTrajectories() can set a per-scan initial-pose delta + // as the shader's model matrix. Matrix toRaylibMatrix(const Eigen::Affine3d& t); } // namespace @@ -234,6 +235,7 @@ void ScanRenderer::draw( rlEnableShader(shader_.id); rlSetUniformMatrix(locMVP_, mvp); + rlSetUniformMatrix(locModel_, MatrixIdentity()); rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locElevMin_, &elevationMin, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locElevMax_, &elevationMax, RL_SHADER_UNIFORM_FLOAT, 1); @@ -281,14 +283,14 @@ void ScanRenderer::draw( } // Points are cached in world space at m_pose (see rebuild()); to show - // them at m_initial_pose instead, prepend the delta - // (m_initial_pose * m_pose^-1) to the shared MVP for this scan only -- - // same technique as drawCachedWithTransform(). With useInitialPose off - // the MVP set once above is left untouched. + // them at m_initial_pose instead, set the delta + // (m_initial_pose * m_pose^-1) as this scan's model matrix -- same + // technique as drawCachedWithTransform(). With useInitialPose off the + // identity model set once above is left untouched. if (useInitialPose) { const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse(); - rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp)); + rlSetUniformMatrix(locModel_, toRaylibMatrix(delta)); } float color[4]; @@ -336,11 +338,6 @@ void ScanRenderer::draw( rlDisableVertexArray(); } - if (useInitialPose) - { - rlSetUniformMatrix(locMVP_, mvp); // undo the last scan's per-scan fold - } - rlDisableShader(); } @@ -437,17 +434,16 @@ void ScanRenderer::drawCachedWithTransform( rlDrawRenderBatchActive(); - Matrix mvpBase = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); - // MatrixMultiply(A, B) composes as "apply A, then B" (see draw()'s own - // modelview-then-projection use above) -- so this applies the delta - // transform to the cached points first, then the normal view/projection. - Matrix mvp = MatrixMultiply(toRaylibMatrix(extraTransform), mvpBase); + Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); float colorF[4] = { color.r / 255.f, color.g / 255.f, color.b / 255.f, color.a / 255.f }; int colorModeInt = static_cast(colorMode); rlEnableShader(shader_.id); rlSetUniformMatrix(locMVP_, mvp); + // The delta goes in the model matrix, not the MVP, so the slab filter and + // gradients see the displaced positions (see kPointVS). + rlSetUniformMatrix(locModel_, toRaylibMatrix(extraTransform)); rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); rlSetUniform(locColorMode_, &colorModeInt, RL_SHADER_UNIFORM_INT, 1); @@ -532,6 +528,7 @@ void ScanRenderer::drawPoints(const PointsGPU& gpu, Color color, float pointSize rlEnableShader(shader_.id); rlSetUniformMatrix(locMVP_, mvp); + rlSetUniformMatrix(locModel_, MatrixIdentity()); rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); @@ -706,15 +703,12 @@ void ScanRenderer::drawTrajectories( return; } - // Hoisted out of the `if (shaderValid_)` block below so the per-scan - // initial-pose MVP fold in the draw loop can use it as the base. - const Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); - if (shaderValid_) { rlDrawRenderBatchActive(); rlEnableShader(shader_.id); - rlSetUniformMatrix(locMVP_, mvp); + rlSetUniformMatrix(locMVP_, MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection())); + rlSetUniformMatrix(locModel_, MatrixIdentity()); int colorModeFlat = 0; rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1); // Explicitly off -- see drawCachedWithTransform()'s comment on why. @@ -734,8 +728,8 @@ void ScanRenderer::drawTrajectories( } // Same initial-pose handling as draw(): the trajectory GPU buffer is - // cached at m_pose, so fold (m_initial_pose * m_pose^-1) into the MVP - // for the polyline; the CPU-side overlays just use shownPose directly. + // cached at m_pose, so set (m_initial_pose * m_pose^-1) as the model + // matrix for the polyline; the CPU-side overlays just use shownPose directly. const Eigen::Affine3d& shownPose = useInitialPose ? pc.m_initial_pose : pc.m_pose; if (visibleImuDiff) @@ -766,7 +760,7 @@ void ScanRenderer::drawTrajectories( if (useInitialPose) { const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse(); - rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp)); + rlSetUniformMatrix(locModel_, toRaylibMatrix(delta)); } float colorF[4] = { pc.traj_color[0], pc.traj_color[1], pc.traj_color[2], 1.0f }; rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); @@ -777,7 +771,7 @@ void ScanRenderer::drawTrajectories( rlDisableVertexArray(); if (useInitialPose) { - rlSetUniformMatrix(locMVP_, mvp); // restore for the next scan + rlSetUniformMatrix(locModel_, MatrixIdentity()); // restore for the next scan } } } diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp index 56cb4a39..eb4ab81f 100644 --- a/core/src/raylib_render_shaders.hpp +++ b/core/src/raylib_render_shaders.hpp @@ -16,20 +16,27 @@ namespace raylib_render_shaders // pointColor (0, the original app's only mode) and the ScanColorMode // gradients (1/2/3), matching the enum's Intensity/Elevation/Distance // ordering exactly (see ScanRenderer::draw()'s static_cast below). + // model displaces the cached world-space points to the pose actually shown + // (identity for a plain draw; a pose delta for drawCachedWithTransform() or + // useInitialPose). It is applied before fragWorldPos, not folded into mvp, so + // the intersection slab and the Elevation/Distance gradients follow the + // displaced points instead of the pose they were cached at. inline constexpr const char* kPointVS = R"( #version 330 in vec3 vertexPosition; in float vertexIntensity; uniform mat4 mvp; +uniform mat4 model; uniform float pointSize; out float fragIntensity; out vec3 fragWorldPos; void main() { - gl_Position = mvp * vec4(vertexPosition, 1.0); + vec4 worldPos = model * vec4(vertexPosition, 1.0); + gl_Position = mvp * worldPos; gl_PointSize = pointSize; fragIntensity = vertexIntensity; - fragWorldPos = vertexPosition; + fragWorldPos = worldPos.xyz; } )";