From b3476183704705e6ed389901a39d29c4f944c3ba Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Thu, 28 Dec 2023 13:34:10 +0100
Subject: [PATCH] Add look_at() viz interface

---
 .../mola_kernel/interfaces/VizInterface.h     |  6 +++-
 mola_viz/include/mola_viz/MolaViz.h           |  5 ++++
 mola_viz/src/MolaViz.cpp                      | 29 +++++++++++++++++++
 3 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/mola_kernel/include/mola_kernel/interfaces/VizInterface.h b/mola_kernel/include/mola_kernel/interfaces/VizInterface.h
index a5dce818..f677bab7 100644
--- a/mola_kernel/include/mola_kernel/interfaces/VizInterface.h
+++ b/mola_kernel/include/mola_kernel/interfaces/VizInterface.h
@@ -12,6 +12,7 @@
 #pragma once
 
 #include <mrpt/gui/CDisplayWindowGUI.h>  // nanogui
+#include <mrpt/math/TPoint3D.h>
 
 #include <future>
 #include <memory>
@@ -46,7 +47,10 @@ class VizInterface
         const std::string& viewportName = "main",
         const std::string& parentWindow = "main") = 0;
 
-   protected:
+    virtual std::future<bool> update_viewport_look_at(
+        const mrpt::math::TPoint3Df& lookAt,
+        const std::string&           viewportName = "main",
+        const std::string&           parentWindow = "main") = 0;
 };
 
 }  // namespace mola
diff --git a/mola_viz/include/mola_viz/MolaViz.h b/mola_viz/include/mola_viz/MolaViz.h
index 2bd762fe..5da1fb3d 100644
--- a/mola_viz/include/mola_viz/MolaViz.h
+++ b/mola_viz/include/mola_viz/MolaViz.h
@@ -89,6 +89,11 @@ class MolaViz : public ExecutableBase, public VizInterface
         const std::string& viewportName = "main",
         const std::string& parentWindow = DEFAULT_WINDOW_NAME) override;
 
+    std::future<bool> update_viewport_look_at(
+        const mrpt::math::TPoint3Df& lookAt,
+        const std::string&           viewportName = "main",
+        const std::string& parentWindow = DEFAULT_WINDOW_NAME) override;
+
     /** @} */
 
     /** @name mola-viz GUI update handlers registry
diff --git a/mola_viz/src/MolaViz.cpp b/mola_viz/src/MolaViz.cpp
index 6092f122..41bacedd 100644
--- a/mola_viz/src/MolaViz.cpp
+++ b/mola_viz/src/MolaViz.cpp
@@ -679,6 +679,35 @@ std::future<bool> MolaViz::update_3d_object(
     return task->get_future();
 }
 
+std::future<bool> MolaViz::update_viewport_look_at(
+    const mrpt::math::TPoint3Df& lookAt, const std::string& viewportName,
+    const std::string& parentWindow)
+{
+    using return_type = bool;
+
+    auto task = std::make_shared<std::packaged_task<return_type()>>(
+        [this, lookAt, viewportName, parentWindow]() {
+            MRPT_LOG_DEBUG_STREAM(
+                "update_viewport_look_at() lookAt=" << lookAt.asString());
+
+            ASSERT_(windows_.count(parentWindow));
+            auto topWin = windows_.at(parentWindow);
+            ASSERT_(topWin);
+
+            // No need to acquire the mutex, since this task will be run
+            // in the proper moment in the proper thread:
+            ASSERT_(topWin->background_scene);
+            topWin->camera().setCameraPointing(lookAt.x, lookAt.y, lookAt.z);
+
+            return true;
+        });
+
+    auto lck = mrpt::lockHelper(guiThreadPendingTasksMtx_);
+    guiThreadPendingTasks_.emplace_back([=]() { (*task)(); });
+    guiThreadMustReLayoutTheseWindows_.insert(parentWindow);
+    return task->get_future();
+}
+
 #if 0
 // Visualize GT:
 if (1)