From 1e9a6bb2e4bf92e9b68b25cd6da6d59faff7821d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 29 Jan 2021 11:09:32 +0100
Subject: [PATCH] Added getPosition and getTarget for the visualizer camera.

---
 .../include/iDynTree/Visualizer.h             | 10 +++++++
 src/visualization/src/Camera.cpp              | 26 +++++++++++++++++++
 src/visualization/src/Camera.h                |  2 ++
 src/visualization/src/DummyImplementations.h  |  2 ++
 .../tests/VisualizerUnitTest.cpp              |  3 +++
 5 files changed, 43 insertions(+)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index 73c6252080c..816dbb21cf1 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -92,6 +92,16 @@ class ICamera
      */
     virtual void setTarget(const iDynTree::Position & cameraPos) = 0;
 
+    /**
+     * Get the linear position of the camera w.r.t to the world.
+     */
+    virtual iDynTree::Position getPosition() = 0;
+
+    /**
+     * Get the target of the camera (i.e. the point the camera is looking into) w.r.t. the world.
+     */
+    virtual iDynTree::Position getTarget() = 0;
+
     /**
      * Set the up vector of the camera w.r.t to the world.
      */
diff --git a/src/visualization/src/Camera.cpp b/src/visualization/src/Camera.cpp
index bf07a6da937..d6412761613 100644
--- a/src/visualization/src/Camera.cpp
+++ b/src/visualization/src/Camera.cpp
@@ -105,6 +105,32 @@ void Camera::setUpVector(const Direction& cameraUpVector)
     }
 }
 
+Position Camera::getPosition()
+{
+    if(m_irrCamera)
+    {
+        return irr2idyntree_pos(m_irrCamera->getPosition());
+    }
+    else
+    {
+        reportError("Camera","getPosition","Impossible to get the position of a null camera");
+        return iDynTree::Position();
+    }
+}
+
+Position Camera::getTarget()
+{
+    if(m_irrCamera)
+    {
+        return irr2idyntree_pos(m_irrCamera->getTarget());
+    }
+    else
+    {
+        reportError("Camera","getTarget","Impossible to get the target of a null camera");
+        return iDynTree::Position();
+    }
+}
+
 ICameraAnimator *Camera::animator()
 {
     if (!m_animator)
diff --git a/src/visualization/src/Camera.h b/src/visualization/src/Camera.h
index 1a6ac9399e6..ab68f1848c9 100644
--- a/src/visualization/src/Camera.h
+++ b/src/visualization/src/Camera.h
@@ -39,6 +39,8 @@ class Camera: public ICamera
     virtual void setPosition(const Position& cameraPos);
     virtual void setTarget(const Position& targetPos);
     virtual void setUpVector(const Direction& upVector);
+    virtual iDynTree::Position getPosition();
+    virtual iDynTree::Position getTarget();
     virtual ICameraAnimator* animator();
 };
 
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index a63feff14cd..8978bf8d910 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -28,6 +28,8 @@ class DummyCamera : public ICamera
     virtual void setPosition(const iDynTree::Position &) {};
     virtual void setTarget(const iDynTree::Position &) {};
     virtual void setUpVector(const Direction&) {};
+    virtual iDynTree::Position getPosition() {return iDynTree::Position();};
+    virtual iDynTree::Position getTarget() {return iDynTree::Position();};
     virtual ICameraAnimator* animator() {return nullptr;};
 };
 
diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp
index 8db8431768e..82dee9f5d4a 100644
--- a/src/visualization/tests/VisualizerUnitTest.cpp
+++ b/src/visualization/tests/VisualizerUnitTest.cpp
@@ -121,6 +121,9 @@ void checkFrameVisualization() {
     viz.init();
 
     viz.camera().setPosition(iDynTree::Position(2.0, 0.0, 2.0));
+    ASSERT_EQUAL_VECTOR(viz.camera().getPosition(), iDynTree::Position(2.0, 0.0, 2.0));
+    viz.camera().setTarget(iDynTree::Position(0.0, 0.0, 0.0));
+    ASSERT_EQUAL_VECTOR(viz.camera().getTarget(), iDynTree::Position(0.0, 0.0, 0.0));
 
     iDynTree::IFrameVisualization& frames = viz.frames();