diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 5b306b3d..05d784ab 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -52,13 +53,22 @@ static const std::vector appShortcuts = { { "", "Ctrl+Shift+O", "Select CAMERA_0 directory" }, { "", "Ctrl+Shift+C", "Open calibration" }, { "", "Ctrl+S", "Export colored point cloud" }, + { "Camera", "F", "Front view" }, + { "", "B", "Back view" }, + { "", "L", "Left view" }, + { "", "R", "Right view" }, + { "", "T", "Top view" }, + { "", "U", "Bottom view" }, + { "", "I", "Isometric view" }, + { "", "Z", "Reset camera" }, + { "", "O", "Toggle orthographic/perspective" }, { "Special keys", "Left arrow", "Previous image (image preview)" }, { "", "Right arrow", "Next image (image preview)" }, { "Mouse related", "Left click + drag", "Orbit camera" }, { "", "Right click + drag", "Pan camera" }, { "", "Scroll", "Zoom camera" }, { "", "Ctrl+Right click", "Set center of rotation (ground plane)" }, - { "", "Middle click", "Set center of rotation (ground plane)" }, + { "", "Ctrl+Middle click", "Set center of rotation (nearest trajectory point)" }, { "", "Shift+R", "Open 'Center of rotation' dialog" }, }; @@ -168,7 +178,16 @@ struct AppState bool shaderOk = false; int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; + // Driving orbit's Euler mode (rotateX/rotateY/translate/rotationCenter/ + // isOrtho), not its azimuth/elevation/distance/target mode -- the same + // camera engine multi_view_tls_registration_step_2 uses, manually + // driven through rlgl (see display()'s camera setup) instead of + // raylib's Camera3D/BeginMode3D. raylib_widgets::OrbitCamera orbit; + // Rebuilt from orbit.euler every frame in display() -- used only for + // drawCompassRuler()'s right/up vectors, same reasoning as step2's own + // app_state.viewLocal (OrbitCamera itself stays Eigen-free). + Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); bool showCenterOfRotationWindow = false; // controls @@ -230,13 +249,55 @@ struct AppState }; // ── helpers ─────────────────────────────────────────────────────────────────── -static Vector3 toRL(float x, float y, float z) +// Plain Eigen::Vector3f -> raylib Vector3 conversion. Used to be an axis +// remap (x, z, -y) that made this app's native Z-up LiDAR data render +// correctly under raylib's Y-up Camera3D/BeginMode3D convention; now that +// the camera is multi_view_tls_registration_step_2's own Z-up rlgl-driven +// one, geometry renders in its native coordinates and this is a no-op +// component copy. +static Vector3 toVec3(const Eigen::Vector3f& v) { - return { x, z, -y }; + return { v.x(), v.y(), v.z() }; } -static Vector3 toRL(const Eigen::Vector3f& v) + +// Finds the trajectory pose closest to `ray` (unconditional nearest, no +// distance cutoff) and returns its world-space position -- mirrors +// multi_view_tls_registration_step_2's getClosestTrajectoryPoint(), backed +// by the same shared raylib_widgets::pickNearestPointOnLine() picker. +// Returns false (outPoint untouched) when the trajectory is empty. +static bool nearestTrajectoryPoint(const Trajectory& traj, const Ray& ray, Vector3& outPoint) +{ + if (traj.poses.empty()) + return false; + + std::vector pts; + pts.reserve(traj.poses.size()); + for (const auto& pose : traj.poses) + pts.push_back(toVec3(pose.T.translation())); + + size_t idx; + if (!raylib_widgets::pickNearestPointOnLine(pts.data(), pts.size(), ray, idx)) + return false; + + outPoint = pts[idx]; + return true; +} + +// Intersects `ray` with the Z=0 ground plane -- same plane +// multi_view_tls_registration_step_2's setNewRotationCenter() intersects +// (via RegistrationPlaneFeature::Plane{0,0,1,0} + rayIntersection()), +// reimplemented directly in raylib/raymath terms since those two types live +// in `core`, which this app deliberately doesn't link. Returns false +// (outPoint untouched) when the ray is ~parallel to the plane. +static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) { - return { v.x(), v.z(), -v.y() }; + const float kTolerance = 0.0001f; + if (ray.direction.z > -kTolerance && ray.direction.z < kTolerance) + return false; + + float t = -ray.position.z / ray.direction.z; + outPoint = Vector3Add(ray.position, Vector3Scale(ray.direction, t)); + return true; } // Load all cam0_*.jpg from CAMERA_0 (sibling of session dir) into s.images, resized by s.imgScale. @@ -546,8 +607,8 @@ static void loadCloud(AppState& s) pw = *M * pw; gpuData.push_back(pw.x()); + gpuData.push_back(pw.y()); gpuData.push_back(pw.z()); - gpuData.push_back(-pw.y()); const float rawIntensity = pt.intensity; float colorF = packGray(rawIntensity); @@ -706,8 +767,8 @@ static void loadCloud(AppState& s) if (d2 > mx * mx) mx = std::sqrt(d2); sumX += pw.x(); - sumY += pw.z(); - sumZ += -pw.y(); + sumY += pw.y(); + sumZ += pw.z(); cnt++; } // chunkImgs and their cv::Mat memory are released here @@ -721,8 +782,20 @@ static void loadCloud(AppState& s) if (cnt > 0) { s.cloud.upload(gpuData, mx); - s.orbit.target = { sumX / cnt, sumY / cnt, sumZ / cnt }; - s.orbit.distance = std::max(5.f, mx * 0.3f); + + // Frame the loaded cloud -- instant, not eased (this runs once on + // load, before there's anything to transition from). Same "recenter + // and look at" formula as OrbitCamera::moveEulerRotationCenterTo() + // (translate.xy = -center.xy keeps the point centered on screen + // regardless of the current rotate angles), applied directly to + // both euler and eulerGoal so there's no stale transition target + // left over from a previous session. + Vector3 center = { sumX / cnt, sumY / cnt, sumZ / cnt }; + float dist = std::max(5.f, mx * 0.3f); + s.orbit.euler.rotationCenter = center; + s.orbit.euler.translate = { -center.x, -center.y, -dist }; + s.orbit.eulerGoal = s.orbit.euler; + s.orbit.eulerTransitionActive = false; } s.status = "Pts: " + std::to_string(s.cloud.count) + " Poses: " + std::to_string(s.traj.poses.size()) + @@ -1128,7 +1201,7 @@ static void drawScene(AppState& s) { auto& a = s.traj.poses[i - 1]; auto& b = s.traj.poses[i]; - DrawLine3D(toRL(a.T.translation()), toRL(b.T.translation()), Color{ 100, 200, 255, 220 }); + DrawLine3D(toVec3(a.T.translation()), toVec3(b.T.translation()), Color{ 100, 200, 255, 220 }); } } @@ -1154,13 +1227,13 @@ static void drawScene(AppState& s) if (!pose) continue; - Vector3 origin = toRL(pose->T * C); + Vector3 origin = toVec3(pose->T * C); Vector3 w[4]; for (int k = 0; k < 4; k++) { Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * fs, ncy[k] * fs, fs) + C; - w[k] = toRL(pose->T * pl); + w[k] = toVec3(pose->T * pl); } bool hl = (ts == hlTs); @@ -1174,7 +1247,7 @@ static void drawScene(AppState& s) for (int k = 0; k < 4; k++) { Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * sc, ncy[k] * sc, sc) + C; - w2[k] = toRL(pose->T * pl); + w2[k] = toVec3(pose->T * pl); } DrawTriangle3D(w2[0], w2[1], w2[2], Color{ 255, 255, 50, 40 }); DrawTriangle3D(w2[2], w2[3], w2[0], Color{ 255, 255, 50, 40 }); @@ -1257,6 +1330,9 @@ int main(int argc, char* argv[]) SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); InitWindow(1400, 900, ("Trajectory Viewer " HDMAPPING_VERSION_STRING)); + // raylib's default exit key (Esc) closes the window outright -- too easy + // to hit by accident. Disabled; there's no keyboard shortcut for quitting. + SetExitKey(KEY_NULL); raylib_widgets::fitWindowToScreen(); // panelW below is user-resizable but the 3D view still needs room. SetWindowMinSize(900, 500); @@ -1320,9 +1396,7 @@ int main(int argc, char* argv[]) while (!WindowShouldClose()) { bool imguiWants = ImGui::GetIO().WantCaptureMouse; - s.orbit.update(!imguiWants); - s.orbit.updateTransition(GetFrameTime()); - Camera3D cam = s.orbit.toRaylib(); + s.orbit.updateEulerTransition(GetFrameTime()); // pick up the ROS export result from the worker thread (if any) { @@ -1346,7 +1420,13 @@ int main(int argc, char* argv[]) // bare F there is the "camera Front" preset). Ctrl+O and bare // C/P are kept aligned with step2 (Ctrl+O = open/load session, // C = compass/ruler). - bool ctrlDown = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL); + // KEY_LEFT/RIGHT_SUPER too: on macOS Cmd (Super) is a distinct + // key from Ctrl, and users -- including whoever asked for this + // binding -- reach for Cmd as "the" modifier there. Treating + // either as ctrlDown matches that expectation instead of + // requiring the literal Ctrl key. + bool ctrlDown = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL) || IsKeyDown(KEY_LEFT_SUPER) || + IsKeyDown(KEY_RIGHT_SUPER); bool shiftDown = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); if (ctrlDown && shiftDown && IsKeyPressed(KEY_O)) actionSelectCamera0Dir(s); @@ -1364,12 +1444,85 @@ int main(int argc, char* argv[]) if (!ctrlDown && IsKeyPressed(KEY_C)) s.showCompassRuler = !s.showCompassRuler; + // Camera drag/zoom -- same raylib_widgets::OrbitCamera Euler + // methods multi_view_tls_registration_step_2's motion()/wheel() + // call, driven from continuous per-frame deltas the way + // OrbitCamera::update() (the other, azimuth/elevation half of + // this struct) already reads input, rather than resurrecting + // step2's GLUT-shaped mouse_old_x/y/mouse_buttons bookkeeping + // (nothing about sharing the camera *math* requires reproducing + // that plumbing too). Gated off while Ctrl/Shift is held -- + // both are reserved for the picking actions below, same + // reasoning as step2's own motion() guard (a trackpad's + // click jitter while a modifier is held must never get read as + // a drag, or it breaks any transition that same click started). + if (!imguiWants && !ctrlDown && !shiftDown) + { + Vector2 d = GetMouseDelta(); + if (IsMouseButtonDown(MOUSE_BUTTON_LEFT)) + s.orbit.dragOrbit(d.x, d.y); + if (IsMouseButtonDown(MOUSE_BUTTON_RIGHT)) + { + if (s.orbit.isOrtho) + s.orbit.dragPanOrtho(d.x, d.y, (float)GetScreenWidth(), (float)GetScreenHeight()); + else + s.orbit.dragPanPerspective(d.x, d.y); + } + } + if (!imguiWants) + { + float wheel = GetMouseWheelMove(); + if (wheel != 0.f) + s.orbit.zoom(wheel, shiftDown); + } + + // Camera presets + orthographic toggle -- same bindings as + // step2's camMenu()/view_kbd_shortcuts() (F/B/L/R/T/U/I/Z, O), + // gated the same way (bare key, no Ctrl/Shift). + if (!ctrlDown && !shiftDown) + { + if (IsKeyPressed(KEY_F)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Front); + if (IsKeyPressed(KEY_B)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Back); + if (IsKeyPressed(KEY_L)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Left); + if (IsKeyPressed(KEY_R)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Right); + if (IsKeyPressed(KEY_T)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Top); + if (IsKeyPressed(KEY_U)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Bottom); + if (IsKeyPressed(KEY_I)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Iso); + if (IsKeyPressed(KEY_Z)) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Reset); + if (IsKeyPressed(KEY_O)) + s.orbit.isOrtho = !s.orbit.isOrtho; + } + if (shiftDown && IsKeyPressed(KEY_R)) s.showCenterOfRotationWindow = true; + // Ctrl+Right-click: ground-plane (Z=0) pick. if (!imguiWants && ctrlDown && IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) - s.orbit.pickGroundPlaneTarget(GetMousePosition(), cam); - if (!imguiWants && IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) - s.orbit.pickGroundPlaneTarget(GetMousePosition(), cam); + { + Vector2 mp = GetMousePosition(); + Ray ray = s.orbit.eulerScreenRay((int)mp.x, (int)mp.y, GetScreenWidth(), GetScreenHeight()); + Vector3 hit; + if (intersectGroundPlaneZ0(ray, hit)) + s.orbit.moveEulerRotationCenterTo(hit); + } + // Ctrl+Middle-click: nearest-trajectory-point pick (like step2's + // Ctrl/Shift+Middle-click) -- falls back to the ground-plane + // pick when no trajectory is loaded yet. + if (!imguiWants && ctrlDown && IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) + { + Vector2 mp = GetMousePosition(); + Ray ray = s.orbit.eulerScreenRay((int)mp.x, (int)mp.y, GetScreenWidth(), GetScreenHeight()); + Vector3 hit; + if (nearestTrajectoryPoint(s.traj, ray, hit) || intersectGroundPlaneZ0(ray, hit)) + s.orbit.moveEulerRotationCenterTo(hit); + } if (IsKeyPressed(KEY_LEFT)) { @@ -1384,24 +1537,71 @@ int main(int argc, char* argv[]) } BeginDrawing(); + + // GetRenderWidth/Height(), not io.DisplaySize: the GL viewport must + // be sized in actual framebuffer pixels, which can differ under DPI + // scaling -- same reasoning as step2's display(). + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); ClearBackground(Color{ 25, 25, 25, 255 }); + rlEnableDepthTest(); + + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + float ratio = float(ImGui::GetIO().DisplaySize.x) / float(ImGui::GetIO().DisplaySize.y); + + // Camera setup -- ported 1:1 from multi_view_tls_registration_step_2's + // display(), driving orbit's Euler mode through rlgl's matrix stack + // directly instead of raylib's BeginMode3D/EndMode3D. + s.viewLocal = Eigen::Affine3f::Identity(); + + if (!s.orbit.isOrtho) + { + s.orbit.applyPerspectiveProjection((int)ImGui::GetIO().DisplaySize.x, (int)ImGui::GetIO().DisplaySize.y); + + Eigen::Vector3f rotationCenter( + s.orbit.euler.rotationCenter.x, s.orbit.euler.rotationCenter.y, s.orbit.euler.rotationCenter.z); + s.viewLocal.translate(rotationCenter); + s.viewLocal.translate( + Eigen::Vector3f(s.orbit.euler.translate.x, s.orbit.euler.translate.y, s.orbit.euler.translate.z)); + if (!s.orbit.lockZ) + s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateX * DEG2RAD, Eigen::Vector3f::UnitX())); + else + s.viewLocal.rotate(Eigen::AngleAxisf(-90.0f * DEG2RAD, Eigen::Vector3f::UnitX())); + s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateY * DEG2RAD, Eigen::Vector3f::UnitZ())); + s.viewLocal.translate(-rotationCenter); + + rlMultMatrixf(s.viewLocal.matrix().data()); + } + else + { + // Still updating viewLocal for the compass -- the rest of the + // ortho projection + gizmo-view lookAt lives in + // OrbitCamera::updateOrtho(). + s.viewLocal.rotate( + Eigen::AngleAxisf((s.orbit.euler.rotateX + s.orbit.euler.rotateY) * DEG2RAD, Eigen::Vector3f::UnitZ())); + s.orbit.updateOrtho(ratio); + } + + s.orbit.captureFrameMatrices(); + + // Origin axes + rotation-center cross -- was step2's showAxes(), + // unconditional here (this app has no show_axes toggle). + DrawLine3D({ 0, 0, 0 }, { 100, 0, 0 }, RED); + DrawLine3D({ 0, 0, 0 }, { 0, 100, 0 }, GREEN); + DrawLine3D({ 0, 0, 0 }, { 0, 0, 100 }, BLUE); + raylib_widgets::drawRotationCenterCross( + s.orbit.euler.rotationCenter, std::max(0.1f, fabsf(s.orbit.euler.translate.z) * 0.05f), WHITE); - BeginMode3D(cam); drawScene(s); - DrawGrid(20, 1.f); - // axes - DrawLine3D({ 0, 0, 0 }, { 2, 0, 0 }, RED); - DrawLine3D({ 0, 0, 0 }, { 0, 2, 0 }, GREEN); - DrawLine3D({ 0, 0, 0 }, { 0, 0, -2 }, BLUE); - raylib_widgets::drawRotationCenterCross(s.orbit.target, s.orbit.distance * 0.05f, WHITE); - EndMode3D(); + + raylib_widgets::end3DMatrixStack(ImGui::GetIO().DisplaySize.x, ImGui::GetIO().DisplaySize.y); if (s.showCompassRuler) { - Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); - Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); - Vector3 up = Vector3CrossProduct(right, fwd); - raylib_widgets::drawCompassRuler(right, up, s.orbit.distance, LIGHTGRAY); + const Eigen::Matrix3f& R = s.viewLocal.rotation(); + Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; + Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; + raylib_widgets::drawCompassRuler(right, up, s.orbit.euler.translate.z, LIGHTGRAY); } // ── upload image viewer texture if worker produced one ──────────────── @@ -1451,6 +1651,30 @@ int main(int argc, char* argv[]) ImGui::EndMenu(); } + if (ImGui::BeginMenu("Camera")) + { + if (ImGui::MenuItem("Front", "key F")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Front); + if (ImGui::MenuItem("Back", "key B")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Back); + if (ImGui::MenuItem("Left", "key L")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Left); + if (ImGui::MenuItem("Right", "key R")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Right); + if (ImGui::MenuItem("Top", "key T")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Top); + if (ImGui::MenuItem("Bottom", "key U")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Bottom); + if (ImGui::MenuItem("Isometric", "key I")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Iso); + ImGui::Separator(); + if (ImGui::MenuItem("Reset", "key Z")) + s.orbit.setEulerPreset(raylib_widgets::OrbitCamera::EulerPreset::Reset); + ImGui::Separator(); + ImGui::MenuItem("Orthographic", "key O", &s.orbit.isOrtho); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("View")) { ImGui::MenuItem("Show path", "P", &s.showPath); @@ -1765,7 +1989,7 @@ int main(int argc, char* argv[]) ImGui::End(); - raylib_widgets::showCenterOfRotationWindow(s.showCenterOfRotationWindow, s.orbit); + raylib_widgets::showEulerCenterOfRotationWindow(s.showCenterOfRotationWindow, s.orbit); // ── shortcuts help window ─────────────────────────────────────────────── if (s.showHelp) diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index ff460d6b..ca83491b 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -7,13 +7,14 @@ project(multi_view_tls_registration_step_2) # multi_view_tls_registration_gui.cpp is raylib-based: the GLUT-window-and- # input-loop and point-cloud/loop-closure rendering code that used to go # through core's own legacy-GL PointCloud::render()/PointClouds::render() -# now uses Core/raylib_render.hpp's ScanRenderer instead. -# -# rl_utils.cpp/rl_utils.h are this app's own raylib-based replacement for -# the camera/picking/mini-compass/misc-ImGui-widget API it used to get from -# core/src/utils.cpp (shared with the remaining GLUT apps, so it can't be -# changed), using rlgl's rl*() legacy-GL-emulation API instead of real -# gl*()/glu*()/glut*() calls -- see rl_utils.h's top comment. +# now uses Core/raylib_render.hpp's ScanRenderer instead. Its +# camera/picking/mini-compass/misc-ImGui-widget API -- once its own local +# rl_utils.cpp/rl_utils.h, before that -- is folded directly +# into multi_view_tls_registration_gui.cpp now (everything in rl_utils.h/.cpp +# turned out to only ever be used there) plus raylib_widgets, shared with the +# camera_lidar_* apps (OrbitCamera's Euler mode, the Euler center-of-rotation +# dialog, app-shell scaffolding, double-precision ray/plane math) -- see that +# file's own top-of-section comment. # # perform_experiment.cpp still #includes GLUT/glew/imgui-GLUT-backend headers, # but never calls anything from them (verified: no glut*/GL_*/ImGui_Impl* @@ -22,7 +23,6 @@ project(multi_view_tls_registration_step_2) set(SOURCES multi_view_tls_registration.cpp perform_experiment.cpp multi_view_tls_registration_gui.cpp multi_view_tls_registration.h - rl_utils.cpp rl_utils.h ../lidar_odometry_step_1/lidar_odometry_utils.cpp ) 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 911b9bce..1ab301fc 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 @@ -103,9 +103,19 @@ #endif // Camera/picking/mini-compass/misc-ImGui-widget API this app used to get -// from (see rl_utils.h's top comment for why it's now a -// local header instead). -#include "rl_utils.h" +// from , then from its own local rl_utils.h/.cpp (see this +// section's comments for what replaced what) -- folded directly into this +// file since everything in rl_utils.h/.cpp turned out to only ever be used +// here, plus raylib_widgets (shared with the camera_lidar_* apps: OrbitCamera's +// Euler mode, the Euler center-of-rotation dialog, app-shell scaffolding, +// double-precision ray/plane math). +#include +#include +#include +#include +#include +#include +#include #ifdef _WIN32 // windows.h (pulled in transitively above, via portable-file-dialogs.h) @@ -119,6 +129,105 @@ /////////////////////////////////////////////////////////////////////////////////// +// Now shared with the camera_lidar_* apps -- see +// raylib_widgets/include/RaylibWidgets/ShortcutsTable.h/AppShell.h. +using raylib_widgets::ImGuiHyperlink; +using raylib_widgets::ShortcutEntry; +using raylib_widgets::ShowMainDockSpace; + +const float DEG_TO_RAD = M_PI / 180.0f; +const float RAD_TO_DEG = 180.0f / M_PI; + +const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); + +const std::string out_fn = "Output file name"; + +constexpr float ImGuiNumberWidth = 120.0f; +constexpr const char* omText = "Roll (left/right)"; +constexpr const char* fiText = "Pitch (up/down)"; +constexpr const char* kaText = "Yaw (turning left/right)"; +constexpr const char* xText = "Longitudinal (forward/backward)"; +constexpr const char* yText = "Lateral (left/right)"; +constexpr const char* zText = "Vertical (up/down)"; + +const uint32_t window_width = 1600; +const uint32_t window_height = 900; + +enum ColorScheme +{ + CS_SOLID, // fixed color + CS_RANDOM, // random + CS_GRAD_INTENS, // gradient based on intensity + CS_GRAD_ELEV, // gradient based on elevation + CS_GRAD_DIST, // gradient based on distance from rotation center + CS_FOLLOW // valid for trajectory +}; + +struct AppStateBase +{ + int viewer_decimate_point_cloud = 2; + + int mouse_old_x = 0, mouse_old_y = 0; + int mouse_buttons = 0; + bool show_axes = true; + ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); + int point_size = 1; + + bool info_gui = false; + bool compass_ruler = true; + + // Still Eigen/rlgl-driven directly (not folded into + // raylib_widgets::OrbitCamera, which deliberately stays Eigen-free for + // its azimuth/elevation half -- only OrbitCamera's own RayPlaneD-using + // pieces gained an Eigen dependency) -- used only by + // drawMiniCompassWithRuler() below and display()'s own rlMultMatrixf + // call. Rebuilt from `camera` every frame. + Eigen::Affine3f viewLocal; + + // Camera state (rotate/translate/rotation-center/ortho/presets/ + // transitions/frame matrices), shared with the camera_lidar_* apps via + // raylib_widgets::OrbitCamera's Euler/ortho mode -- see its header. + raylib_widgets::OrbitCamera camera; + + // Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width + // support is uniform enough here not to need a runtime check -- always true. + bool glLineWidthSupport = true; +}; + +inline AppStateBase app_state; + +// Edge-triggered "please open the Center of rotation dialog" request -- +// set by view_kbd_shortcuts()'s Shift+R, consumed once by +// showEulerCenterOfRotationWindow() in display() below. +bool cor_gui = false; + +bool scroll_hint_enabled = true; +bool scroll_hint_active = false; +int scroll_hint_count = 0; +float scroll_hint_accu = 0.0f; +double scroll_hint_lastT = 0.0; + +std::string truncPath(const std::string& fullPath); + +void wheel(int button, int dir, int x, int y); +void motion(int x, int y); + +void showAxes(); +void drawIntersectionGrids(const PointClouds& point_clouds_container, const PointClouds::PointCloudDimensions& dims); +void camMenu(); +void view_kbd_shortcuts(); + +void drawMiniCompassWithRuler(); + +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); +LaserBeam GetLaserBeam(int x, int y); +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index); + +void setNewRotationCenter(int x, int y); + +bool checkClHelp(int argc, char** argv); + // GPU (rlgl-based) point cloud renderer -- replaces core's legacy-GL // PointCloud::render()/PointClouds::render() (see Core/raylib_render.hpp). // Rebuilt on session load and whenever a scan's pose changes; syncPoses() @@ -129,16 +238,14 @@ ScanRenderer scan_renderer; // This frame's 3D model-view-projection matrix, captured right after the // camera transform is set up in display() (before the projection/modelview // stack gets reset to the 2D screen ortho for ImGui -- see -// end3DMatrixStack()). renderLoopClosureLabels() uses it to project pose -// world positions to screen space for DrawText, since it runs after that -// reset (2D text needs the 2D ortho active, but still needs to know where -// each 3D point landed on screen). +// raylib_widgets::end3DMatrixStack()). renderLoopClosureLabels() uses it to +// project pose world positions to screen space for DrawText, since it runs +// after that reset (2D text needs the 2D ortho active, but still needs to +// know where each 3D point landed on screen). Matrix frame_mvp_3d{}; // Forward declarations for this file's own functions defined near -// display() below (everything else that used to be here is now declared -// by rl_utils.h, included above) -- panel functions earlier in this file -// call some of these. +// display() below -- panel functions earlier in this file call some of these. void observationPickingRender(const ObservationPicking& observation_picking); void renderLoopClosure( PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); @@ -154,6 +261,596 @@ void mouse(int glut_button, int state, int x, int y); /////////////////////////////////////////////////////////////////////////////////// +std::string truncPath(const std::string& fullPath) +{ + namespace fspath = std::filesystem; + fspath::path path(fullPath); + + auto parent1 = path.parent_path().filename().string(); + auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder + auto filename = path.filename().string(); + + return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; +} + +void wheel(int button, int dir, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MouseWheel += dir; // or direction * 1.0f depending on your setup + + if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) + { + // GetMouseWheelMove(), not `dir`: dir is already quantized to +-1 by + // main()'s caller (see its comment), which discards a trackpad's + // fractional per-frame scroll magnitude -- reading it again here + // (stable within the same frame, since raylib only updates it once + // per PollInputEvents()) lets zoom() scale the step by how much was + // actually scrolled instead of always taking a full step. + app_state.camera.zoom(GetMouseWheelMove(), io.KeyShift); + + if (scroll_hint_enabled) + { + if (!scroll_hint_active) + { + scroll_hint_accu += fabs(dir); + + if (scroll_hint_accu > 30.0f) // tweak threshold + { + scroll_hint_accu = 0.0f; + scroll_hint_active = true; + scroll_hint_count++; + } + } + + if (scroll_hint_active) + scroll_hint_lastT = ImGui::GetTime(); + + // Reset and disable hint if Shift is pressed while scrolling + if (io.KeyShift || scroll_hint_count > 3) + { + scroll_hint_active = false; + scroll_hint_enabled = false; + } + } + } +} + +void motion(int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + if (!io.WantCaptureMouse) + { + float dx, dy; + dx = (float)(x - app_state.mouse_old_x); + dy = (float)(y - app_state.mouse_old_y); + + // Ctrl/Shift held: reserved for the discrete click actions and the + // keyboard shortcuts in view_kbd_shortcuts() -- mouse() sets + // mouse_buttons for *every* button-down, including a Ctrl/Shift+ + // click used to pick a new rotation center (which starts a camera + // transition -- see getClosestTrajectoryPoint()/ + // setNewRotationCenter()/the Center of rotation dialog). Without + // this guard, any stray sub-pixel movement on the same click + // (trackpads are far more prone to this than a physical mouse + // button) got read as an ordinary orbit/pan drag and immediately + // broke that transition via dragOrbit()/dragPanPerspective()'s + // breakEulerTransition() call. + if (!io.KeyCtrl && !io.KeyShift) + { + if (app_state.mouse_buttons & 1) // left button + { + app_state.camera.dragOrbit(dx, dy); + } + + if (app_state.mouse_buttons & 4) // right button + { + if (app_state.camera.isOrtho) + app_state.camera.dragPanOrtho(dx, dy, io.DisplaySize.x, io.DisplaySize.y); + else + app_state.camera.dragPanPerspective(dx, dy); + } + } + + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; + } +} + +void showAxes() +{ + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes + { + const auto& rc = app_state.camera.euler.rotationCenter; + rlBegin(RL_LINES); + rlColor3f(1.f, 1.f, 1.f); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x + 1.f, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x - 1.f, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y - 1.f, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y + 1.f, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z - 1.f); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z + 1.f); + rlEnd(); + } + + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // origin axes + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(100, 0.0f, 0.0f); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 100, 0.0f); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 100); + rlEnd(); + } +} + +// Ported from PointClouds::draw_grids() (core/src/point_clouds.cpp, legacy +// immediate-mode GL, shared with the GLUT apps so it can't be changed) -- +// rl*() rename, one helper per cutting plane instead of one copy-pasted +// block per grid density. Spans the session's bounding box (dims), snapped +// outward to whole grid steps, same as the original. +void drawGridXZ(float step, Color color, const PointClouds::PointCloudDimensions& dims) +{ + float x_min = std::floor(dims.x_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float x = x_min; x <= x_max; x += step) + { + rlVertex3f(x, 0.0f, z_min); + rlVertex3f(x, 0.0f, z_max); + } + for (float z = z_min; z <= z_max; z += step) + { + rlVertex3f(x_min, 0.0f, z); + rlVertex3f(x_max, 0.0f, z); + } + rlEnd(); +} + +void drawGridYZ(float step, Color color, const PointClouds::PointCloudDimensions& dims) +{ + float y_min = std::floor(dims.y_min / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float y = y_min; y <= y_max; y += step) + { + rlVertex3f(0.0f, y, z_min); + rlVertex3f(0.0f, y, z_max); + } + for (float z = z_min; z <= z_max; z += step) + { + rlVertex3f(0.0f, y_min, z); + rlVertex3f(0.0f, y_max, z); + } + rlEnd(); +} + +void drawGridXY(float step, Color color, const PointClouds::PointCloudDimensions& dims) +{ + float x_min = std::floor(dims.x_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float x = x_min; x <= x_max; x += step) + { + rlVertex3f(x, y_min, 0.0f); + rlVertex3f(x, y_max, 0.0f); + } + for (float y = y_min; y <= y_max; y += step) + { + rlVertex3f(x_min, y, 0.0f); + rlVertex3f(x_max, y, 0.0f); + } + rlEnd(); +} + +// Draws whichever of the 9 grid-density/plane checkboxes (View menu, next to +// the xz/yz/xy_intersection toggles) are on -- was the unconditional +// draw_grids() call at the top of the legacy PointClouds::render(). Not +// gated on xz/yz/xy_intersection itself (matching the original): a grid can +// be shown independent of whether its plane's intersection slab is active. +void drawIntersectionGrids(const PointClouds& point_clouds_container, const PointClouds::PointCloudDimensions& dims) +{ + const Color light = ColorFromNormalized(Vector4{ 0.7f, 0.7f, 0.7f, 1.0f }); + const Color dark = ColorFromNormalized(Vector4{ 0.3f, 0.3f, 0.3f, 1.0f }); + + if (point_clouds_container.xz_grid_10x10) + drawGridXZ(10.0f, light, dims); + if (point_clouds_container.xz_grid_1x1) + drawGridXZ(1.0f, dark, dims); + if (point_clouds_container.xz_grid_01x01) + drawGridXZ(0.1f, dark, dims); + + if (point_clouds_container.yz_grid_10x10) + drawGridYZ(10.0f, light, dims); + if (point_clouds_container.yz_grid_1x1) + drawGridYZ(1.0f, dark, dims); + if (point_clouds_container.yz_grid_01x01) + drawGridYZ(0.1f, dark, dims); + + if (point_clouds_container.xy_grid_10x10) + drawGridXY(10.0f, light, dims); + if (point_clouds_container.xy_grid_1x1) + drawGridXY(1.0f, dark, dims); + if (point_clouds_container.xy_grid_01x01) + drawGridXY(0.1f, dark, dims); +} + +void camMenu() +{ + using raylib_widgets::OrbitCamera; + + if (ImGui::BeginMenu("Camera")) + { + if (ImGui::MenuItem("Front (yz view)", "key F")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); + if (ImGui::MenuItem("Back", "key B")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); + if (ImGui::MenuItem("Left (xz view)", "key L")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); + if (ImGui::MenuItem("Right", "key R")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); + if (ImGui::MenuItem("Top (xy view)", "key T")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); + if (ImGui::MenuItem("Bottom", "key U")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); + if (ImGui::MenuItem("Isometric", "key I")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); + ImGui::Separator(); + if (ImGui::MenuItem("Reset", "key Z")) + { + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); + app_state.viewer_decimate_point_cloud = 2; + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Change camera view to fixed positions"); + ImGui::Separator(); + ImGui::Text("Metrics:"); + if (ImGui::BeginTable("Metrics", 4)) + { + ImGui::TableSetupColumn("Coord"); + ImGui::TableSetupColumn("rotate"); + ImGui::TableSetupColumn("translate"); + ImGui::TableSetupColumn("rot center"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + + std::string text = "X"; + float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", app_state.camera.euler.rotateX); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.x); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.x); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", app_state.camera.euler.rotateY); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.y); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.z); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); + + ImGui::EndTable(); + } + ImGui::Text("Mouse sensitivity: %.4f", app_state.camera.eulerMouseSensitivity); + + ImGui::EndTooltip(); + } + + if (scroll_hint_active) + { + ImVec2 mousePos = ImGui::GetMousePos(); + ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); + ImGui::SetNextWindowBgAlpha(0.7f); + ImGui::BeginTooltip(); + ImGui::Text("Tip: To accelerate hold Shift + scroll"); + ImGui::EndTooltip(); + + if (ImGui::GetTime() - scroll_hint_lastT > 1) + scroll_hint_active = false; + } +} + +void view_kbd_shortcuts() +{ + using raylib_widgets::OrbitCamera; + + ImGuiIO& io = ImGui::GetIO(); + + if (io.WantCaptureKeyboard) + return; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + app_state.camera.euler.translate.x += 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + app_state.camera.euler.translate.x -= 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + app_state.camera.euler.translate.y += 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + app_state.camera.euler.translate.y -= 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + app_state.camera.euler.rotateY -= 0.6f; + app_state.camera.breakEulerTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + app_state.camera.euler.rotateY += 0.6f; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + app_state.camera.euler.rotateX -= 0.6f; + app_state.camera.breakEulerTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + app_state.camera.euler.rotateX += 0.6f; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) + cor_gui = true; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !app_state.camera.isOrtho) + app_state.camera.lockZ = !app_state.camera.lockZ; + + if (io.KeyCtrl || io.KeyAlt || io.KeyShift) + return; + + if (ImGui::IsKeyPressed(ImGuiKey_B)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); + if (ImGui::IsKeyPressed(ImGuiKey_F)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); + if (ImGui::IsKeyPressed(ImGuiKey_I)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); + if (ImGui::IsKeyPressed(ImGuiKey_L)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); + if (ImGui::IsKeyPressed(ImGuiKey_R)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); + if (ImGui::IsKeyPressed(ImGuiKey_T)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); + if (ImGui::IsKeyPressed(ImGuiKey_U)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); + if (ImGui::IsKeyPressed(ImGuiKey_Z)) + { + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); + app_state.viewer_decimate_point_cloud = 2; + } + + if (ImGui::IsKeyPressed(ImGuiKey_C, false)) + app_state.compass_ruler = !app_state.compass_ruler; + if (ImGui::IsKeyPressed(ImGuiKey_O, false)) + app_state.camera.isOrtho = !app_state.camera.isOrtho; + if (ImGui::IsKeyPressed(ImGuiKey_X, false)) + app_state.show_axes = !app_state.show_axes; + + if (ImGui::IsKeyPressed(ImGuiKey_1)) + app_state.point_size = 1; + if (ImGui::IsKeyPressed(ImGuiKey_2)) + app_state.point_size = 2; + if (ImGui::IsKeyPressed(ImGuiKey_3)) + app_state.point_size = 3; + if (ImGui::IsKeyPressed(ImGuiKey_4)) + app_state.point_size = 4; + if (ImGui::IsKeyPressed(ImGuiKey_5)) + app_state.point_size = 5; + if (ImGui::IsKeyPressed(ImGuiKey_6)) + app_state.point_size = 6; + if (ImGui::IsKeyPressed(ImGuiKey_7)) + app_state.point_size = 7; + if (ImGui::IsKeyPressed(ImGuiKey_8)) + app_state.point_size = 8; + if (ImGui::IsKeyPressed(ImGuiKey_9)) + app_state.point_size = 9; +} + +// Drawing itself lives in raylib_widgets::drawCompassRuler (shared with the +// camera_lidar_* apps' identical overlay) -- this just adapts this app's own +// camera/background state (app_state.viewLocal's rotation matrix, +// app_state.camera.euler.translate.z zoom, app_state.bg_color) into that +// function's right/up/zoomDistance/rulerColor parameters. Row 0/1 of a +// world-to-eye rotation matrix R are exactly the world-space directions that +// map to eye-space +X/+Y (screen right/up): (R * dir).x() == dot(R.row(0), dir). +void drawMiniCompassWithRuler() +{ + const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); + Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; + Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; + Color rulerColor = + ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); + raylib_widgets::drawCompassRuler( + right, up, app_state.camera.euler.translate.z, rulerColor, + raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); +} + +// Was distanceToPlane()+its own loop -- both now delegate to the shared +// raylib_widgets::intersectPlane() (Eigen double precision, matching this +// app's world coordinates). Falls back to laser_beam.position itself (like +// the original) when the ray is ~parallel to the plane. +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) +{ + Eigen::Vector3d hit = laser_beam.position; + raylib_widgets::intersectPlane(laser_beam.position, laser_beam.direction, plane.a, plane.b, plane.c, plane.d, hit); + return hit; +} + +// Delegates the actual unprojection to the shared +// raylib_widgets::OrbitCamera::eulerScreenRay() (also used by +// camera_lidar_trajectory_viewer) and adapts its raylib Ray into this app's +// own Eigen-based LaserBeam type, which rayIntersection()/ +// distance_point_to_line()/callers throughout this file still expect. +LaserBeam GetLaserBeam(int x, int y) +{ + Ray ray = app_state.camera.eulerScreenRay(x, y, GetScreenWidth(), GetScreenHeight()); + + LaserBeam laser_beam; + laser_beam.position = Eigen::Vector3d(ray.position.x, ray.position.y, ray.position.z); + laser_beam.direction = Eigen::Vector3d(ray.direction.x, ray.direction.y, ray.direction.z); + + return laser_beam; +} + +// Delegates to the shared raylib_widgets::distancePointToLine(). +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) +{ + return raylib_widgets::distancePointToLine(point, line.position, line.direction); +} + +// Shared with camera_lidar_trajectory_viewer's equivalent +// nearestTrajectoryPoint() -- both now delegate the actual nearest-point- +// to-ray search to raylib_widgets::pickNearestPointOnLine() instead of +// each keeping its own copy of this loop. +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index) +{ + picked_index = -1; + + const auto laser_beam = GetLaserBeam(x, y); + Ray ray; + ray.position = Vector3{ + static_cast(laser_beam.position.x()), static_cast(laser_beam.position.y()), + static_cast(laser_beam.position.z()) }; + ray.direction = Vector3{ + static_cast(laser_beam.direction.x()), static_cast(laser_beam.direction.y()), + static_cast(laser_beam.direction.z()) }; + + std::vector pts; + std::vector> ptOwners; // (point cloud index, local_trajectory index), parallel to pts + for (int i = 0; i < session_.point_clouds_container.point_clouds.size(); i++) + { + for (int j = 0; j < session_.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + { + const auto& p = session_.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + Eigen::Vector3d vp = session_.point_clouds_container.point_clouds[i].m_pose * p; + pts.push_back(Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }); + ptOwners.push_back({ i, j }); + } + } + + // Defaults to the still-pending transition target (mirrors the + // original, which read/wrote its own persistent new_rotation_center + // field here rather than a fresh local -- so a call that finds no + // point still re-triggers a transition toward whatever that field last + // held). + Vector3 center = app_state.camera.eulerGoal.rotationCenter; + + size_t bestIdx; + if (raylib_widgets::pickNearestPointOnLine(pts.data(), pts.size(), ray, bestIdx)) + { + center = pts[bestIdx]; + const auto [index_i, index_j] = ptOwners[bestIdx]; + + if (gcpPicking) + { + session_.ground_control_points.picking_mode_index_to_node_inner = index_i; + session_.ground_control_points.picking_mode_index_to_node_outer = index_j; + } + + picked_index = index_i; + } + + app_state.camera.moveEulerRotationCenterTo(center); +} + +void setNewRotationCenter(int x, int y) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = 0; + Eigen::Vector3f center_eigen = rayIntersection(laser_beam, pl).cast(); + + spdlog::info("Setting new rotation center to: {}, {}, {}", center_eigen.x(), center_eigen.y(), center_eigen.z()); + + app_state.camera.moveEulerRotationCenterTo(Vector3{ center_eigen.x(), center_eigen.y(), center_eigen.z() }); +} + +bool checkClHelp(int argc, char** argv) +{ + for (int i = 1; i < argc; ++i) + { + std::string arg(argv[i]); + + if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") + { + return true; + } + } + return false; +} + +/////////////////////////////////////////////////////////////////////////////////// + #ifdef _WIN32 bool consWin = true; #endif @@ -1032,13 +1729,8 @@ void observation_picking_gui() if (ImGui::Button("Reset view")) { - app_state.new_rotation_center = app_state.rotation_center; - app_state.new_rotate_x = 0.0; - app_state.new_rotate_y = 0.0; - app_state.new_translate_x = app_state.translate_x; - app_state.new_translate_y = app_state.translate_y; - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + app_state.camera.startEulerTransition( + 0.0f, 0.0f, app_state.camera.euler.translate, app_state.camera.euler.rotationCenter); } } ImGui::EndDisabled(); @@ -1265,9 +1957,9 @@ void lio_segments_gui() if (index_end < 0) index_end = 0; - app_state.rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); - app_state.rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); - app_state.rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + app_state.camera.euler.rotationCenter.x = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + app_state.camera.euler.rotationCenter.y = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + app_state.camera.euler.rotationCenter.z = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); session.point_clouds_container.show_all_from_range(index_begin, index_end); } ImGui::SameLine(); @@ -1282,9 +1974,9 @@ void lio_segments_gui() if (index_end > session.point_clouds_container.point_clouds.size() - 1) index_end = session.point_clouds_container.point_clouds.size() - 1; - app_state.rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); - app_state.rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); - app_state.rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + app_state.camera.euler.rotationCenter.x = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + app_state.camera.euler.rotationCenter.y = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + app_state.camera.euler.rotationCenter.z = (float)session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); session.point_clouds_container.show_all_from_range(index_begin, index_end); } ImGui::SameLine(); @@ -2004,18 +2696,18 @@ void settings_gui() ImGui::NewLine(); - ImGui::InputFloat("camera_x", &app_state.new_rotation_center.x()); - ImGui::InputFloat("camera_y", &app_state.new_rotation_center.y()); - ImGui::InputFloat("camera_z", &app_state.new_rotation_center.z()); + ImGui::InputFloat("camera_x", &app_state.camera.eulerGoal.rotationCenter.x); + ImGui::InputFloat("camera_y", &app_state.camera.eulerGoal.rotationCenter.y); + ImGui::InputFloat("camera_z", &app_state.camera.eulerGoal.rotationCenter.z); if (ImGui::Button("set camera")) { - // app_state.new_rotate_x = app_state.rotate_x; - // app_state.new_rotate_y = app_state.rotate_y; - // app_state.new_translate_x = -app_state.new_rotation_center.x(); - // app_state.new_translate_y = -app_state.new_rotation_center.y(); - // app_state.new_translate_z = -app_state.new_rotation_center.z(); - app_state.camera_transition_active = true; + // app_state.camera.eulerGoal.rotateX = app_state.camera.euler.rotateX; + // app_state.camera.eulerGoal.rotateY = app_state.camera.euler.rotateY; + // app_state.camera.eulerGoal.translate.x = -app_state.camera.eulerGoal.rotationCenter.x; + // app_state.camera.eulerGoal.translate.y = -app_state.camera.eulerGoal.rotationCenter.y; + // app_state.camera.eulerGoal.translate.z = -app_state.camera.eulerGoal.rotationCenter.z; + app_state.camera.eulerTransitionActive = true; } if (ImGui::Button("Set initial pose to Identity and update other poses")) @@ -2514,14 +3206,18 @@ void renderLoopClosure( scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), + Eigen::Vector3d(app_state.camera.euler.rotationCenter.x, app_state.camera.euler.rotationCenter.y, app_state.camera.euler.rotationCenter.z), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), - 1); + 1, + point_clouds_container.xz_intersection, + point_clouds_container.yz_intersection, + point_clouds_container.xy_intersection, + static_cast(point_clouds_container.intersection_width)); // Pose-sequence trail across the whole session, as a chain of thick // green cylinders (sphere at each joint), sized relative to the current - // zoom (app_state.translate_z) so it stays visible next to the point cloud. - const float tubeRadius = std::max(0.005f, fabsf(app_state.translate_z) * 0.001f); + // zoom (app_state.camera.euler.translate.z) so it stays visible next to the point cloud. + const float tubeRadius = std::max(0.005f, fabsf(app_state.camera.euler.translate.z) * 0.001f); bool first = true; Vector3 prev{}; for (const auto& pc : pointClouds) @@ -2867,9 +3563,13 @@ void renderControlPoints(const ControlPoints& control_points, PointClouds& point ScanColorMode::Intensity, static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), + Eigen::Vector3d(app_state.camera.euler.rotationCenter.x, app_state.camera.euler.rotationCenter.y, app_state.camera.euler.rotationCenter.z), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), - 1); + 1, + point_clouds_container.xz_intersection, + point_clouds_container.yz_intersection, + point_clouds_container.xy_intersection, + static_cast(point_clouds_container.intersection_width)); for (size_t i = 0; i < pointClouds.size(); ++i) { @@ -3149,93 +3849,81 @@ void display() rlLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - updateCameraTransition(); + app_state.camera.updateEulerTransition(io.DeltaTime); app_state.viewLocal = Eigen::Affine3f::Identity(); - if (!app_state.is_ortho) + if (!app_state.camera.isOrtho) { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + app_state.camera.applyPerspectiveProjection((int)io.DisplaySize.x, (int)io.DisplaySize.y); // janusz if (is_loop_closure_gui) { if (new_loop_closure_index) { - // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) - //{ - // app_state.new_rotation_center.x() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); - // app_state.new_rotation_center.y() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); - // app_state.new_rotation_center.z() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); - // - // app_state.new_translate_x = -app_state.new_rotation_center.x(); - // app_state.new_translate_y = -app_state.new_rotation_center.y(); - // app_state.camera_transition_active = true; - //} + if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) + { + const auto& t = session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation(); + app_state.camera.moveEulerRotationCenterTo( + Vector3{ static_cast(t.x()), static_cast(t.y()), static_cast(t.z()) }); + } if (session.pose_graph_loop_closure.manipulate_active_edge) { + Vector3 center = app_state.camera.eulerGoal.rotationCenter; + if (session.pose_graph_loop_closure.edges.size() > 0) { if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) { - app_state.new_rotation_center.x() = - session.point_clouds_container - .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] - .index_from] - .m_pose.translation() - .x(); - app_state.new_rotation_center.y() = + const auto& t = session.point_clouds_container .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] .index_from] - .m_pose.translation() - .y(); - app_state.new_rotation_center.z() = - session.point_clouds_container - .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] - .index_from] - .m_pose.translation() - .z(); + .m_pose.translation(); + center = Vector3{ static_cast(t.x()), static_cast(t.y()), static_cast(t.z()) }; } } - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + app_state.camera.moveEulerRotationCenterTo(center); } new_loop_closure_index = false; } } - app_state.viewLocal.translate(app_state.rotation_center); + Eigen::Vector3f rotationCenter( + app_state.camera.euler.rotationCenter.x, app_state.camera.euler.rotationCenter.y, app_state.camera.euler.rotationCenter.z); + app_state.viewLocal.translate(rotationCenter); - app_state.viewLocal.translate(Eigen::Vector3f(app_state.translate_x, app_state.translate_y, app_state.translate_z)); - if (!app_state.lock_z) - app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + app_state.viewLocal.translate(Eigen::Vector3f( + app_state.camera.euler.translate.x, app_state.camera.euler.translate.y, app_state.camera.euler.translate.z)); + if (!app_state.camera.lockZ) + app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.camera.euler.rotateX * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else app_state.viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.camera.euler.rotateY * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - app_state.viewLocal.translate(-app_state.rotation_center); + app_state.viewLocal.translate(-rotationCenter); rlMultMatrixf(app_state.viewLocal.matrix().data()); } else - updateOrthoView(); + { + // Still updating app_state.viewLocal for the compass -- the rest of + // the original updateOrthoView() (rlOrtho + the ortho gizmo lookAt) + // now lives in raylib_widgets::OrbitCamera::updateOrtho(). + app_state.viewLocal.rotate(Eigen::AngleAxisf( + (app_state.camera.euler.rotateX + app_state.camera.euler.rotateY) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + app_state.camera.updateOrtho(ratio); + } - app_state.frame_view_3d = rlGetMatrixModelview(); - app_state.frame_proj_3d = rlGetMatrixProjection(); - frame_mvp_3d = MatrixMultiply(app_state.frame_view_3d, app_state.frame_proj_3d); + app_state.camera.captureFrameMatrices(); + frame_mvp_3d = MatrixMultiply(app_state.camera.frameView3D, app_state.camera.frameProj3D); showAxes(); + drawIntersectionGrids(session.point_clouds_container, session_dims); // renderLoopClosure() hides every scan except the current source/target // range while loop closure editing is active (see its comment) -- @@ -3282,27 +3970,17 @@ void display() { session.control_points.index_picked_point = -1; // reset picked point when pose changes - app_state.new_rotation_center.x() = - session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); - app_state.new_rotation_center.y() = - session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); - app_state.new_rotation_center.z() = - session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); + const auto& t = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation(); + Vector3 center = { static_cast(t.x()), static_cast(t.y()), static_cast(t.z()) }; - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; + Vector3 translate = app_state.camera.euler.translate; if (session.control_points.track_pose_with_camera) { - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - } - else - { - app_state.new_translate_x = app_state.translate_x; - app_state.new_translate_y = app_state.translate_y; + translate.x = -center.x; + translate.y = -center.y; } - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + + app_state.camera.startEulerTransition(app_state.camera.euler.rotateX, app_state.camera.euler.rotateY, translate, center); } // rlImGuiBegin() only polls raylib input into ImGui's IO and calls @@ -3318,7 +3996,11 @@ void display() ShowMainDockSpace(); if (session.control_points.is_imgui) - session.control_points.imgui(session.point_clouds_container, app_state.rotation_center); + session.control_points.imgui( + session.point_clouds_container, + Eigen::Vector3f( + app_state.camera.euler.rotationCenter.x, app_state.camera.euler.rotationCenter.y, + app_state.camera.euler.rotationCenter.z)); if (session.ground_control_points.is_imgui) session.ground_control_points.imgui(session.point_clouds_container); @@ -3343,7 +4025,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!app_state.is_ortho) + if (!app_state.camera.isOrtho) { // Named-field copy (not a raw struct memcpy): Matrix's // declared field order isn't guaranteed to match the @@ -3367,8 +4049,8 @@ void display() } else ImGuizmo::Manipulate( - app_state.m_ortho_gizmo_view, - app_state.m_ortho_projection, + app_state.camera.orthoGizmoView, + app_state.camera.orthoProjection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, @@ -3445,11 +4127,17 @@ void display() scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), + Eigen::Vector3d(app_state.camera.euler.rotationCenter.x, app_state.camera.euler.rotationCenter.y, app_state.camera.euler.rotationCenter.z), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), - app_state.viewer_decimate_point_cloud); + app_state.viewer_decimate_point_cloud, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, + static_cast(session.point_clouds_container.intersection_width)); scan_renderer.drawTrajectories( - session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff); + session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff, + session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection); observationPickingRender(observation_picking); @@ -3534,7 +4222,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!app_state.is_ortho) + if (!app_state.camera.isOrtho) { Matrix projMat = rlGetMatrixProjection(); Matrix modelMat = rlGetMatrixModelview(); @@ -3555,8 +4243,8 @@ void display() } else ImGuizmo::Manipulate( - app_state.m_ortho_gizmo_view, - app_state.m_ortho_projection, + app_state.camera.orthoGizmoView, + app_state.camera.orthoProjection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, @@ -4305,8 +4993,9 @@ void display() centroid /= static_cast(tls_registration.tum.tum_poses.size()); centroid -= session.point_clouds_container.offset; - app_state.new_rotation_center = centroid.cast(); - app_state.camera_transition_active = true; + Eigen::Vector3f centroid_f = centroid.cast(); + app_state.camera.eulerGoal.rotationCenter = Vector3{ centroid_f.x(), centroid_f.y(), centroid_f.z() }; + app_state.camera.eulerTransitionActive = true; } if (ImGui::IsItemHovered()) ImGui::SetTooltip( @@ -4716,17 +5405,12 @@ void display() } ImGui::EndDisabled(); - if (ImGui::MenuItem("Orthographic", "key O", &app_state.is_ortho)) + if (ImGui::MenuItem("Orthographic", "key O", &app_state.camera.isOrtho)) { - if (app_state.is_ortho) + if (app_state.camera.isOrtho) { - app_state.new_rotation_center = app_state.rotation_center; - app_state.new_rotate_x = 0.0; - app_state.new_rotate_y = 0.0; - app_state.new_translate_x = app_state.translate_x; - app_state.new_translate_y = app_state.translate_y; - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + app_state.camera.startEulerTransition( + 0.0f, 0.0f, app_state.camera.euler.translate, app_state.camera.euler.rotationCenter); } } if (ImGui::IsItemHovered()) @@ -4735,7 +5419,7 @@ void display() ImGui::MenuItem("Show axes", "key X", &app_state.show_axes); ImGui::MenuItem("Show compass/ruler", "key C", &app_state.compass_ruler); - ImGui::MenuItem("Lock Z", "Shift + Z", &app_state.lock_z, !app_state.is_ortho); + ImGui::MenuItem("Lock Z", "Shift + Z", &app_state.camera.lockZ, !app_state.camera.isOrtho); ImGui::Separator(); @@ -4884,16 +5568,16 @@ void display() SetMouseCursor(MOUSE_CURSOR_DEFAULT); } - cor_window(); + raylib_widgets::showEulerCenterOfRotationWindow(cor_gui, app_state.camera, xText, yText, zText); - info_window(infoLines, appShortcuts); + raylib_widgets::ShowInfoWindow(app_state.info_gui, infoLines, appShortcuts, HDMAPPING_VERSION_STRING, __DATE__); draw_translate_preview(); // 3D drawing is done -- switch to the 2D screen-space projection the // mini-compass (DrawLineEx/DrawText) and rlImGuiEnd()'s UI render both - // need (see end3DMatrixStack()'s comment). - end3DMatrixStack(); + // need (see raylib_widgets::end3DMatrixStack()'s comment). + raylib_widgets::end3DMatrixStack(io.DisplaySize.x, io.DisplaySize.y); if (is_loop_closure_gui) renderLoopClosureLabels(session.point_clouds_container); @@ -5009,14 +5693,9 @@ void translate_gui() translate_tool.has_transform = false; translate_tool.transform = Eigen::Affine3d::Identity(); - app_state.is_ortho = true; - app_state.new_rotation_center = app_state.rotation_center; - app_state.new_rotate_x = 0.0; - app_state.new_rotate_y = 0.0; - app_state.new_translate_x = app_state.translate_x; - app_state.new_translate_y = app_state.translate_y; - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + app_state.camera.isOrtho = true; + app_state.camera.startEulerTransition( + 0.0f, 0.0f, app_state.camera.euler.translate, app_state.camera.euler.rotationCenter); SetMouseCursor(MOUSE_CURSOR_CROSSHAIR); } @@ -5185,6 +5864,8 @@ void mouse(int glut_button, int state, int x, int y) if (session.control_points.index_pose >= 0 && session.control_points.index_pose < session.point_clouds_container.point_clouds.size()) { + Vector3 center = app_state.camera.eulerGoal.rotationCenter; + for (size_t j = 0; j < session.point_clouds_container.point_clouds[i].points_local.size(); j++) { const auto& p = session.point_clouds_container.point_clouds[i].points_local[j]; @@ -5196,20 +5877,13 @@ void mouse(int glut_button, int state, int x, int y) { min_distance = dist; - app_state.new_rotation_center.x() = vp.x(); - app_state.new_rotation_center.y() = vp.y(); - app_state.new_rotation_center.z() = vp.z(); + center = Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }; session.control_points.index_picked_point = j; } } - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; + app_state.camera.moveEulerRotationCenterTo(center); } } else @@ -5302,6 +5976,10 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), SetConfigFlags(flags); InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); + // raylib's default exit key (Esc) closes the window outright -- too easy + // to hit by accident while e.g. cancelling a dialog or backing out of a + // gizmo drag. Disabled; there's no keyboard shortcut for quitting. + SetExitKey(KEY_NULL); SetTargetFPS(60); // The hardcoded window_width/window_height default (1600x900) can be @@ -5322,7 +6000,7 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), scan_renderer.init(); - reshape(static_cast(window_width), static_cast(window_height)); + app_state.camera.applyPerspectiveProjection(static_cast(window_width), static_cast(window_height)); return true; } diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp deleted file mode 100644 index e0a062e0..00000000 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ /dev/null @@ -1,1073 +0,0 @@ -#include "rl_utils.h" - -#include "external/glad.h" -#include "raylib.h" -#include "raymath.h" -#include "rlgl.h" - -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#ifdef _WIN32 -// NOGDI/NOUSER: windows.h's wingdi.h/winuser.h #define (or, for CloseWindow/ -// ShowCursor, directly declare) identifiers that collide with raylib.h's -// own DrawText/CloseWindow/ShowCursor -- without these, windows.h wins and -// every call to raylib's DrawText() above silently becomes a call to the -// Win32 GDI DrawTextA() instead, which doesn't compile against raylib's -// arguments. NOUSER also strips SW_SHOWNORMAL (a windows.h macro), so -// ImGuiHyperlink's ShellExecuteA call below uses its literal value (1, a -// stable, decades-unchanged Win32 constant) instead. -// -// windows.h must come before shellapi.h -- shellapi.h depends on macros/ -// types windows.h defines, and this file (unlike core/src/utils.cpp, which -// gets windows.h transitively via its precompiled header before this same -// ordering matters) has nothing else pulling windows.h in first. -#define NOGDI -#define NOUSER -// clang-format off -#include -#include -// clang-format on -#endif - -/////////////////////////////////////////////////////////////////////////////////// -// Formerly 's extern globals -- now members of AppStateBase, -// defined in rl_utils.h (see its top comment for why). -/////////////////////////////////////////////////////////////////////////////////// - -bool cor_gui = false; - -bool scroll_hint_enabled = true; -bool scroll_hint_active = false; -int scroll_hint_count = 0; -float scroll_hint_accu = 0.0f; -double scroll_hint_lastT = 0.0; - -bool show_about = false; - -// ============================================================================ -// Formerly core/src/utils.cpp -- local now (see the big comment at the top -// of this file for why). Everywhere the original was pure ImGui/Eigen/GLM -// (no gl*/glu*/glut* calls), it's copied verbatim. Everywhere it touched -// legacy GL, it's reimplemented with rlgl's rl*() legacy-emulation API -// (a software matrix stack + immediate-mode layer that mirrors gl*()'s -// call shape but works under a core-profile context), or with raylib/ -// raymath equivalents (gluUnProject -> Vector3Unproject, glutBitmapCharacter -// -> DrawText). Function names/signatures/globals are unchanged so every -// call site elsewhere in this file (display(), mouse(), the panel -// functions, ...) needed no changes. -// ============================================================================ - -std::string truncPath(const std::string& fullPath) -{ - namespace fspath = std::filesystem; - fspath::path path(fullPath); - - auto parent1 = path.parent_path().filename().string(); - auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder - auto filename = path.filename().string(); - - return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; -} - -void wheel(int button, int dir, int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MouseWheel += dir; // or direction * 1.0f depending on your setup - - if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) - { - if (dir > 0) - { - if (app_state.is_ortho) - { - app_state.camera_ortho_xy_view_zoom -= 0.1f * app_state.camera_ortho_xy_view_zoom; - - if (app_state.camera_ortho_xy_view_zoom < 0.1) - { - app_state.camera_ortho_xy_view_zoom = 0.1; - } - } - else - { - if (io.KeyShift) - app_state.translate_z += 5.0f; - else - app_state.translate_z += 1.0f; - } - } - else - { - if (app_state.is_ortho) - app_state.camera_ortho_xy_view_zoom += 0.1 * app_state.camera_ortho_xy_view_zoom; - else - { - if (io.KeyShift) - app_state.translate_z -= 5.0f; - else - app_state.translate_z -= 1.0f; - } - } - - app_state.mouse_sensitivity = fabs(app_state.translate_z) / 100; // 1 for app_state.translate_z 50 (default zoom) - app_state.camera_transition_active = false; - - if (scroll_hint_enabled) - { - if (!scroll_hint_active) - { - scroll_hint_accu += fabs(dir); - - if (scroll_hint_accu > 30.0f) // tweak threshold - { - scroll_hint_accu = 0.0f; - scroll_hint_active = true; - scroll_hint_count++; - } - } - - if (scroll_hint_active) - scroll_hint_lastT = ImGui::GetTime(); - - // Reset and disable hint if Shift is pressed while scrolling - if (io.KeyShift || scroll_hint_count > 3) - { - scroll_hint_active = false; - scroll_hint_enabled = false; - } - } - } -} - -// Was glMatrixMode/glLoadIdentity/gluPerspective/glOrtho -- rewritten with -// rlgl's software matrix-stack API (RL_PROJECTION/RL_MODELVIEW), which -// works under raylib's core-profile context. gluPerspective(fovy, aspect, -// near, far) has no rl* equivalent, so it's expanded to the equivalent -// rlFrustum() call by hand (standard fovy -> frustum-bounds formula). -void reshape(int w, int h) -{ - // GetRenderWidth/Height(), not w/h: see display()'s matching comment -- - // w/h are logical points (window size), the GL viewport needs actual - // framebuffer pixels. - rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - rlMatrixMode(RL_PROJECTION); - rlLoadIdentity(); - if (!app_state.is_ortho) - { - const double fovy = 60.0; - const double aspect = (double)w / (double)h; - const double nearP = 0.01, farP = 10000.0; - const double top = nearP * tan(fovy * 0.5 * M_PI / 180.0); - const double bottom = -top; - const double right = top * aspect; - const double left = -right; - rlFrustum(left, right, bottom, top, nearP, farP); - } - else - { - ImGuiIO& io = ImGui::GetIO(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - rlOrtho( - -app_state.camera_ortho_xy_view_zoom, - app_state.camera_ortho_xy_view_zoom, - -app_state.camera_ortho_xy_view_zoom / ratio, - app_state.camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - } - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); -} - -// GL-free -- copied verbatim, minus the trailing glutPostRedisplay() (a -// no-op here: this app's main loop already redraws every frame). -void motion(int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - - if (!io.WantCaptureMouse) - { - float dx, dy; - dx = (float)(x - app_state.mouse_old_x); - dy = (float)(y - app_state.mouse_old_y); - - if (app_state.mouse_buttons & 1) // left button - { - app_state.rotate_x += dy * 0.2f; - app_state.rotate_y += dx * 0.2f; - breakCameraTransition(); - } - - if (app_state.mouse_buttons & 4) // right button - { - if (app_state.is_ortho) - { - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - Eigen::Vector3d v( - dx * (app_state.camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), - dy * (app_state.camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), - 0); - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = (app_state.rotate_x + app_state.rotate_y) * M_PI / 180.0; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - Eigen::Vector3d v_t = m * v; - app_state.camera_ortho_xy_view_shift_x += v_t.x(); - app_state.camera_ortho_xy_view_shift_y += v_t.y(); - } - else - { - app_state.translate_x += dx * 0.1f * app_state.mouse_sensitivity; - app_state.translate_y -= dy * 0.1f * app_state.mouse_sensitivity; - breakCameraTransition(); - } - } - - app_state.mouse_old_x = x; - app_state.mouse_old_y = y; - } -} - -// GL-free -- copied verbatim. -static bool first_time = true; - -void ShowMainDockSpace() -{ - ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | - ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | - ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; - - ImGuiViewport* viewport = ImGui::GetMainViewport(); - ImGui::SetNextWindowPos(viewport->WorkPos); - ImGui::SetNextWindowSize(viewport->WorkSize); - ImGui::SetNextWindowViewport(viewport->ID); - - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - - ImGui::Begin("MainDockSpace", nullptr, window_flags); - - ImGui::PopStyleVar(2); - - // This is the dockspace! - ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); - ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); - - if (first_time) - { - first_time = false; - - auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); - auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); - - ImGui::DockBuilderDockWindow("Console", dock_id_bottom); - ImGui::DockBuilderFinish(dockspace_id); - } - - ImGui::End(); -} - -// Was glBegin(GL_LINES)/glColor3f/glVertex3f/glEnd -- rl* rename. -void showAxes() -{ - if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes - { - rlBegin(RL_LINES); - rlColor3f(1.f, 1.f, 1.f); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x() + 1.f, app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x() - 1.f, app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y() - 1.f, app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y() + 1.f, app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z() - 1.f); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); - rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z() + 1.f); - rlEnd(); - } - - if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // origin axes - { - rlBegin(RL_LINES); - rlColor3f(1.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(100, 0.0f, 0.0f); - - rlColor3f(0.0f, 1.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 100, 0.0f); - - rlColor3f(0.0f, 0.0f, 1.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 100); - rlEnd(); - } -} - -// GL-free -- copied verbatim. -void updateCameraTransition() -{ - if (!app_state.camera_transition_active) - return; - - float t = 1.0f - powf(1.0f - std::min(ImGui::GetIO().DeltaTime * camera_transition_speed, 1.0f), 3.0f); - - bool doneXrc = fabs(app_state.new_rotation_center.x() - app_state.rotation_center.x()) < 0.01f; - bool doneYrc = fabs(app_state.new_rotation_center.y() - app_state.rotation_center.y()) < 0.01f; - bool doneZrc = fabs(app_state.new_rotation_center.z() - app_state.rotation_center.z()) < 0.01f; - bool doneXr = fabs(app_state.new_rotate_x - app_state.rotate_x) < 0.01f; - bool doneYr = fabs(app_state.new_rotate_y - app_state.rotate_y) < 0.01f; - bool doneXt = fabs(app_state.new_translate_x - app_state.translate_x) < 0.01f; - bool doneYt = fabs(app_state.new_translate_y - app_state.translate_y) < 0.01f; - bool doneZt = fabs(app_state.new_translate_z - app_state.translate_z) < 0.01f; - - if (!doneXrc) - app_state.rotation_center.x() += (app_state.new_rotation_center.x() - app_state.rotation_center.x()) * t; - if (!doneYrc) - app_state.rotation_center.y() += (app_state.new_rotation_center.y() - app_state.rotation_center.y()) * t; - if (!doneZrc) - app_state.rotation_center.z() += (app_state.new_rotation_center.z() - app_state.rotation_center.z()) * t; - if (!doneXr) - app_state.rotate_x += (app_state.new_rotate_x - app_state.rotate_x) * t; - if (!doneYr) - app_state.rotate_y += (app_state.new_rotate_y - app_state.rotate_y) * t; - if (!doneXt) - app_state.translate_x += (app_state.new_translate_x - app_state.translate_x) * t; - if (!doneYt) - app_state.translate_y += (app_state.new_translate_y - app_state.translate_y) * t; - if (!doneZt) - app_state.translate_z += (app_state.new_translate_z - app_state.translate_z) * t; - - app_state.camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); - - if (!app_state.camera_transition_active) - { - app_state.rotation_center = app_state.new_rotation_center; - app_state.rotate_x = app_state.new_rotate_x; - app_state.rotate_y = app_state.new_rotate_y; - app_state.translate_x = app_state.new_translate_x; - app_state.translate_y = app_state.new_translate_y; - app_state.translate_z = app_state.new_translate_z; - } -} - -// GL-free -- copied verbatim. -void breakCameraTransition() -{ - if (app_state.camera_transition_active == false) - return; - app_state.rotation_center = app_state.new_rotation_center; - app_state.camera_transition_active = false; -} - -// GL-free -- copied verbatim. -void setCameraPreset(CameraPreset preset) -{ - bool triggered = false; - - switch (preset) - { - case CAMERA_FRONT: - app_state.new_rotate_x = -90.0f; - app_state.new_rotate_y = +90.0f; - triggered = true; - break; - case CAMERA_BACK: - app_state.new_rotate_x = -90.0f; - app_state.new_rotate_y = -90.0f; - triggered = true; - break; - case CAMERA_LEFT: - app_state.new_rotate_x = -90.0f; - app_state.new_rotate_y = 180.0f; - triggered = true; - break; - case CAMERA_RIGHT: - app_state.new_rotate_x = -90.0f; - app_state.new_rotate_y = 0.0f; - triggered = true; - break; - case CAMERA_TOP: - app_state.new_rotate_x = 0.0f; - app_state.new_rotate_y = 90.0f; - triggered = true; - break; - case CAMERA_BOTTOM: - app_state.new_rotate_x = 180.0f; - app_state.new_rotate_y = -90.0f; - triggered = true; - break; - case CAMERA_ISO: - app_state.new_rotate_x = -35.264f; - app_state.new_rotate_y = 135.0f; - triggered = true; - break; - case CAMERA_RESET: - app_state.new_rotation_center = Eigen::Vector3f::Zero(); - app_state.new_rotate_x = 0; - app_state.new_rotate_y = 0; - app_state.new_translate_x = 0; - app_state.new_translate_y = 0; - app_state.new_translate_z = -50.0f; - app_state.mouse_sensitivity = fabs(app_state.translate_z) / 100; - - app_state.camera_ortho_xy_view_zoom = 10; - app_state.camera_ortho_xy_view_shift_x = 0.0; - app_state.camera_ortho_xy_view_shift_y = 0.0; - app_state.camera_mode_ortho_z_center_h = 0.0; - - app_state.viewer_decimate_point_cloud = 1000; - triggered = false; - break; - } - - if (triggered) - { - app_state.new_rotation_center = app_state.rotation_center; - app_state.new_translate_x = app_state.translate_x; - app_state.new_translate_y = app_state.translate_y; - app_state.new_translate_z = app_state.translate_z; - } - - app_state.camera_transition_active = true; -} - -// GL-free -- copied verbatim. -void camMenu() -{ - if (ImGui::BeginMenu("Camera")) - { - if (ImGui::MenuItem("Front (yz view)", "key F")) - setCameraPreset(CAMERA_FRONT); - if (ImGui::MenuItem("Back", "key B")) - setCameraPreset(CAMERA_BACK); - if (ImGui::MenuItem("Left (xz view)", "key L")) - setCameraPreset(CAMERA_LEFT); - if (ImGui::MenuItem("Right", "key R")) - setCameraPreset(CAMERA_RIGHT); - if (ImGui::MenuItem("Top (xy view)", "key T")) - setCameraPreset(CAMERA_TOP); - if (ImGui::MenuItem("Bottom", "key U")) - setCameraPreset(CAMERA_BOTTOM); - if (ImGui::MenuItem("Isometric", "key I")) - setCameraPreset(CAMERA_ISO); - ImGui::Separator(); - if (ImGui::MenuItem("Reset", "key Z")) - setCameraPreset(CAMERA_RESET); - - ImGui::EndMenu(); - } - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - ImGui::Text("Change camera view to fixed positions"); - ImGui::Separator(); - ImGui::Text("Metrics:"); - if (ImGui::BeginTable("Metrics", 4)) - { - ImGui::TableSetupColumn("Coord"); - ImGui::TableSetupColumn("rotate"); - ImGui::TableSetupColumn("translate"); - ImGui::TableSetupColumn("rot center"); - ImGui::TableHeadersRow(); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - - std::string text = "X"; - float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("X"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", app_state.rotate_x); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.translate_x); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.rotation_center.x()); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Y"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", app_state.rotate_y); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.translate_y); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.rotation_center.y()); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Z"); - - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.translate_z); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.rotation_center.y()); - - ImGui::EndTable(); - } - ImGui::Text("Mouse sensitivity: %.4f", app_state.mouse_sensitivity); - - ImGui::EndTooltip(); - } - - if (scroll_hint_active) - { - ImVec2 mousePos = ImGui::GetMousePos(); - ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); - ImGui::SetNextWindowBgAlpha(0.7f); - ImGui::BeginTooltip(); - ImGui::Text("Tip: To accelerate hold Shift + scroll"); - ImGui::EndTooltip(); - - if (ImGui::GetTime() - scroll_hint_lastT > 1) - scroll_hint_active = false; - } -} - -// GL-free -- copied verbatim. -void view_kbd_shortcuts() -{ - ImGuiIO& io = ImGui::GetIO(); - - if (io.WantCaptureKeyboard) - return; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - app_state.translate_x += 0.5f * app_state.mouse_sensitivity; - breakCameraTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - app_state.translate_x -= 0.5f * app_state.mouse_sensitivity; - breakCameraTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - app_state.translate_y += 0.5f * app_state.mouse_sensitivity; - breakCameraTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - app_state.translate_y -= 0.5f * app_state.mouse_sensitivity; - breakCameraTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - app_state.rotate_y -= 0.6; - breakCameraTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - app_state.rotate_y += 0.6; - breakCameraTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - app_state.rotate_x -= 0.6; - breakCameraTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - app_state.rotate_x += 0.6; - breakCameraTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) - cor_gui = true; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !app_state.is_ortho) - app_state.lock_z = !app_state.lock_z; - - if (io.KeyCtrl || io.KeyAlt || io.KeyShift) - return; - - if (ImGui::IsKeyPressed(ImGuiKey_B)) - setCameraPreset(CAMERA_BACK); - if (ImGui::IsKeyPressed(ImGuiKey_F)) - setCameraPreset(CAMERA_FRONT); - if (ImGui::IsKeyPressed(ImGuiKey_I)) - setCameraPreset(CAMERA_ISO); - if (ImGui::IsKeyPressed(ImGuiKey_L)) - setCameraPreset(CAMERA_LEFT); - if (ImGui::IsKeyPressed(ImGuiKey_R)) - setCameraPreset(CAMERA_RIGHT); - if (ImGui::IsKeyPressed(ImGuiKey_T)) - setCameraPreset(CAMERA_TOP); - if (ImGui::IsKeyPressed(ImGuiKey_U)) - setCameraPreset(CAMERA_BOTTOM); - if (ImGui::IsKeyPressed(ImGuiKey_Z)) - setCameraPreset(CAMERA_RESET); - - if (ImGui::IsKeyPressed(ImGuiKey_C, false)) - app_state.compass_ruler = !app_state.compass_ruler; - if (ImGui::IsKeyPressed(ImGuiKey_O, false)) - app_state.is_ortho = !app_state.is_ortho; - if (ImGui::IsKeyPressed(ImGuiKey_X, false)) - app_state.show_axes = !app_state.show_axes; - - if (ImGui::IsKeyPressed(ImGuiKey_1)) - app_state.point_size = 1; - if (ImGui::IsKeyPressed(ImGuiKey_2)) - app_state.point_size = 2; - if (ImGui::IsKeyPressed(ImGuiKey_3)) - app_state.point_size = 3; - if (ImGui::IsKeyPressed(ImGuiKey_4)) - app_state.point_size = 4; - if (ImGui::IsKeyPressed(ImGuiKey_5)) - app_state.point_size = 5; - if (ImGui::IsKeyPressed(ImGuiKey_6)) - app_state.point_size = 6; - if (ImGui::IsKeyPressed(ImGuiKey_7)) - app_state.point_size = 7; - if (ImGui::IsKeyPressed(ImGuiKey_8)) - app_state.point_size = 8; - if (ImGui::IsKeyPressed(ImGuiKey_9)) - app_state.point_size = 9; -} - -// GL-free -- copied verbatim. -void cor_window() -{ - if (cor_gui) - { - ImGui::OpenPopup("Center of rotation"); - cor_gui = false; - } - - if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) - { - ImGui::Text("Select new center of rotation [m]:"); - ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputFloat("X", &app_state.new_rotation_center.x(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(xText); - ImGui::SameLine(); - ImGui::InputFloat("Y", &app_state.new_rotation_center.y(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(yText); - ImGui::SameLine(); - ImGui::InputFloat("Z", &app_state.new_rotation_center.z(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(zText); - ImGui::PopItemWidth(); - - ImGui::Separator(); - - if (ImGui::Button("Set")) - { - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - app_state.new_translate_z = app_state.translate_z; - - app_state.camera_transition_active = true; - - ImGui::CloseCurrentPopup(); - } - - ImGui::SameLine(); - if (ImGui::Button("Cancel")) - { - ImGui::CloseCurrentPopup(); - } - - ImGui::EndPopup(); - } -} - -// GL-free -- copied verbatim. -void ImGuiHyperlink(const char* url, ImVec4 color) -{ - ImGui::PushStyleColor(ImGuiCol_Text, color); - ImGui::TextUnformatted(url); - ImGui::PopStyleColor(); - - ImVec2 pos = ImGui::GetItemRectMin(); - ImVec2 size = ImGui::GetItemRectSize(); - - if (ImGui::IsItemHovered()) - ImGui::SetMouseCursor(ImGuiMouseCursor_Hand); - - if (ImGui::IsItemHovered()) - { - ImDrawList* draw_list = ImGui::GetWindowDrawList(); - draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); - } - - if (ImGui::IsItemClicked()) - { -#ifdef _WIN32 - ShellExecuteA(0, "open", url, 0, 0, 1 /* SW_SHOWNORMAL, unavailable under NOUSER -- see this file's top comment */); -#elif __APPLE__ - std::string cmd = std::string("open ") + url; - system(cmd.c_str()); -#else - std::string cmd = std::string("xdg-open ") + url; - system(cmd.c_str()); -#endif - } -} - -// ShortcutEntry/ShowShortcutsTable moved to raylib_widgets (shared with the -// camera_lidar_* apps) -- the generic shortcut-label scaffolding that used to -// live here was merged directly into gui.cpp's appShortcuts (see the comment -// there), removing the two-list indirection (and a pre-existing off-by-one: -// gui.cpp's list was missing a "Ctrl+J" entry, silently misaligning every -// entry after "J" against this list's descriptions). - -// GL-free -- copied verbatim (glGetString(GL_RENDERER/...) is a plain -// string query, still valid under a core-profile context). -void info_window(const std::vector& infoLines, const std::vector& appShortcuts) -{ - if (!app_state.info_gui) - return; - - if (ImGui::Begin( - "Info", - &app_state.info_gui, - ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) - { - bool firstLine = true; - for (const auto& line : infoLines) - { - if (line.empty()) - ImGui::NewLine(); - else if (line.rfind("https://", 0) == 0) - ImGuiHyperlink(line.c_str()); - else - ImGui::Text(line.c_str()); - - if (firstLine) - { - ImGui::SameLine( - ImGui::GetWindowWidth() - ImGui::CalcTextSize("ImGui").x - ImGui::GetStyle().ItemSpacing.x * 2 - - ImGui::GetStyle().FramePadding.x * 2); - if (ImGui::Button("ImGui")) - show_about = true; - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - const GLubyte* renderer = glGetString(GL_RENDERER); - const GLubyte* version = glGetString(GL_VERSION); - const GLubyte* glslVersion = glGetString(GL_SHADING_LANGUAGE_VERSION); - - ImGui::Text("Renderer: %s", renderer); - ImGui::Text("OpenGL version supported: %s", version); - ImGui::Text("GLSL version: %s", glslVersion); - ImGui::EndTooltip(); - } - - firstLine = false; - } - } - - ImGui::NewLine(); - ImGui::Text("Author: Janusz Bedkowski & contributors"); - ImGui::NewLine(); - ImGui::Text("Part of HDMapping software suite"); - ImGui::Text("Version: %s (%s)", HDMAPPING_VERSION_STRING, __DATE__); - ImGui::Text("Project page: "); - ImGui::SameLine(); - ImGuiHyperlink("https://github.com/MapsHD/HDMapping"); - - ImGui::NewLine(); - ImGui::Separator(); - ImGui::NewLine(); - - raylib_widgets::ShowShortcutsTable(appShortcuts); - - if (show_about) - ImGui::ShowAboutWindow(&show_about); - } - - ImGui::End(); -} - -// Drawing itself now lives in raylib_widgets::drawCompassRuler (shared with -// the camera_lidar_* apps' identical overlay) -- this just adapts this app's -// own camera/background state (app_state.viewLocal's rotation matrix, -// app_state.translate_z zoom, app_state.bg_color) into that function's -// right/up/zoomDistance/rulerColor parameters. Row 0/1 of a world-to-eye -// rotation matrix R are exactly the world-space directions that map to -// eye-space +X/+Y (screen right/up): (R * dir).x() == dot(R.row(0), dir). -void drawMiniCompassWithRuler() -{ - const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); - Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; - Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; - Color rulerColor = - ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); - raylib_widgets::drawCompassRuler( - right, up, app_state.translate_z, rulerColor, raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); -} - -// GL-free -- copied verbatim. -float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p) -{ - return (plane.a * p.x() + plane.b * p.y() + plane.c * p.z() + plane.d); -} - -// GL-free -- copied verbatim. -Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) -{ - float TOLERANCE = 0.0001; - Eigen::Vector3d out_point; - out_point.x() = laser_beam.position.x(); - out_point.y() = laser_beam.position.y(); - out_point.z() = laser_beam.position.z(); - - float a = plane.a * laser_beam.direction.x() + plane.b * laser_beam.direction.y() + plane.c * laser_beam.direction.z(); - - if (a > -TOLERANCE && a < TOLERANCE) - { - return out_point; - } - - float distance = distanceToPlane(plane, out_point); - - out_point.x() = laser_beam.position.x() - laser_beam.direction.x() * (distance / a); - out_point.y() = laser_beam.position.y() - laser_beam.direction.y() * (distance / a); - out_point.z() = laser_beam.position.z() - laser_beam.direction.z() * (distance / a); - - return out_point; -} - -// Was gluUnProject(winX, winY, winZ, modelview, projection, viewport, ...) -// against glGetDoublev(GL_MODELVIEW/PROJECTION_MATRIX) -- rewritten with -// raymath's Vector3Unproject against rlgl's current matrix stack -// (rlGetMatrixModelview/Projection), following the same NDC-space -// conversion raylib's own GetScreenToWorldRayEx uses. The original's -// far point used winZ=-1000 (an out-of-range hack to get a point far along -// the ray, since gluUnProject doesn't clamp); using the actual far-plane -// NDC z=1 here is equally valid for the same purpose (only direction, not -// magnitude, of laser_beam.direction matters to callers). -LaserBeam GetLaserBeam(int x, int y) -{ - int width = GetScreenWidth(); - int height = GetScreenHeight(); - - float ndcX = (2.0f * (float)x) / (float)width - 1.0f; - float ndcY = 1.0f - (2.0f * (float)y) / (float)height; - - Matrix matView = app_state.frame_view_3d; - Matrix matProj = app_state.frame_proj_3d; - - Vector3 nearPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 0.0f }, matProj, matView); - Vector3 farPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 1.0f }, matProj, matView); - - LaserBeam laser_beam; - laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); - laser_beam.direction = Eigen::Vector3d(farPoint.x - nearPoint.x, farPoint.y - nearPoint.y, farPoint.z - nearPoint.z); - - return laser_beam; -} - -// GL-free -- copied verbatim. -double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) -{ - Eigen::Vector3d AP = point - line.position; - return (AP.cross(line.direction)).norm(); -} - -// GL-free -- copied verbatim. -void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index) -{ - picked_index = -1; - - const auto laser_beam = GetLaserBeam(x, y); - double min_distance = std::numeric_limits::max(); - int index_i = -1; - int index_j = -1; - - for (int i = 0; i < session_.point_clouds_container.point_clouds.size(); i++) - { - for (int j = 0; j < session_.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) - { - const auto& p = session_.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); - Eigen::Vector3d vp = session_.point_clouds_container.point_clouds[i].m_pose * p; - - double dist = distance_point_to_line(vp, laser_beam); - - if (dist < min_distance) - { - min_distance = dist; - index_i = i; - index_j = j; - - app_state.new_rotation_center.x() = static_cast(vp.x()); - app_state.new_rotation_center.y() = static_cast(vp.y()); - app_state.new_rotation_center.z() = static_cast(vp.z()); - - if (gcpPicking) - { - session_.ground_control_points.picking_mode_index_to_node_inner = index_i; - session_.ground_control_points.picking_mode_index_to_node_outer = index_j; - } - - picked_index = index_i; - } - } - } - - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - app_state.new_translate_z = app_state.translate_z; - app_state.camera_transition_active = true; -} - -// GL-free -- copied verbatim. -void setNewRotationCenter(int x, int y) -{ - const auto laser_beam = GetLaserBeam(x, y); - - RegistrationPlaneFeature::Plane pl; - - pl.a = 0; - pl.b = 0; - pl.c = 1; - pl.d = 0; - app_state.new_rotation_center = rayIntersection(laser_beam, pl).cast(); - - std::cout << "Setting new rotation center to:\n" << app_state.new_rotation_center << std::endl; - - app_state.new_rotate_x = app_state.rotate_x; - app_state.new_rotate_y = app_state.rotate_y; - app_state.new_translate_x = -app_state.new_rotation_center.x(); - app_state.new_translate_y = -app_state.new_rotation_center.y(); - app_state.new_translate_z = app_state.translate_z; - - app_state.camera_transition_active = true; -} - -// GL-free -- copied verbatim. -bool checkClHelp(int argc, char** argv) -{ - for (int i = 1; i < argc; ++i) - { - std::string arg(argv[i]); - - if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") - { - return true; - } - } - return false; -} - -// Was glOrtho + gluLookAt (folded into GL_PROJECTION, matching the -// original's call order -- gluLookAt ran before the GL_MODELVIEW switch -// below) -- rewritten as rlOrtho + rlMultMatrixf with the same lookAt -// matrix already computed via GLM for app_state.m_ortho_gizmo_view just above it. -void updateOrthoView() -{ - // still updating app_state.viewLocal for compass - app_state.viewLocal.rotate(Eigen::AngleAxisf((app_state.rotate_x + app_state.rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - - ImGuiIO& io = ImGui::GetIO(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - rlOrtho( - -app_state.camera_ortho_xy_view_zoom, - app_state.camera_ortho_xy_view_zoom, - -app_state.camera_ortho_xy_view_zoom / ratio, - app_state.camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - - glm::mat4 proj = glm::orthoLH_ZO( - -app_state.camera_ortho_xy_view_zoom, - app_state.camera_ortho_xy_view_zoom, - -app_state.camera_ortho_xy_view_zoom / ratio, - app_state.camera_ortho_xy_view_zoom / ratio, - -100, - 100); - - std::copy(&proj[0][0], &proj[3][3], app_state.m_ortho_projection); - - Eigen::Vector3d v_eye_t( - -app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t( - -app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); - - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -(app_state.rotate_x + app_state.rotate_y) * DEG_TO_RAD; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - - Eigen::Vector3d v_t = m * v; - - glm::mat4 lookat = glm::lookAt( - glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), - glm::vec3(v_t.x(), v_t.y(), v_t.z())); - std::copy(&lookat[0][0], &lookat[3][3], app_state.m_ortho_gizmo_view); - - rlMultMatrixf(&lookat[0][0]); - - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); -} - -// Restores rlgl's default 2D screen-space projection (matches what -// raylib's own EndMode3D() does), since this app drives the rlgl matrix -// stack manually (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/ -// display() above) instead of using raylib's BeginMode3D/EndMode3D -// wrapper. Must be called after all 3D drawing and before any 2D drawing -// (the mini-compass, ImGui) each frame. -void end3DMatrixStack() -{ - rlDrawRenderBatchActive(); - rlMatrixMode(RL_PROJECTION); - rlLoadIdentity(); - // io.DisplaySize, not GetScreenWidth()/GetScreenHeight(): reshape() - // sets the actual GL viewport from io.DisplaySize (see display()'s call - // to it), and the two can differ under DPI scaling -- this has to - // match the viewport currently in effect, or 2D screen-space math done - // against it (e.g. renderLoopClosureLabels()'s world-to-pixel - // projection) lands off by the mismatch. - ImGuiIO& io = ImGui::GetIO(); - rlOrtho(0, io.DisplaySize.x, io.DisplaySize.y, 0, 0.0f, 1.0f); - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); - rlDisableDepthTest(); -} diff --git a/apps/multi_view_tls_registration/rl_utils.h b/apps/multi_view_tls_registration/rl_utils.h deleted file mode 100644 index aed861bc..00000000 --- a/apps/multi_view_tls_registration/rl_utils.h +++ /dev/null @@ -1,180 +0,0 @@ -#pragma once - -// raylib-based replacement for the app-agnostic camera/picking/mini-compass/ -// misc-ImGui-widget API that this app used to get from -// (core/src/utils.cpp). That file is shared by several other GLUT apps and -// can't be changed, and raylib's context here is OpenGL 3.3 core profile (no -// fixed-function pipeline), so this header/its .cpp are a from-scratch -// reimplementation of the same API surface -- same names, same call shape -- -// backed by rlgl's rl*() legacy-GL-emulation API (a software matrix stack + -// immediate-mode layer that mirrors gl*()'s call shape but works under core -// profile) instead of real gl*()/glu*()/glut*() calls. See rl_utils.cpp's -// top comment for the function-by-function porting notes. -// -// Deliberately not shared with any other app (unlike Core/utils.hpp): this -// is multi_view_tls_registration's own local header, analogous to how -// core/src/utils.cpp served the same role for the GLUT apps. - -#include "raylib.h" - -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -/////////////////////////////////////////////////////////////////////////////////// - -const float DEG_TO_RAD = M_PI / 180.0f; -const float RAD_TO_DEG = 180.0f / M_PI; - -const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); - -const std::string out_fn = "Output file name"; - -constexpr float ImGuiNumberWidth = 120.0f; -constexpr const char* omText = "Roll (left/right)"; -constexpr const char* fiText = "Pitch (up/down)"; -constexpr const char* kaText = "Yaw (turning left/right)"; -constexpr const char* xText = "Longitudinal (forward/backward)"; -constexpr const char* yText = "Lateral (left/right)"; -constexpr const char* zText = "Vertical (up/down)"; - -const uint32_t window_width = 1600; -const uint32_t window_height = 900; - -const float camera_transition_speed = 1.0f; // higher = faster - -enum CameraPreset -{ - CAMERA_FRONT, - CAMERA_BACK, - CAMERA_LEFT, - CAMERA_RIGHT, - CAMERA_TOP, - CAMERA_BOTTOM, - CAMERA_ISO, - CAMERA_RESET -}; - -enum ColorScheme -{ - CS_SOLID, // fixed color - CS_RANDOM, // random - CS_GRAD_INTENS, // gradient based on intensity - CS_GRAD_ELEV, // gradient based on elevation - CS_GRAD_DIST, // gradient based on distance from rotation center - CS_FOLLOW // valid for trajectory -}; - -/////////////////////////////////////////////////////////////////////////////////// -struct AppStateBase -{ - int viewer_decimate_point_cloud = 2; - - int mouse_old_x = 0, mouse_old_y = 0; - int mouse_buttons = 0; - float mouse_sensitivity = 1.0f; - bool is_ortho = false; - bool lock_z = false; - bool show_axes = true; - ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); - int point_size = 1; - - bool info_gui = false; - bool compass_ruler = true; - - Eigen::Affine3f viewLocal; - - Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); - float rotate_x = -35.264f, rotate_y = 135.0f; - float translate_x = 0.0f, translate_y = 0.0f, translate_z = -50.0f; - - double camera_ortho_xy_view_zoom = 10; - double camera_ortho_xy_view_shift_x = 0.0; - double camera_ortho_xy_view_shift_y = 0.0; - double camera_mode_ortho_z_center_h = 0.0; - - // Target camera state for smooth transitions - Eigen::Vector3f new_rotation_center = rotation_center; - float new_rotate_x = rotate_x; - float new_rotate_y = rotate_y; - float new_translate_x = translate_x; - float new_translate_y = translate_y; - float new_translate_z = translate_z; - - // Transition timing - bool camera_transition_active = false; - - // The 3D view/projection rlgl had active during this frame's scene render, - // cached by display() right before end3DMatrixStack() resets rlgl's matrix - // stack to the 2D screen-space ortho used for the mini-compass/ImGui pass. - // GetLaserBeam() (called from mouse(), which runs *before* display() each - // frame -- see main()) needs these: querying rlGetMatrixModelview()/ - // rlGetMatrixProjection() live at that point would still see the previous - // frame's post-end3DMatrixStack() state (identity modelview, 2D ortho - // projection), not the 3D camera, producing a meaningless pick ray. - Matrix frame_view_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; - Matrix frame_proj_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; - - // Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width - // support is uniform enough here not to need a runtime check -- always true. - bool glLineWidthSupport = true; - - float m_ortho_projection[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - float m_ortho_gizmo_view[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -}; - -inline AppStateBase app_state; - -// Now shared with the camera_lidar_* apps -- see raylib_widgets/include/RaylibWidgets/ShortcutsTable.h. -using raylib_widgets::ShortcutEntry; - -/////////////////////////////////////////////////////////////////////////////////// - -std::string truncPath(const std::string& fullPath); - -void wheel(int button, int dir, int x, int y); -void reshape(int w, int h); -void motion(int x, int y); -void ShowMainDockSpace(); - -void showAxes(); -void updateCameraTransition(); -void breakCameraTransition(); -void setCameraPreset(CameraPreset preset); -void camMenu(); -void view_kbd_shortcuts(); -void cor_window(); - -void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); -void info_window(const std::vector& infoLines, const std::vector& appShortcuts); - -void drawMiniCompassWithRuler(); - -float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p); -Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); -LaserBeam GetLaserBeam(int x, int y); -double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); -void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index); - -void setNewRotationCenter(int x, int y); - -bool checkClHelp(int argc, char** argv); - -void updateOrthoView(); - -// New (no equivalent in the original ): restores rlgl's -// default 2D screen-space projection (matches what raylib's own -// EndMode3D() does), since this app drives the rlgl matrix stack manually -// (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/display()) instead of -// using raylib's BeginMode3D/EndMode3D wrapper. Must be called after all 3D -// drawing and before any 2D drawing (the mini-compass, ImGui) each frame. -void end3DMatrixStack(); diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index ed8d72e0..2097cad8 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -97,6 +97,15 @@ class ScanRenderer // (rebuild()'s default). Uses whatever rlgl projection/modelview // matrices are currently active (BeginMode3D or a manually-driven // rlMatrixMode/rlMultMatrixf stack, either works). + // + // xzIntersection/yzIntersection/xyIntersection/intersectionWidth: ported + // from PointCloud::render()'s per-point xz/yz/xy_intersection slab + // filter (core/src/point_cloud.cpp) -- with any of the three set, only + // points within intersectionWidth of the corresponding plane through the + // world origin (Z=0 for xy, Y=0 for xz, X=0 for yz) are drawn; with all + // three false (the default), every point draws, matching today's + // behavior. Applied in the fragment shader (a discard), not a CPU-side + // filter -- see raylib_render_shaders.hpp. void draw( const std::vector& pointClouds, float pointSize, @@ -105,7 +114,11 @@ class ScanRenderer float elevationMax = 1.f, const Eigen::Vector3d& distanceCenter = Eigen::Vector3d::Zero(), float distanceMax = 1.f, - int decimateStride = 1) const; + int decimateStride = 1, + bool xzIntersection = false, + bool yzIntersection = false, + bool xyIntersection = false, + float intersectionWidth = 0.1f) const; // Number of glDrawArrays calls draw() issued the last time it ran (one // per visible scan) -- raylib/rlgl don't expose a draw-call counter for @@ -150,9 +163,14 @@ class ScanRenderer // Also draws the fuse-inclination-from-IMU quad markers, the fixed-om/fi // rings, and the show_IMU/show_pose orientation crosses. If // visibleImuDiff is set, also draws the IMU-vs-LIO angular-difference - // debug lines. Intersection-slab gating (xz/yz/xy) is not implemented so - // these always draw when the relevant per-scan flag is set. - void drawTrajectories(const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const; + // debug lines. Matches the legacy PointCloud::render()'s trajectory + // section: this whole overlay is skipped entirely (not slab-filtered + // per-point like draw() above) whenever any of xzIntersection/ + // yzIntersection/xyIntersection is set, so a cross-section view isn't + // cluttered by trajectory points/markers outside the slab. + void drawTrajectories( + const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff, bool xzIntersection = false, + bool yzIntersection = false, bool xyIntersection = false) const; // Draws a single already-cached scan (see rebuild()) straight from its // persistent, full-resolution GPU buffer -- no CPU re-transform or @@ -249,6 +267,10 @@ class ScanRenderer int locElevMax_ = -1; int locDistCenter_ = -1; int locDistMax_ = -1; + int locXzOn_ = -1; + int locYzOn_ = -1; + int locXyOn_ = -1; + int locIntersectionWidth_ = -1; mutable int lastDrawCallCount_ = 0; mutable int lastVertexCount_ = 0; }; diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 2a1fb0da..9339f7f3 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -1591,32 +1591,7 @@ void PointCloud::render( } else { - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width) - { - render_point = true; - } - } - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - render_point = true; - } - } - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - render_point = true; - } - } - - if (!xz_intersection && !yz_intersection && !xy_intersection) - { - render_point = true; - } + render_point = true; } if (render_point) diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 0c3d7595..13c3baa2 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -54,6 +54,10 @@ void ScanRenderer::init() locElevMax_ = rlGetLocationUniform(shader_.id, "elevMax"); locDistCenter_ = rlGetLocationUniform(shader_.id, "distCenter"); locDistMax_ = rlGetLocationUniform(shader_.id, "distMax"); + locXzOn_ = rlGetLocationUniform(shader_.id, "xzOn"); + locYzOn_ = rlGetLocationUniform(shader_.id, "yzOn"); + locXyOn_ = rlGetLocationUniform(shader_.id, "xyOn"); + locIntersectionWidth_ = rlGetLocationUniform(shader_.id, "intersectionWidth"); } else { @@ -212,7 +216,11 @@ void ScanRenderer::draw( float elevationMax, const Eigen::Vector3d& distanceCenter, float distanceMax, - int decimateStride) const + int decimateStride, + bool xzIntersection, + bool yzIntersection, + bool xyIntersection, + float intersectionWidth) const { lastDrawCallCount_ = 0; lastVertexCount_ = 0; @@ -239,6 +247,19 @@ void ScanRenderer::draw( rlSetUniform(locDistCenter_, distCenterF, RL_SHADER_UNIFORM_VEC3, 1); rlSetUniform(locDistMax_, &distanceMax, RL_SHADER_UNIFORM_FLOAT, 1); + // Uniform program state persists across draw calls sharing this shader + // (drawCachedWithTransform()/drawPoints()/drawTrajectories() below all + // use it too), so these must be set unconditionally every call rather + // than relying on a previous call having left them at "off" -- see each + // of those functions' own explicit xzOn=yzOn=xyOn=0. + int xzOnInt = xzIntersection ? 1 : 0; + int yzOnInt = yzIntersection ? 1 : 0; + int xyOnInt = xyIntersection ? 1 : 0; + rlSetUniform(locXzOn_, &xzOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locYzOn_, &yzOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locXyOn_, &xyOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locIntersectionWidth_, &intersectionWidth, RL_SHADER_UNIFORM_FLOAT, 1); + // decimateStride > 1 skips points by widening the vertex attribute // stride the GPU fetches from (e.g. stride=10 reads every 10th vertex), // rather than by re-uploading a thinned-out buffer -- so it's a pure @@ -391,6 +412,13 @@ void ScanRenderer::drawCachedWithTransform( rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); rlSetUniform(locColorMode_, &colorMode, RL_SHADER_UNIFORM_INT, 1); + // Explicitly off: uniform program state persists across draw calls + // sharing this shader, and this preview draw should never be + // slab-filtered regardless of what draw() last set these to. + int intersectionOff = 0; + rlSetUniform(locXzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locYzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locXyOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); rlEnableVertexArray(gpu.vao); // Explicitly re-specified (not just inherited from the VAO's last @@ -454,6 +482,11 @@ void ScanRenderer::drawPoints(const PointsGPU& gpu, Color color, float pointSize rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); + // Explicitly off -- see drawCachedWithTransform()'s comment on why. + int intersectionOff = 0; + rlSetUniform(locXzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locYzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locXyOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); rlEnableVertexArray(gpu.vao); glDrawArrays(GL_POINTS, 0, gpu.vertexCount); @@ -584,10 +617,16 @@ void ScanRenderer::rebuildTrajectoryGPU(TrajGPU& traj, const PointCloud& pc, int traj.vertexCount = static_cast(data.size() / 3); } -void ScanRenderer::drawTrajectories(const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const +void ScanRenderer::drawTrajectories( + const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff, bool xzIntersection, + bool yzIntersection, bool xyIntersection) const { int stride = reduceRenderedTrajectory < 1 ? 1 : reduceRenderedTrajectory; + // GPU cache bookkeeping stays unconditional (independent of whether this + // call ends up drawing anything below) so toggling intersection mode + // on/off across frames can't leave trajClouds_ out of sync with + // pointClouds or leak a GPU buffer. if (trajClouds_.size() > pointClouds.size()) { for (size_t i = pointClouds.size(); i < trajClouds_.size(); ++i) @@ -597,6 +636,16 @@ void ScanRenderer::drawTrajectories(const std::vector& pointClouds, } trajClouds_.resize(pointClouds.size()); + // Matches the legacy PointCloud::render()'s trajectory section: skip + // this whole overlay (trajectory points, IMU-diff lines, quad markers, + // rings, orientation crosses) entirely whenever a cross-section slab is + // active, rather than slab-filtering it per-point like draw() does for + // the main point cloud. + if (xzIntersection || yzIntersection || xyIntersection) + { + return; + } + if (shaderValid_) { rlDrawRenderBatchActive(); @@ -605,6 +654,11 @@ void ScanRenderer::drawTrajectories(const std::vector& pointClouds, rlSetUniformMatrix(locMVP_, mvp); int colorModeFlat = 0; rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1); + // Explicitly off -- see drawCachedWithTransform()'s comment on why. + int intersectionOff = 0; + rlSetUniform(locXzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locYzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locXyOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); } for (size_t idx = 0; idx < pointClouds.size(); ++idx) diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp index e2eafcc4..b0995e97 100644 --- a/core/src/raylib_render_shaders.hpp +++ b/core/src/raylib_render_shaders.hpp @@ -44,6 +44,17 @@ uniform float elevMin; uniform float elevMax; uniform vec3 distCenter; uniform float distMax; +// Intersection-slab gating: was PointCloud::render()'s (core/src/point_cloud.cpp, +// legacy immediate-mode GL) per-point xz/yz/xy_intersection check, ported here +// as a fragment discard instead of a CPU-side render_point bool. xzOn/yzOn/xyOn +// are 0/1 (bool uniforms aren't portable pre-4.x); intersectionWidth is the +// same half-width in world units on both sides of each cutting plane through +// the world origin. With all three off, every point draws (matches the +// original's "no intersection mode active -> always render" fallback). +uniform int xzOn; +uniform int yzOn; +uniform int xyOn; +uniform float intersectionWidth; in float fragIntensity; in vec3 fragWorldPos; out vec4 finalColor; @@ -51,6 +62,15 @@ out vec4 finalColor; R"( void main() { + if (xzOn != 0 || yzOn != 0 || xyOn != 0) + { + bool inSlab = false; + if (xzOn != 0 && abs(fragWorldPos.y) < intersectionWidth) inSlab = true; + if (yzOn != 0 && abs(fragWorldPos.x) < intersectionWidth) inSlab = true; + if (xyOn != 0 && abs(fragWorldPos.z) < intersectionWidth) inSlab = true; + if (!inSlab) discard; + } + if (colorMode == 1) { finalColor = vec4(jet(fragIntensity), pointColor.a); diff --git a/raylib_widgets/CMakeLists.txt b/raylib_widgets/CMakeLists.txt index 12ae74d9..285f7abd 100644 --- a/raylib_widgets/CMakeLists.txt +++ b/raylib_widgets/CMakeLists.txt @@ -4,13 +4,16 @@ project(raylib_widgets) # Small UI overlay helpers (compass/ruler, DPI-aware window fit-to-screen, # a generic shortcuts-help table, an orbit camera + its center-of-rotation -# dialog) shared between apps/multi_view_tls_registration and the -# camera_lidar_* apps. Depends on raylib + imgui_raylib only -- no Eigen, no -# core, no calib_core -- so it's linkable from both the core_raylib side -# (already coupled to core/core_math) and the calib_core side (deliberately -# not) without adding coupling either way; every current/planned consumer is -# a raylib+ImGui app already, so depending on imgui_raylib (not the separate -# GLUT-backed `imgui` target) doesn't add anything new for them. +# dialog, app-shell scaffolding) shared between apps/multi_view_tls_registration +# and the camera_lidar_* apps. Depends on raylib + imgui_raylib + Eigen +# (header-only) -- no `core`/`calib_core` -- so it's linkable from both the +# core_raylib side (already coupled to core/core_math) and the calib_core +# side (which links Eigen itself already, but deliberately not `core`) +# without adding `core` coupling either way; every current/planned consumer +# is a raylib+ImGui app already, so depending on imgui_raylib (not the +# separate GLUT-backed `imgui` target) doesn't add anything new for them. +# Only RayPlaneD.h actually uses Eigen -- everything else stays exactly as +# dependency-light as before. add_library(raylib_widgets STATIC src/CompassRuler.cpp src/WindowFit.cpp @@ -18,9 +21,11 @@ add_library(raylib_widgets STATIC src/OrbitCamera.cpp src/CenterOfRotationWindow.cpp src/PointPicking.cpp + src/AppShell.cpp + src/RayPlaneD.cpp ) -target_include_directories(raylib_widgets PUBLIC include) +target_include_directories(raylib_widgets PUBLIC include ${EIGEN3_INCLUDE_DIR}) target_link_libraries(raylib_widgets PUBLIC raylib imgui_raylib) set_target_properties(raylib_widgets PROPERTIES POSITION_INDEPENDENT_CODE ON) diff --git a/raylib_widgets/include/RaylibWidgets/AppShell.h b/raylib_widgets/include/RaylibWidgets/AppShell.h new file mode 100644 index 00000000..1ecf9e56 --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/AppShell.h @@ -0,0 +1,38 @@ +#pragma once +#include + +#include + +#include +#include + +// Generic ImGui app-shell helpers -- a full-window docking host, a +// clickable hyperlink, and an "about/shortcuts" info window -- shared +// between apps/multi_view_tls_registration (rl_utils.cpp's +// ShowMainDockSpace/ImGuiHyperlink/info_window) and any other raylib_widgets +// consumer that wants the same scaffolding. Depends on raylib_widgets' +// existing imgui_raylib link only -- no Eigen/core coupling. +namespace raylib_widgets { + +// Full-window transparent host for ImGui docking. Call once per frame, +// before building any dockable panel windows. On its first call ever, splits +// off a left column and a bottom "Console" dock -- later calls just +// re-assert the dockspace. +void ShowMainDockSpace(); + +// Clickable-looking text that opens `url` in the system browser when +// clicked (Windows: ShellExecuteA; macOS/Linux: `open`/`xdg-open`). +void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); + +// "About/shortcuts" window. `open` is both the visibility toggle (the call +// is a no-op while false) and gets cleared by the window's own close +// button/X. infoLines is rendered as plain text, except a blank line (blank +// line in the window) or one starting with "https://" (rendered as a +// clickable ImGuiHyperlink()); appShortcuts is rendered via +// ShowShortcutsTable(). versionString/buildDate go into a "Version: %s (%s)" +// line (pass e.g. HDMAPPING_VERSION_STRING and __DATE__). +void ShowInfoWindow( + bool& open, const std::vector& infoLines, const std::vector& appShortcuts, + const char* versionString, const char* buildDate); + +} // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h b/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h index 2969b40b..4389ad60 100644 --- a/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h +++ b/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h @@ -16,4 +16,17 @@ namespace raylib_widgets { // camera.moveTargetTo(). void showCenterOfRotationWindow(bool& open, OrbitCamera& camera); +// Same dialog, for OrbitCamera's Euler mode instead of its azimuth/ +// elevation/target mode above -- was multi_view_tls_registration's local +// cor_window(). Binds to camera.eulerGoal.rotationCenter (edited in place, +// matching cor_window()'s original behavior of editing its own persistent +// new_rotation_center field rather than a freshly-seeded copy) and calls +// camera.moveEulerRotationCenterTo() on Set. xTooltip/yTooltip/zTooltip are +// optional per-field hover tooltips (multi_view_tls_registration's original +// had these -- e.g. "Longitudinal (forward/backward)"); pass nullptr (the +// default) to skip. +void showEulerCenterOfRotationWindow( + bool& open, OrbitCamera& camera, const char* xTooltip = nullptr, const char* yTooltip = nullptr, + const char* zTooltip = nullptr); + } // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/OrbitCamera.h b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h index 22eaff18..a288ad9f 100644 --- a/raylib_widgets/include/RaylibWidgets/OrbitCamera.h +++ b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h @@ -18,10 +18,19 @@ struct OrbitCamera { // Center-of-rotation ("target") smooth-transition state, mirroring // multi_view_tls_registration's rotation_center/new_rotation_center/ - // camera_transition_active animation -- only target eases, azimuth/ - // elevation/distance are left as-is (matching that app's actual - // click-picked/typed-center behavior). - Vector3 transitionTarget = target; + // camera_transition_active animation. moveTargetTo() (the only way + // pickGroundPlaneTarget()/CenterOfRotationWindow trigger a transition) + // only ever moves `target` -- it freezes transitionAzimuth/Elevation/ + // Distance/SphericalOrthoHeight to their current values, so those ease + // "toward themselves" (a no-op) whenever only the target changes, + // exactly like before this struct grew orientation/zoom transitions. + // setPreset() below is what actually drives those too, as one combined + // transition. + Vector3 transitionTarget = target; + float transitionAzimuth = azimuth; + float transitionElevation = elevation; + float transitionDistance = distance; + float transitionSphericalOrthoHeight = 20.f; // matches sphericalOrthoHeight's default below bool transitionActive = false; float transitionSpeed = 1.f; @@ -43,6 +52,182 @@ struct OrbitCamera { // plane, starting a transition of `target` to the hit point on success. // Returns false (no-op) when the ray is ~parallel to the plane. bool pickGroundPlaneTarget(Vector2 mouse, Camera3D cam, float groundY = 0.f); + + // Orthographic toggle for this (azimuth/elevation/distance) camera -- + // raylib's own Camera3D/BeginMode3D already supports + // CAMERA_ORTHOGRAPHIC natively, so unlike the Euler mode below this + // doesn't need any manual rlgl matrix-stack work: toRaylib() just sets + // cam.projection/cam.fovy accordingly. sphericalOrthoHeight is the + // view's vertical extent in world units (raylib's ortho fovy + // convention) -- update()'s wheel-zoom branch adjusts this instead of + // `distance` while sphericalOrtho is set, mirroring + // multi_view_tls_registration's separate ortho zoom. Named distinctly + // from the Euler mode's own isOrtho/orthoProjection/etc. below -- + // that's a separate camera model bolted onto this same struct, not + // shared state. + bool sphericalOrtho = false; + float sphericalOrthoHeight = 20.f; + + // Fixed preset views, ported from multi_view_tls_registration's + // CameraPreset/setCameraPreset() (see EulerPreset/setEulerPreset() + // below) but expressed in this camera's own azimuth/elevation terms -- + // the two apps use different world "up" conventions (this one is + // raylib's standard Y-up; multi_view_tls_registration's Euler mode is + // Z-up), so the *numbers* differ, but each preset names the same + // logical view. Front/Back/Left/Right/Top/Bottom/Iso only change + // azimuth/elevation -- target/distance/sphericalOrthoHeight (pan/zoom) + // carry over unchanged, matching the original's behavior of preserving + // pan and zoom across a preset switch. Reset also restores target/ + // distance/sphericalOrthoHeight to this struct's own defaults. + enum class ViewPreset { Front, Back, Left, Right, Top, Bottom, Iso, Reset }; + void setPreset(ViewPreset preset); + + // ------------------------------------------------------------------ + // Euler-angle + orthographic mode, ported from + // multi_view_tls_registration's original rotate_x/rotate_y/ + // translate_x/y/z/rotation_center/is_ortho camera (driven through + // rlgl's manual matrix stack -- rlMatrixMode/rlFrustum/rlOrtho/ + // rlMultMatrixf -- rather than raylib's Camera3D/BeginMode3D). Kept + // entirely separate from the azimuth/elevation/distance/target fields + // and methods above, which remain the camera_lidar_* apps' own model + // and are untouched by any of this. + // ------------------------------------------------------------------ + + struct EulerState { + float rotateX = -35.264f; + float rotateY = 135.0f; + Vector3 translate = { 0.f, 0.f, -50.f }; + Vector3 rotationCenter = { 0.f, 0.f, 0.f }; + }; + + EulerState euler; + + // Eased-transition goal state for the Euler mode (separate from + // transitionTarget/transitionActive above, which only ever ease + // `target`). + EulerState eulerGoal = euler; + bool eulerTransitionActive = false; + float eulerTransitionSpeed = 1.f; + + bool lockZ = false; + bool isOrtho = false; + float orthoZoom = 10.f; + float orthoShiftX = 0.f, orthoShiftY = 0.f; + float orthoZCenterH = 0.f; + // Mirrors the original's app_state.mouse_sensitivity: fabs(translate.z)/100, + // recomputed by zoom()/setEulerPreset(Reset) -- scales keyboard-shortcut + // and perspective-pan step sizes so they still feel proportional at any + // zoom level. + float eulerMouseSensitivity = 1.f; + + // Ortho projection + ortho "gizmo view" matrices, rebuilt each frame by + // updateOrtho() -- flat column-major float[16] (index = col*4+row), the + // layout ImGuizmo::Manipulate expects. Distinct from the real rlgl + // projection updateOrtho() also sets up (right-handed, [-1,1] depth, + // via rlOrtho): these mirror the original's GLM-computed + // left-handed/[0,1]-depth pair, used only for ImGuizmo's own matrices. + float orthoProjection[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + float orthoGizmoView[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + + // This frame's captured 3D view/projection -- set by captureFrameMatrices() + // right after applyEuler.../updateOrtho() build them, and read back by + // picking code (e.g. multi_view_tls_registration's GetLaserBeam) to build + // an unproject ray. Defaults to identity so an out-of-sequence call + // before the first captureFrameMatrices() degrades gracefully instead of + // unprojecting through a zero matrix. + Matrix frameView3D = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + Matrix frameProj3D = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + + enum class EulerPreset { Front, Back, Left, Right, Top, Bottom, Iso, Reset }; + + // Sets up rlgl's projection matrix stack for a perspective frame -- was + // multi_view_tls_registration's reshape()'s !is_ortho branch. w/h should + // be logical window size (the GL viewport itself is still sized from the + // actual framebuffer, matching the original). + void applyPerspectiveProjection(int w, int h) const; + // Was reshape()'s is_ortho branch. `aspect` is the caller's display + // width/height (e.g. ImGui's io.DisplaySize.x/y) -- kept as a plain + // parameter rather than reading ImGui here, since OrbitCamera otherwise + // depends on nothing but raylib. + void applyOrthoProjection(float aspect) const; + + // Builds this frame's ortho projection + folds an eye/center/up lookAt + // (derived from euler.rotateX/rotateY and the ortho pan/height state) + // into rlgl's current (projection) matrix -- was updateOrthoView(), + // minus the caller's own app-specific compass-rotation bookkeeping + // (that line stays with the caller, which already owns an Eigen + // dependency this widget deliberately doesn't have). Must be called + // with RL_PROJECTION active and freshly loaded identity (display()'s + // job, same as today), and leaves RL_MODELVIEW active + identity after, + // like the original. + void updateOrtho(float aspect); + + // Stores rlGetMatrixModelview()/rlGetMatrixProjection() into + // frameView3D/frameProj3D. Call once per frame right after + // applyPerspectiveProjection()+the view multiply, or updateOrtho() -- + // before any 2D/ImGui drawing resets the matrix stack. + void captureFrameMatrices(); + + // Unprojects screen point (x,y) through frameView3D/frameProj3D into a + // world-space ray -- was multi_view_tls_registration's GetLaserBeam(). + // Pass the actual framebuffer width/height (e.g. GetScreenWidth()/ + // GetScreenHeight()), not a cached value, so a mid-frame resize can't + // desync it from the viewport applyPerspectiveProjection()/ + // applyOrthoProjection() actually set up. Like GetLaserBeam(), this + // reads whatever frameView3D/frameProj3D last captured -- typically the + // *previous* frame's matrices when called from input handling that runs + // before this frame's captureFrameMatrices(), which is deliberate (see + // frameView3D's declaration comment). + Ray eulerScreenRay(int x, int y, int screenW, int screenH) const; + + // Left-drag: orbit. Was motion()'s `mouse_buttons & 1` branch. + void dragOrbit(float dx, float dy); + // Right-drag in perspective mode: pan. Was motion()'s `mouse_buttons & 4` + // branch, !is_ortho case. + void dragPanPerspective(float dx, float dy); + // Right-drag in ortho mode: pan. Was motion()'s `mouse_buttons & 4` + // branch, is_ortho case. displayW/displayH: caller's display size (e.g. + // ImGui's io.DisplaySize), needed for the same screen-space-to-world + // scaling the original used. + void dragPanOrtho(float dx, float dy, float displayW, float displayH); + // Mouse-wheel zoom/dolly -- was wheel()'s camera-mutating branch, now + // scaled by the actual per-frame scroll magnitude (raylib's + // GetMouseWheelMove()) instead of only its sign. A physical mouse wheel + // notch reports ~1.0, so passing that through reproduces the original's + // fixed per-notch step exactly; a trackpad's continuous smooth-scroll + // deltas (fractions of 1.0, many per frame during a gesture) now move + // the camera proportionally instead of snapping a full step on every + // one of those frames -- which is what made a trackpad pinch/scroll + // feel like it "teleported" before this took magnitude into account. + // Deliberately does NOT call breakEulerTransition() (which also snaps + // rotationCenter) -- matches the original, which just cleared + // camera_transition_active here, leaving rotationCenter wherever it had + // eased to. + void zoom(float wheelDelta, bool shiftHeld); + + // Was setCameraPreset(). Note: the original's CAMERA_RESET also reset + // an app-specific viewer_decimate_point_cloud setting outside the + // camera's own state -- callers still need to do that themselves. + void setEulerPreset(EulerPreset preset); + // General eased-transition start -- was the repeated + // `new_rotation_center = ...; new_rotate_x = rotate_x; ...; + // camera_transition_active = true;` pattern at each call site. + void startEulerTransition(float rotateX, float rotateY, Vector3 translate, Vector3 rotationCenter); + // Convenience for the common "recenter on a picked point" pattern (was + // repeated in getClosestTrajectoryPoint()/setNewRotationCenter()/ + // cor_window()'s Set button): keeps the current rotate angles and Z + // distance, moves rotationCenter to `center`, and sets translate.xy to + // -center.xy so the new center recentres under the (unchanged) view. + void moveEulerRotationCenterTo(Vector3 center); + // Eases euler toward eulerGoal; call once per frame with the frame's + // delta time. Was updateCameraTransition(). + void updateEulerTransition(float dt); + // Was breakCameraTransition(): snaps euler.rotationCenter to + // eulerGoal.rotationCenter and ends the transition. Note this leaves + // rotateX/rotateY/translate wherever they had already eased to -- matches + // the original exactly (asymmetric with updateEulerTransition's + // full-snap-on-completion). + void breakEulerTransition(); }; // Small 3-axis crosshair marking the current center of rotation. Call inside @@ -51,4 +236,17 @@ struct OrbitCamera { // newly-picked/typed center along with the camera. void drawRotationCenterCross(Vector3 center, float size, Color color); +// Restores rlgl's default 2D screen-space projection (matches what raylib's +// own EndMode3D() does) -- for apps that drive the rlgl matrix stack +// manually each frame (rlMatrixMode/rlFrustum/rlOrtho/rlMultMatrixf, e.g. +// via OrbitCamera's Euler-mode applyPerspectiveProjection()/updateOrtho()) +// instead of using raylib's BeginMode3D/EndMode3D wrapper, which can't be +// mixed with that -- was multi_view_tls_registration's end3DMatrixStack(). +// Must be called after all 3D drawing and before any 2D drawing (a mini- +// compass, ImGui) each frame. displayW/displayH should be the caller's +// logical display size (e.g. ImGui's io.DisplaySize.x/y), not +// GetScreenWidth()/Height() -- those can differ under DPI scaling, and this +// has to match whatever the viewport was actually set from. +void end3DMatrixStack(float displayW, float displayH); + } // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/PointPicking.h b/raylib_widgets/include/RaylibWidgets/PointPicking.h index 0cd24bc1..2e53b679 100644 --- a/raylib_widgets/include/RaylibWidgets/PointPicking.h +++ b/raylib_widgets/include/RaylibWidgets/PointPicking.h @@ -35,4 +35,15 @@ namespace raylib_widgets float viewportHeightPx, float pixelThreshold, size_t& outIndex); + + // Finds the point in `points` whose perpendicular distance to `ray`'s + // infinite line is smallest -- unlike pickNearestPoint() above (which + // rejects anything past `pixelThreshold` screen pixels, or behind the + // camera), this always returns the single closest point, however far. + // For "snap the camera to the nearest waypoint on a sparse trajectory/ + // polyline" use cases -- shared between multi_view_tls_registration_ + // step_2's rotation-center-on-trajectory picking and + // camera_lidar_trajectory_viewer's equivalent. Returns false only when + // `points` is empty. + bool pickNearestPointOnLine(const Vector3* points, size_t count, const Ray& ray, size_t& outIndex); } // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/RayPlaneD.h b/raylib_widgets/include/RaylibWidgets/RayPlaneD.h new file mode 100644 index 00000000..25622353 --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/RayPlaneD.h @@ -0,0 +1,31 @@ +#pragma once +#include + +// Double-precision ray/plane/line math -- for callers whose world +// coordinates need more than raylib's native float precision (e.g. large +// projected/UTM-like coordinates), shared between +// multi_view_tls_registration_step_2's ground-plane rotation-center picking +// and any other consumer with the same need. Deliberately kept out of +// PointPicking.h, which stays Eigen-free for camera_lidar_calibration/ +// camera_lidar_intrinsics_calib -- only link/include this header if you +// actually need double precision. +namespace raylib_widgets { + +// Intersects the ray (rayPos + t*rayDir, t >= 0 not enforced -- callers +// that only want points ahead of the ray should check the sign of the +// result relative to rayPos themselves) with the plane +// a*x + b*y + c*z + d = 0. Returns false (outPoint untouched) if the ray is +// ~parallel to the plane (rayDir's component along the plane normal is +// within 1e-4 of zero). +bool intersectPlane( + const Eigen::Vector3d& rayPos, const Eigen::Vector3d& rayDir, double a, double b, double c, double d, Eigen::Vector3d& outPoint); + +// Perpendicular distance from `point` to the infinite line through rayPos +// with direction rayDir. Does NOT normalize by |rayDir| -- the result scales +// with rayDir's own magnitude, matching the original call sites' only use +// of this (finding the nearest of several candidate points against one +// fixed ray, where a constant scale factor across all candidates doesn't +// change which one wins). +double distancePointToLine(const Eigen::Vector3d& point, const Eigen::Vector3d& rayPos, const Eigen::Vector3d& rayDir); + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/AppShell.cpp b/raylib_widgets/src/AppShell.cpp new file mode 100644 index 00000000..0e29a850 --- /dev/null +++ b/raylib_widgets/src/AppShell.cpp @@ -0,0 +1,151 @@ +#include "RaylibWidgets/AppShell.h" + +#include // ImGui::DockBuilder* + +#ifdef _WIN32 +// NOGDI/NOUSER: windows.h's wingdi.h/winuser.h #define (or, for CloseWindow/ +// ShowCursor, directly declare) identifiers that collide with raylib.h's own +// DrawText/CloseWindow/ShowCursor in any translation unit that also includes +// raylib.h -- harmless here (this file doesn't), but matches the convention +// every other raylib_widgets .cpp that touches windows.h follows. NOUSER +// also strips SW_SHOWNORMAL, so ImGuiHyperlink's ShellExecuteA call below +// uses its literal value (1, a stable, decades-unchanged Win32 constant) +// instead. +#define NOGDI +#define NOUSER +// clang-format off +#include +#include +// clang-format on +#endif + +namespace raylib_widgets { + +void ShowMainDockSpace() +{ + static bool first_time = true; + + ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | + ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; + + ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(viewport->WorkPos); + ImGui::SetNextWindowSize(viewport->WorkSize); + ImGui::SetNextWindowViewport(viewport->ID); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + + ImGui::Begin("MainDockSpace", nullptr, window_flags); + + ImGui::PopStyleVar(2); + + ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); + ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); + + if (first_time) + { + first_time = false; + + auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); + auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); + (void)dock_id_left; + + ImGui::DockBuilderDockWindow("Console", dock_id_bottom); + ImGui::DockBuilderFinish(dockspace_id); + } + + ImGui::End(); +} + +void ImGuiHyperlink(const char* url, ImVec4 color) +{ + ImGui::PushStyleColor(ImGuiCol_Text, color); + ImGui::TextUnformatted(url); + ImGui::PopStyleColor(); + + ImVec2 pos = ImGui::GetItemRectMin(); + ImVec2 size = ImGui::GetItemRectSize(); + + if (ImGui::IsItemHovered()) + ImGui::SetMouseCursor(ImGuiMouseCursor_Hand); + + if (ImGui::IsItemHovered()) + { + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); + } + + if (ImGui::IsItemClicked()) + { +#ifdef _WIN32 + ShellExecuteA(0, "open", url, 0, 0, 1 /* SW_SHOWNORMAL, unavailable under NOUSER -- see this file's top comment */); +#elif __APPLE__ + std::string cmd = std::string("open ") + url; + system(cmd.c_str()); +#else + std::string cmd = std::string("xdg-open ") + url; + system(cmd.c_str()); +#endif + } +} + +void ShowInfoWindow( + bool& open, const std::vector& infoLines, const std::vector& appShortcuts, const char* versionString, + const char* buildDate) +{ + if (!open) + return; + + static bool show_about = false; + + if (ImGui::Begin( + "Info", &open, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | + ImGuiWindowFlags_NoCollapse)) + { + bool firstLine = true; + for (const auto& line : infoLines) + { + if (line.empty()) + ImGui::NewLine(); + else if (line.rfind("https://", 0) == 0) + ImGuiHyperlink(line.c_str()); + else + ImGui::TextUnformatted(line.c_str()); + + if (firstLine) + { + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("ImGui").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); + if (ImGui::Button("ImGui")) + show_about = true; + + firstLine = false; + } + } + + ImGui::NewLine(); + ImGui::Text("Author: Janusz Bedkowski & contributors"); + ImGui::NewLine(); + ImGui::Text("Part of HDMapping software suite"); + ImGui::Text("Version: %s (%s)", versionString, buildDate); + ImGui::Text("Project page: "); + ImGui::SameLine(); + ImGuiHyperlink("https://github.com/MapsHD/HDMapping"); + + ImGui::NewLine(); + ImGui::Separator(); + ImGui::NewLine(); + + ShowShortcutsTable(appShortcuts); + + if (show_about) + ImGui::ShowAboutWindow(&show_about); + } + + ImGui::End(); +} + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/CenterOfRotationWindow.cpp b/raylib_widgets/src/CenterOfRotationWindow.cpp index 017acd61..6a86beb9 100644 --- a/raylib_widgets/src/CenterOfRotationWindow.cpp +++ b/raylib_widgets/src/CenterOfRotationWindow.cpp @@ -43,4 +43,54 @@ void showCenterOfRotationWindow(bool& open, OrbitCamera& camera) } } +void showEulerCenterOfRotationWindow( + bool& open, OrbitCamera& camera, const char* xTooltip, const char* yTooltip, const char* zTooltip) +{ + if (open) + { + ImGui::OpenPopup("Center of rotation"); + open = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) + { + // Bound directly to camera.eulerGoal.rotationCenter (not a + // separately-initialized local) -- matches the original + // cor_window(), which edited its own persistent new_rotation_center + // field in place rather than seeding a fresh copy each time the + // popup opens. + Vector3& pending = camera.eulerGoal.rotationCenter; + + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(120.f); + ImGui::InputFloat("X", &pending.x, 0.0f, 0.0f, "%.3f"); + if (xTooltip && ImGui::IsItemHovered()) + ImGui::SetTooltip("%s", xTooltip); + ImGui::SameLine(); + ImGui::InputFloat("Y", &pending.y, 0.0f, 0.0f, "%.3f"); + if (yTooltip && ImGui::IsItemHovered()) + ImGui::SetTooltip("%s", yTooltip); + ImGui::SameLine(); + ImGui::InputFloat("Z", &pending.z, 0.0f, 0.0f, "%.3f"); + if (zTooltip && ImGui::IsItemHovered()) + ImGui::SetTooltip("%s", zTooltip); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + camera.moveEulerRotationCenterTo(pending); + ImGui::CloseCurrentPopup(); + } + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } +} + } // namespace raylib_widgets diff --git a/raylib_widgets/src/OrbitCamera.cpp b/raylib_widgets/src/OrbitCamera.cpp index 0984242c..528a33d3 100644 --- a/raylib_widgets/src/OrbitCamera.cpp +++ b/raylib_widgets/src/OrbitCamera.cpp @@ -1,6 +1,7 @@ #include "RaylibWidgets/OrbitCamera.h" #include "raymath.h" +#include "rlgl.h" #include #include @@ -19,8 +20,13 @@ Camera3D OrbitCamera::toRaylib() const { cam.position = pos; cam.target = target; cam.up = {0.f, 1.f, 0.f}; - cam.fovy = 45.f; - cam.projection = CAMERA_PERSPECTIVE; + if (sphericalOrtho) { + cam.fovy = sphericalOrthoHeight; + cam.projection = CAMERA_ORTHOGRAPHIC; + } else { + cam.fovy = 45.f; + cam.projection = CAMERA_PERSPECTIVE; + } return cam; } @@ -47,16 +53,29 @@ void OrbitCamera::update(bool active) { target = Vector3Add(target, Vector3Scale(up, d.y * speed)); cancelTransition(); } - // Scroll → zoom + // Scroll → zoom (distance in perspective, sphericalOrthoHeight in ortho + // -- mirrors multi_view_tls_registration's own perspective-dolly vs. + // ortho-zoom split in OrbitCamera::zoom()). float wheel = GetMouseWheelMove(); if (wheel != 0.f) { - distance -= wheel * distance * 0.1f; - distance = std::max(0.5f, distance); + if (sphericalOrtho) { + sphericalOrthoHeight -= wheel * sphericalOrthoHeight * 0.1f; + sphericalOrthoHeight = std::max(0.5f, sphericalOrthoHeight); + } else { + distance -= wheel * distance * 0.1f; + distance = std::max(0.5f, distance); + } } } void OrbitCamera::moveTargetTo(Vector3 newTarget) { transitionTarget = newTarget; + // Freeze the others to their current value so they ease "toward + // themselves" (a no-op) -- see this field's declaration comment. + transitionAzimuth = azimuth; + transitionElevation = elevation; + transitionDistance = distance; + transitionSphericalOrthoHeight = sphericalOrthoHeight; transitionActive = true; } @@ -73,17 +92,79 @@ void OrbitCamera::updateTransition(float dt) { if (!doneY) target.y += (transitionTarget.y - target.y) * t; if (!doneZ) target.z += (transitionTarget.z - target.z) * t; - transitionActive = !(doneX && doneY && doneZ); - if (!transitionActive) + // Azimuth is an unbounded, unwrapped angle (unlike elevation, which + // update() clamps to +-89) -- after enough manual dragging it can sit + // far outside [0,360), so the raw goal-minus-current delta can be a + // near-full-circle the "long way around". Wrapping it into (-180,180] + // first makes the eased rotation always take the shorter path. + float deltaAz = transitionAzimuth - azimuth; + while (deltaAz > 180.f) deltaAz -= 360.f; + while (deltaAz < -180.f) deltaAz += 360.f; + bool doneAz = std::fabs(deltaAz) < 0.01f; + if (!doneAz) azimuth += deltaAz * t; + + bool doneEl = std::fabs(transitionElevation - elevation) < 0.01f; + if (!doneEl) elevation += (transitionElevation - elevation) * t; + + bool doneDist = std::fabs(transitionDistance - distance) < 0.01f; + if (!doneDist) distance += (transitionDistance - distance) * t; + + bool doneOrtho = std::fabs(transitionSphericalOrthoHeight - sphericalOrthoHeight) < 0.01f; + if (!doneOrtho) sphericalOrthoHeight += (transitionSphericalOrthoHeight - sphericalOrthoHeight) * t; + + transitionActive = !(doneX && doneY && doneZ && doneAz && doneEl && doneDist && doneOrtho); + if (!transitionActive) { target = transitionTarget; + azimuth = transitionAzimuth; + elevation = transitionElevation; + distance = transitionDistance; + sphericalOrthoHeight = transitionSphericalOrthoHeight; + } } void OrbitCamera::cancelTransition() { if (!transitionActive) return; target = transitionTarget; + azimuth = transitionAzimuth; + elevation = transitionElevation; + distance = transitionDistance; + sphericalOrthoHeight = transitionSphericalOrthoHeight; transitionActive = false; } +void OrbitCamera::setPreset(ViewPreset preset) { + // Only Front/Back/Left/Right/Top/Bottom/Iso change azimuth/elevation; + // Reset changes target/distance/sphericalOrthoHeight too -- see this + // method's declaration comment. + transitionTarget = target; + transitionDistance = distance; + transitionSphericalOrthoHeight = sphericalOrthoHeight; + + switch (preset) { + case ViewPreset::Front: transitionAzimuth = 0.f; transitionElevation = 0.f; break; + case ViewPreset::Back: transitionAzimuth = 180.f; transitionElevation = 0.f; break; + case ViewPreset::Left: transitionAzimuth = 90.f; transitionElevation = 0.f; break; + case ViewPreset::Right: transitionAzimuth = -90.f; transitionElevation = 0.f; break; + // +-89, not +-90: exactly 90 sits right on update()'s own elevation + // clamp boundary -- landing a hair inside it means a manual drag right + // after the transition completes doesn't immediately re-clamp/jump. + case ViewPreset::Top: transitionAzimuth = azimuth; transitionElevation = 89.f; break; + case ViewPreset::Bottom: transitionAzimuth = azimuth; transitionElevation = -89.f; break; + // Matches multi_view_tls_registration's CAMERA_ISO angle (35.264 deg + // -- true isometric) for consistency between the two apps. + case ViewPreset::Iso: transitionAzimuth = 45.f; transitionElevation = 35.264f; break; + case ViewPreset::Reset: + transitionAzimuth = 30.f; + transitionElevation = 25.f; + transitionDistance = 30.f; + transitionTarget = { 0.f, 0.f, 0.f }; + transitionSphericalOrthoHeight = 20.f; + break; + } + + transitionActive = true; +} + bool OrbitCamera::pickGroundPlaneTarget(Vector2 mouse, Camera3D cam, float groundY) { Ray ray = GetScreenToWorldRay(mouse, cam); @@ -97,10 +178,283 @@ bool OrbitCamera::pickGroundPlaneTarget(Vector2 mouse, Camera3D cam, float groun return true; } +// --------------------------------------------------------------------- +// Euler-angle + orthographic mode -- see OrbitCamera.h's comment on the +// `euler`/`isOrtho`/... fields for what this ports from. +// --------------------------------------------------------------------- + +void OrbitCamera::applyPerspectiveProjection(int w, int h) const { + // GetRenderWidth/Height(), not w/h: the GL viewport must be sized in + // actual framebuffer pixels, which can differ from logical w/h under + // DPI scaling -- matches the original reshape(). + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + + const double fovy = 60.0; + const double aspect = (double)w / (double)h; + const double nearP = 0.01, farP = 10000.0; + const double top = nearP * tan(fovy * 0.5 * (double)DEG2RAD); + const double bottom = -top; + const double right = top * aspect; + const double left = -right; + rlFrustum(left, right, bottom, top, nearP, farP); + + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +void OrbitCamera::applyOrthoProjection(float aspect) const { + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + rlOrtho(-orthoZoom, orthoZoom, -orthoZoom / aspect, orthoZoom / aspect, -100000, 100000); + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +void OrbitCamera::updateOrtho(float aspect) { + // Real rlgl projection (right-handed, [-1,1] depth) -- what the scene + // actually renders through. + rlOrtho(-orthoZoom, orthoZoom, -orthoZoom / aspect, orthoZoom / aspect, -100000, 100000); + + // Second, independent projection matrix for ImGuizmo only -- ported + // from the original's glm::orthoLH_ZO(-zoom, zoom, -zoom/aspect, + // zoom/aspect, -100, 100) (left-handed, [0,1] depth -- distinct from + // rlOrtho's own convention above). Written directly as flat + // column-major floats (index = col*4+row) rather than through raylib's + // Matrix type: Matrix's m0..m15 field *names* follow that same + // indexing, but its C++ declaration order groups them by row, so a raw + // `&matrix.m0` reinterpret-as-float* would silently scramble this. + for (float& f : orthoProjection) f = 0.f; + orthoProjection[0] = 1.f / orthoZoom; + orthoProjection[5] = aspect / orthoZoom; + orthoProjection[10] = 1.f / 200.f; // 1/(zFar - zNear), zNear=-100, zFar=100 + orthoProjection[14] = 0.5f; // -zNear/(zFar - zNear) + orthoProjection[15] = 1.f; + + // Ortho "camera" for the gizmo view: looks straight down at + // (orthoShiftX, orthoShiftY, orthoZCenterH) from 10 units above, with + // "up" rotated to match the current in-plane rotation + // (rotateX + rotateY, same yaw the perspective mode's rlMultMatrixf + // view applies) -- ported from the original's TaitBryan-yaw-only + // rotation of (0,1,0), which reduces to this plain 2D rotation. + Vector3 eye = { -orthoShiftX, orthoShiftY, orthoZCenterH + 10.f }; + Vector3 center = { -orthoShiftX, orthoShiftY, orthoZCenterH }; + float ka = -(euler.rotateX + euler.rotateY) * DEG2RAD; + Vector3 up = { -std::sin(ka), std::cos(ka), 0.f }; + + Matrix lookat = MatrixLookAt(eye, center, up); + float16 flat = MatrixToFloatV(lookat); + for (int i = 0; i < 16; ++i) orthoGizmoView[i] = flat.v[i]; + + // Folds the lookAt into rlgl's *projection* stack (not modelview) -- + // matches the original's "was glOrtho + gluLookAt folded into + // GL_PROJECTION" comment: this app drives its ortho camera entirely + // through the projection matrix, leaving modelview identity (see the + // rlMatrixMode(RL_MODELVIEW) below). MatrixToFloat(), not `&lookat.m0` + // -- see this function's comment above on why the latter is unsafe. + rlMultMatrixf(MatrixToFloat(lookat)); + + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +void OrbitCamera::captureFrameMatrices() { + frameView3D = rlGetMatrixModelview(); + frameProj3D = rlGetMatrixProjection(); +} + +Ray OrbitCamera::eulerScreenRay(int x, int y, int screenW, int screenH) const { + float ndcX = (2.0f * (float)x) / (float)screenW - 1.0f; + float ndcY = 1.0f - (2.0f * (float)y) / (float)screenH; + + // Far point uses NDC z=1 (the actual far plane) rather than an + // out-of-range hack -- only direction, not magnitude, matters to + // callers, and this matches raylib's own GetScreenToWorldRayEx. + Vector3 nearPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 0.0f }, frameProj3D, frameView3D); + Vector3 farPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 1.0f }, frameProj3D, frameView3D); + + Ray ray; + ray.position = nearPoint; + ray.direction = Vector3Subtract(farPoint, nearPoint); + return ray; +} + +void OrbitCamera::dragOrbit(float dx, float dy) { + euler.rotateX += dy * 0.2f; + euler.rotateY += dx * 0.2f; + breakEulerTransition(); +} + +void OrbitCamera::dragPanPerspective(float dx, float dy) { + euler.translate.x += dx * 0.1f * eulerMouseSensitivity; + euler.translate.y -= dy * 0.1f * eulerMouseSensitivity; + breakEulerTransition(); +} + +void OrbitCamera::dragPanOrtho(float dx, float dy, float displayW, float displayH) { + if (displayW <= 0.f || displayH <= 0.f) return; + + float ratio = displayW / displayH; + float vx = dx * (orthoZoom / displayW * 2.f); + float vy = dy * (orthoZoom / displayH * 2.f / ratio); + + // Rotate the pan vector by the current in-plane rotation (rotateX + + // rotateY), matching the original's TaitBryan-yaw-only rotation + // (opposite sign convention from updateOrtho()'s gizmo-up rotation -- + // this is a different vector, ported independently from the same + // original source). + float ka = (euler.rotateX + euler.rotateY) * DEG2RAD; + float ck = std::cos(ka), sk = std::sin(ka); + + orthoShiftX += ck * vx - sk * vy; + orthoShiftY += sk * vx + ck * vy; + // Deliberately no breakEulerTransition() call here -- matches the + // original, which didn't break the transition on ortho pan either. +} + +void OrbitCamera::zoom(float wheelDelta, bool shiftHeld) { + if (wheelDelta == 0.f) return; + + // wheelDelta=+-1 (a real mouse wheel's per-notch magnitude) reproduces + // the original's fixed per-notch step exactly; any other magnitude + // (trackpad smooth-scroll) scales it proportionally -- see this + // function's declaration comment. + if (isOrtho) { + orthoZoom -= 0.1f * orthoZoom * wheelDelta; + if (orthoZoom < 0.1f) orthoZoom = 0.1f; + } else { + float step = shiftHeld ? 5.f : 1.f; + euler.translate.z += step * wheelDelta; + } + + eulerMouseSensitivity = std::fabs(euler.translate.z) / 100.f; + // Not breakEulerTransition(): matches the original, which just cleared + // camera_transition_active here without snapping rotation_center. + eulerTransitionActive = false; +} + +void OrbitCamera::setEulerPreset(EulerPreset preset) { + bool triggered = false; + + switch (preset) { + case EulerPreset::Front: + eulerGoal.rotateX = -90.0f; + eulerGoal.rotateY = +90.0f; + triggered = true; + break; + case EulerPreset::Back: + eulerGoal.rotateX = -90.0f; + eulerGoal.rotateY = -90.0f; + triggered = true; + break; + case EulerPreset::Left: + eulerGoal.rotateX = -90.0f; + eulerGoal.rotateY = 180.0f; + triggered = true; + break; + case EulerPreset::Right: + eulerGoal.rotateX = -90.0f; + eulerGoal.rotateY = 0.0f; + triggered = true; + break; + case EulerPreset::Top: + eulerGoal.rotateX = 0.0f; + eulerGoal.rotateY = 90.0f; + triggered = true; + break; + case EulerPreset::Bottom: + eulerGoal.rotateX = 180.0f; + eulerGoal.rotateY = -90.0f; + triggered = true; + break; + case EulerPreset::Iso: + eulerGoal.rotateX = -35.264f; + eulerGoal.rotateY = 135.0f; + triggered = true; + break; + case EulerPreset::Reset: + eulerGoal.rotationCenter = { 0.f, 0.f, 0.f }; + eulerGoal.rotateX = 0.f; + eulerGoal.rotateY = 0.f; + eulerGoal.translate = { 0.f, 0.f, -50.0f }; + eulerMouseSensitivity = std::fabs(euler.translate.z) / 100.f; + + orthoZoom = 10.f; + orthoShiftX = 0.f; + orthoShiftY = 0.f; + orthoZCenterH = 0.f; + + triggered = false; + break; + } + + if (triggered) { + eulerGoal.rotationCenter = euler.rotationCenter; + eulerGoal.translate = euler.translate; + } + + eulerTransitionActive = true; +} + +void OrbitCamera::startEulerTransition(float rotateX, float rotateY, Vector3 translate, Vector3 rotationCenter) { + eulerGoal.rotateX = rotateX; + eulerGoal.rotateY = rotateY; + eulerGoal.translate = translate; + eulerGoal.rotationCenter = rotationCenter; + eulerTransitionActive = true; +} + +void OrbitCamera::moveEulerRotationCenterTo(Vector3 center) { + startEulerTransition(euler.rotateX, euler.rotateY, Vector3{ -center.x, -center.y, euler.translate.z }, center); +} + +void OrbitCamera::updateEulerTransition(float dt) { + if (!eulerTransitionActive) return; + + float t = 1.0f - std::pow(1.0f - std::min(dt * eulerTransitionSpeed, 1.0f), 3.0f); + + auto ease = [t](float& cur, float goal) -> bool { + if (std::fabs(goal - cur) < 0.01f) return true; + cur += (goal - cur) * t; + return false; + }; + + bool doneRcX = ease(euler.rotationCenter.x, eulerGoal.rotationCenter.x); + bool doneRcY = ease(euler.rotationCenter.y, eulerGoal.rotationCenter.y); + bool doneRcZ = ease(euler.rotationCenter.z, eulerGoal.rotationCenter.z); + bool doneRx = ease(euler.rotateX, eulerGoal.rotateX); + bool doneRy = ease(euler.rotateY, eulerGoal.rotateY); + bool doneTx = ease(euler.translate.x, eulerGoal.translate.x); + bool doneTy = ease(euler.translate.y, eulerGoal.translate.y); + bool doneTz = ease(euler.translate.z, eulerGoal.translate.z); + + eulerTransitionActive = !(doneRcX && doneRcY && doneRcZ && doneRx && doneRy && doneTx && doneTy && doneTz); + + if (!eulerTransitionActive) euler = eulerGoal; +} + +void OrbitCamera::breakEulerTransition() { + if (!eulerTransitionActive) return; + euler.rotationCenter = eulerGoal.rotationCenter; + eulerTransitionActive = false; +} + void drawRotationCenterCross(Vector3 center, float size, Color color) { DrawLine3D(Vector3{ center.x - size, center.y, center.z }, Vector3{ center.x + size, center.y, center.z }, color); DrawLine3D(Vector3{ center.x, center.y - size, center.z }, Vector3{ center.x, center.y + size, center.z }, color); DrawLine3D(Vector3{ center.x, center.y, center.z - size }, Vector3{ center.x, center.y, center.z + size }, color); } +void end3DMatrixStack(float displayW, float displayH) { + rlDrawRenderBatchActive(); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + rlOrtho(0, displayW, displayH, 0, 0.0f, 1.0f); + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); + rlDisableDepthTest(); +} + } // namespace raylib_widgets diff --git a/raylib_widgets/src/PointPicking.cpp b/raylib_widgets/src/PointPicking.cpp index 733573d8..30327a5a 100644 --- a/raylib_widgets/src/PointPicking.cpp +++ b/raylib_widgets/src/PointPicking.cpp @@ -41,4 +41,33 @@ namespace raylib_widgets return found; } + + bool pickNearestPointOnLine(const Vector3* points, size_t count, const Ray& ray, size_t& outIndex) + { + if (points == nullptr || count == 0) + return false; + + float dirLenSq = Vector3LengthSqr(ray.direction); + if (dirLenSq < 1e-12f) + return false; // degenerate ray direction + + float bestDistSq = std::numeric_limits::max(); + bool found = false; + + for (size_t i = 0; i < count; ++i) + { + Vector3 toP = Vector3Subtract(points[i], ray.position); + Vector3 crossed = Vector3CrossProduct(toP, ray.direction); + float perpDistSq = Vector3LengthSqr(crossed) / dirLenSq; // squared perpendicular distance + + if (perpDistSq < bestDistSq) + { + bestDistSq = perpDistSq; + outIndex = i; + found = true; + } + } + + return found; + } } // namespace raylib_widgets diff --git a/raylib_widgets/src/RayPlaneD.cpp b/raylib_widgets/src/RayPlaneD.cpp new file mode 100644 index 00000000..1e783862 --- /dev/null +++ b/raylib_widgets/src/RayPlaneD.cpp @@ -0,0 +1,25 @@ +#include "RaylibWidgets/RayPlaneD.h" + +namespace raylib_widgets { + +bool intersectPlane( + const Eigen::Vector3d& rayPos, const Eigen::Vector3d& rayDir, double a, double b, double c, double d, Eigen::Vector3d& outPoint) +{ + const double kTolerance = 0.0001; + + double denom = a * rayDir.x() + b * rayDir.y() + c * rayDir.z(); + if (denom > -kTolerance && denom < kTolerance) + return false; + + double distFromPlane = a * rayPos.x() + b * rayPos.y() + c * rayPos.z() + d; + outPoint = rayPos - rayDir * (distFromPlane / denom); + return true; +} + +double distancePointToLine(const Eigen::Vector3d& point, const Eigen::Vector3d& rayPos, const Eigen::Vector3d& rayDir) +{ + Eigen::Vector3d AP = point - rayPos; + return (AP.cross(rayDir)).norm(); +} + +} // namespace raylib_widgets