3 ➡️ 4 (#382)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

include/ignition/rendering/RayQuery.hh

@@ -45,6 +45,12 @@ namespace ignition
       /// \brief Intersected object id
       public: unsigned int objectId = 0;
 
+      /// \brief Returns false if result is not valid
+      public: operator bool() const
+              {
+                return distance > 0;
+              }
+
       /// \brief Returns false if result is not valid
       public: operator bool()
               {

include/ignition/rendering/Utils.hh

@@ -21,11 +21,15 @@
 
 #include <ignition/math/Helpers.hh>
 #include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/Vector2.hh>
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/Pose3.hh>
 
+#include "ignition/rendering/Camera.hh"
 #include "ignition/rendering/config.hh"
 #include "ignition/rendering/Export.hh"
+#include "ignition/rendering/RayQuery.hh"
+
 
 namespace ignition
 {
@@ -35,6 +39,50 @@ namespace ignition
     // Inline bracket to help doxygen filtering.
     inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
     //
+    /// \brief Retrieve the first point on a surface in the 3D scene hit by a
+    /// ray cast from the given 2D screen coordinates.
+    /// \param[in] _screenPos 2D coordinates on the screen, in pixels.
+    /// \param[in] _camera User camera
+    /// \param[in] _rayQuery Ray query for mouse clicks
+    /// \param[in] _maxDistance maximum distance to check the collision
+    /// \return 3D coordinates of a point in the 3D scene.
+    IGNITION_RENDERING_VISIBLE
+    math::Vector3d screenToScene(
+        const math::Vector2i &_screenPos,
+        const CameraPtr &_camera,
+        const RayQueryPtr &_rayQuery,
+        float maxDistance = 10.0);
+
+    /// \brief Retrieve the first point on a surface in the 3D scene hit by a
+    /// ray cast from the given 2D screen coordinates.
+    /// \param[in] _screenPos 2D coordinates on the screen, in pixels.
+    /// \param[in] _camera User camera
+    /// \param[in] _rayQuery Ray query for mouse clicks
+    /// \param[inout] _rayResult Ray query result
+    /// \param[in] _maxDistance maximum distance to check the collision
+    /// \return 3D coordinates of a point in the 3D scene.
+    IGNITION_RENDERING_VISIBLE
+    math::Vector3d screenToScene(
+        const math::Vector2i &_screenPos,
+        const CameraPtr &_camera,
+        const RayQueryPtr &_rayQuery,
+        RayQueryResult &_rayResult,
+        float maxDistance = 10.0);
+
+    /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a
+    /// ray cast from the given 2D screen coordinates.
+    /// \param[in] _screenPos 2D coordinates on the screen, in pixels.
+    /// \param[in] _camera User camera
+    /// \param[in] _rayQuery Ray query for mouse clicks
+    /// \param[in] _offset Offset along the plane normal
+    /// \return 3D coordinates of a point in the 3D scene.
+    IGNITION_RENDERING_VISIBLE
+    math::Vector3d screenToPlane(
+      const math::Vector2i &_screenPos,
+      const CameraPtr &_camera,
+      const RayQueryPtr &_rayQuery,
+      const float offset = 0.0);
+
     /// \brief Get the screen scaling factor.
     /// \return The screen scaling factor.
     IGNITION_RENDERING_VISIBLE

src/Utils.cc

@@ -20,6 +20,12 @@
 #include <X11/Xresource.h>
 #endif
 
+#include "ignition/math/Plane.hh"
+#include "ignition/math/Vector2.hh"
+#include "ignition/math/Vector3.hh"
+
+#include "ignition/rendering/Camera.hh"
+#include "ignition/rendering/RayQuery.hh"
 #include "ignition/rendering/Utils.hh"
 
 namespace ignition
@@ -28,6 +34,71 @@ namespace rendering
 {
 inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
 //
+/////////////////////////////////////////////////
+math::Vector3d screenToScene(
+    const math::Vector2i &_screenPos,
+    const CameraPtr &_camera,
+    const RayQueryPtr &_rayQuery,
+    RayQueryResult &_rayResult,
+    float _maxDistance)
+{
+  // Normalize point on the image
+  double width = _camera->ImageWidth();
+  double height = _camera->ImageHeight();
+
+  double nx = 2.0 * _screenPos.X() / width - 1.0;
+  double ny = 1.0 - 2.0 * _screenPos.Y() / height;
+
+  // Make a ray query
+  _rayQuery->SetFromCamera(
+      _camera, math::Vector2d(nx, ny));
+
+  _rayResult = _rayQuery->ClosestPoint();
+  if (_rayResult)
+    return _rayResult.point;
+
+  // Set point to be maxDistance m away if no intersection found
+  return _rayQuery->Origin() +
+      _rayQuery->Direction() * _maxDistance;
+}
+
+/////////////////////////////////////////////////
+math::Vector3d screenToScene(
+    const math::Vector2i &_screenPos,
+    const CameraPtr &_camera,
+    const RayQueryPtr &_rayQuery,
+    float _maxDistance)
+{
+  RayQueryResult rayResult;
+  return screenToScene(_screenPos, _camera, _rayQuery, rayResult, _maxDistance);
+}
+
+/////////////////////////////////////////////////
+math::Vector3d screenToPlane(
+    const math::Vector2i &_screenPos,
+    const CameraPtr &_camera,
+    const RayQueryPtr &_rayQuery,
+    const float offset)
+{
+  // Normalize point on the image
+  double width = _camera->ImageWidth();
+  double height = _camera->ImageHeight();
+
+  double nx = 2.0 * _screenPos.X() / width - 1.0;
+  double ny = 1.0 - 2.0 * _screenPos.Y() / height;
+
+  // Make a ray query
+  _rayQuery->SetFromCamera(
+      _camera, math::Vector2d(nx, ny));
+
+  ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), offset);
+
+  math::Vector3d origin = _rayQuery->Origin();
+  math::Vector3d direction = _rayQuery->Direction();
+  double distance = plane.Distance(origin, direction);
+  return origin + direction * distance;
+}
+
 /////////////////////////////////////////////////
 float screenScalingFactor()
 {

src/Utils_TEST.cc

@@ -0,0 +1,167 @@
+/* * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <gtest/gtest.h>
+
+#include <ignition/common/Console.hh>
+
+#include "ignition/rendering/Camera.hh"
+#include "ignition/rendering/RayQuery.hh"
+#include "ignition/rendering/RenderEngine.hh"
+#include "ignition/rendering/RenderingIface.hh"
+#include "ignition/rendering/Scene.hh"
+#include "ignition/rendering/Utils.hh"
+#include "ignition/rendering/Visual.hh"
+
+#include "test_config.h"  // NOLINT(build/include)
+
+using namespace ignition;
+using namespace rendering;
+
+class UtilTest : public testing::Test,
+                 public testing::WithParamInterface<const char *>
+{
+  // Documentation inherited
+  public: void SetUp() override
+  {
+    ignition::common::Console::SetVerbosity(4);
+  }
+
+  public: void ClickToScene(const std::string &_renderEngine);
+};
+
+void UtilTest::ClickToScene(const std::string &_renderEngine)
+{
+  RenderEngine *engine = rendering::engine(_renderEngine);
+  if (!engine)
+  {
+    igndbg << "Engine '" << _renderEngine
+           << "' is not supported" << std::endl;
+    return;
+  }
+  ScenePtr scene = engine->CreateScene("scene");
+
+  CameraPtr camera(scene->CreateCamera());
+  EXPECT_TRUE(camera != nullptr);
+
+  camera->SetLocalPosition(0.0, 0.0, 15);
+  camera->SetLocalRotation(0.0, IGN_PI / 2, 0.0);
+
+  unsigned int width = 640u;
+  unsigned int height = 480u;
+  camera->SetImageWidth(width);
+  camera->SetImageHeight(height);
+
+  const int halfWidth  = static_cast<int>(width / 2);
+  const int halfHeight = static_cast<int>(height / 2);
+  const ignition::math::Vector2i centerClick(halfWidth, halfHeight);
+
+  RayQueryPtr rayQuery = scene->CreateRayQuery();
+  EXPECT_TRUE(rayQuery != nullptr);
+
+  // screenToPlane
+  math::Vector3d result = screenToPlane(centerClick, camera, rayQuery);
+
+  EXPECT_NEAR(0.0, result.Z(), 1e-10);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+
+  // call with non-zero plane offset
+  result = screenToPlane(centerClick, camera, rayQuery, 5.0);
+
+  EXPECT_NEAR(5.0, result.Z(), 1e-10);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+
+  // screenToScene
+  // API without RayQueryResult and default max distance
+  result = screenToScene(centerClick, camera, rayQuery);
+
+  // No objects currently in the scene, so return a point max distance in
+  // front of camera
+  // The default max distance is 10 meters away
+  EXPECT_NEAR(5.0 - camera->NearClipPlane(), result.Z(), 4e-6);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+
+  // Try with different max distance
+  RayQueryResult rayResult;
+  result = screenToScene(centerClick, camera, rayQuery, rayResult, 20.0);
+
+  EXPECT_NEAR(-5.0 - camera->NearClipPlane(), result.Z(), 4e-6);
+  EXPECT_NEAR(0.0, result.X(), 4e-6);
+  EXPECT_NEAR(0.0, result.Y(), 4e-6);
+  EXPECT_FALSE(rayResult);
+  EXPECT_EQ(0u, rayResult.objectId);
+
+  VisualPtr root = scene->RootVisual();
+
+  // create box visual to collide with the ray
+  VisualPtr box = scene->CreateVisual();
+  box->AddGeometry(scene->CreateBox());
+  box->SetOrigin(0.0, 0.0, 0.0);
+  box->SetLocalPosition(0.0, 0.0, 0.0);
+  box->SetLocalRotation(0.0, 0.0, 0.0);
+  box->SetLocalScale(1.0, 1.0, 1.0);
+  root->AddChild(box);
+
+  // API without RayQueryResult and default max distance
+  result = screenToScene(centerClick, camera, rayQuery, rayResult);
+
+  EXPECT_NEAR(0.5, result.Z(), 1e-10);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+  EXPECT_TRUE(rayResult);
+  EXPECT_NEAR(14.5 - camera->NearClipPlane(), rayResult.distance, 4e-6);
+  EXPECT_EQ(box->Id(), rayResult.objectId);
+
+  result = screenToScene(centerClick, camera, rayQuery, rayResult, 20.0);
+
+  EXPECT_NEAR(0.5, result.Z(), 1e-10);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+  EXPECT_TRUE(rayResult);
+  EXPECT_NEAR(14.5 - camera->NearClipPlane(), rayResult.distance, 4e-6);
+  EXPECT_EQ(box->Id(), rayResult.objectId);
+
+  // Move camera closer to box
+  camera->SetLocalPosition(0.0, 0.0, 7.0);
+  camera->SetLocalRotation(0.0, IGN_PI / 2, 0.0);
+
+  result = screenToScene(centerClick, camera, rayQuery, rayResult);
+
+  EXPECT_NEAR(0.5, result.Z(), 1e-10);
+  EXPECT_NEAR(0.0, result.X(), 2e-6);
+  EXPECT_NEAR(0.0, result.Y(), 2e-6);
+  EXPECT_TRUE(rayResult);
+  EXPECT_NEAR(6.5 - camera->NearClipPlane(), rayResult.distance, 4e-6);
+  EXPECT_EQ(box->Id(), rayResult.objectId);
+}
+
+/////////////////////////////////////////////////
+TEST_P(UtilTest, ClickToScene)
+{
+  ClickToScene(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(ClickToScene, UtilTest,
+    RENDER_ENGINE_VALUES,
+    ignition::rendering::PrintToStringParam());
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}