From 76d93864eb982ed6e09b9abf2a44b2a76adcbb65 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Fri, 10 Jul 2020 11:19:57 +0300
Subject: [PATCH] Working virtual camera pose input

---
 applications/gui2/src/modules/camera.cpp | 8 ++++++++
 applications/gui2/src/modules/camera.hpp | 2 ++
 applications/gui2/src/views/camera3d.cpp | 8 ++++++--
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 352d2c67b..24eec071b 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -140,3 +140,11 @@ bool Camera::hasFrame() {
 	}
 	return false;
 }
+
+void Camera::sendPose(const Eigen::Matrix4d &pose) {
+	if (auto ptr = std::atomic_load(&current_fs_)) {
+		auto response = ptr->frames[frame_idx].response();
+		auto &rgbdresponse = response.cast<ftl::rgbd::Frame>();
+		rgbdresponse.setPose() = pose;
+	}
+}
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index 3ecdf949b..018ee20c4 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -30,6 +30,8 @@ public:
 	/** Check if new frame is available */
 	bool hasFrame();
 
+	void sendPose(const Eigen::Matrix4d &pose);
+
 	ftl::render::Colouriser* colouriser() { return colouriser_.get(); };
 	ftl::overlay::Overlay* overlay() { return overlay_.get(); }
 
diff --git a/applications/gui2/src/views/camera3d.cpp b/applications/gui2/src/views/camera3d.cpp
index 885e38641..37b1fa8f1 100644
--- a/applications/gui2/src/views/camera3d.cpp
+++ b/applications/gui2/src/views/camera3d.cpp
@@ -1,4 +1,5 @@
 #include "camera3d.hpp"
+#include "../modules/camera.hpp"
 
 using ftl::gui2::CameraView3D;
 
@@ -55,7 +56,8 @@ bool CameraView3D::keyboardEvent(int key, int scancode, int action, int modifier
 		//transform_ix_ = ix-1;
 	}
 
-	LOG(INFO) << "New pose: \n" << getUpdatedPose();
+	//LOG(INFO) << "New pose: \n" << getUpdatedPose();
+	//ctrl_->sendPose(getUpdatedPose());
 	return true;
 }
 
@@ -72,7 +74,8 @@ bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vecto
 	rx_ += rel[0];
 	ry_ += rel[1];
 
-	LOG(INFO) << "New pose: \n" << getUpdatedPose();
+	//LOG(INFO) << "New pose: \n" << getUpdatedPose();
+	//ctrl_->sendPose(getUpdatedPose());
 	return true;
 }
 
@@ -107,5 +110,6 @@ Eigen::Matrix4d CameraView3D::getUpdatedPose() {
 
 void CameraView3D::draw(NVGcontext* ctx) {
 	imview_->fit(); // TODO: should be moved to ::performLayout(ctx)?
+	ctrl_->sendPose(getUpdatedPose());
 	CameraView::draw(ctx);
 }
-- 
GitLab