From dbbc3d70a424b15d05bb11d9c2f58280ea96d3fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jul 2021 22:57:13 +0200 Subject: [PATCH 1/2] Moved ScreenToPlane and ScreenToScene from ign-gui to ign-rendering (#363) * Moved ScreenToPlane and ScreenToScene from ign-gui to ign-rendering Signed-off-by: ahcorde * added plane offset Signed-off-by: ahcorde * Added new signature Signed-off-by: ahcorde * Fix doxygen Signed-off-by: Steven Peters * Add const version of RayQueryResult bool operator Signed-off-by: Steven Peters * Improvements to Utils_TEST * Use tighter tolerances in expectations * Use IGN_PI/2 instead of 1.57 to tighten X tolerance * Clarify comments about max distance, which is only used when the ray does not intersect an object * Add expectations on RayQueryResult values * Add test of API without RayQueryResult Signed-off-by: Steven Peters * Test ScreenToPlane with non-zero offset Signed-off-by: Steven Peters Co-authored-by: Steven Peters --- include/ignition/rendering/RayQuery.hh | 6 + include/ignition/rendering/Utils.hh | 49 ++++++++ src/Utils.cc | 71 +++++++++++ src/Utils_TEST.cc | 167 +++++++++++++++++++++++++ 4 files changed, 293 insertions(+) create mode 100644 src/Utils_TEST.cc diff --git a/include/ignition/rendering/RayQuery.hh b/include/ignition/rendering/RayQuery.hh index b3130e46d..772177808 100644 --- a/include/ignition/rendering/RayQuery.hh +++ b/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() { diff --git a/include/ignition/rendering/Utils.hh b/include/ignition/rendering/Utils.hh index 79fefab45..d34d1b1b5 100644 --- a/include/ignition/rendering/Utils.hh +++ b/include/ignition/rendering/Utils.hh @@ -18,9 +18,14 @@ #define IGNITION_RENDERING_UTILS_HH_ #include +#include +#include +#include "ignition/rendering/Camera.hh" #include "ignition/rendering/config.hh" #include "ignition/rendering/Export.hh" +#include "ignition/rendering/RayQuery.hh" + namespace ignition { @@ -30,6 +35,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 diff --git a/src/Utils.cc b/src/Utils.cc index 20ed08285..094f48568 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -20,6 +20,12 @@ #include #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() { diff --git a/src/Utils_TEST.cc b/src/Utils_TEST.cc new file mode 100644 index 000000000..17f00314a --- /dev/null +++ b/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 + +#include + +#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 +{ + // 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(width / 2); + const int halfHeight = static_cast(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(); +} From 9341eb24f37a5c2b1ceb5c2dc07ab8473a2a0f9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 21 Jul 2021 00:06:39 +0200 Subject: [PATCH 2/2] Fixed screento functions names (#368) Signed-off-by: ahcorde --- include/ignition/rendering/Utils.hh | 6 +++--- src/Utils.cc | 8 ++++---- src/Utils_TEST.cc | 18 +++++++++--------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/include/ignition/rendering/Utils.hh b/include/ignition/rendering/Utils.hh index d34d1b1b5..7c1c15e35 100644 --- a/include/ignition/rendering/Utils.hh +++ b/include/ignition/rendering/Utils.hh @@ -43,7 +43,7 @@ namespace ignition /// \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( + math::Vector3d screenToScene( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, @@ -58,7 +58,7 @@ namespace ignition /// \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( + math::Vector3d screenToScene( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, @@ -73,7 +73,7 @@ namespace ignition /// \param[in] _offset Offset along the plane normal /// \return 3D coordinates of a point in the 3D scene. IGNITION_RENDERING_VISIBLE - math::Vector3d ScreenToPlane( + math::Vector3d screenToPlane( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, diff --git a/src/Utils.cc b/src/Utils.cc index 094f48568..8bd576ddd 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -35,7 +35,7 @@ namespace rendering inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { // ///////////////////////////////////////////////// -math::Vector3d ScreenToScene( +math::Vector3d screenToScene( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, @@ -63,18 +63,18 @@ math::Vector3d ScreenToScene( } ///////////////////////////////////////////////// -math::Vector3d ScreenToScene( +math::Vector3d screenToScene( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, float _maxDistance) { RayQueryResult rayResult; - return ScreenToScene(_screenPos, _camera, _rayQuery, rayResult, _maxDistance); + return screenToScene(_screenPos, _camera, _rayQuery, rayResult, _maxDistance); } ///////////////////////////////////////////////// -math::Vector3d ScreenToPlane( +math::Vector3d screenToPlane( const math::Vector2i &_screenPos, const CameraPtr &_camera, const RayQueryPtr &_rayQuery, diff --git a/src/Utils_TEST.cc b/src/Utils_TEST.cc index 17f00314a..b88785969 100644 --- a/src/Utils_TEST.cc +++ b/src/Utils_TEST.cc @@ -71,23 +71,23 @@ void UtilTest::ClickToScene(const std::string &_renderEngine) RayQueryPtr rayQuery = scene->CreateRayQuery(); EXPECT_TRUE(rayQuery != nullptr); - // ScreenToPlane - math::Vector3d result = ScreenToPlane(centerClick, camera, rayQuery); + // 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); + 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 + // screenToScene // API without RayQueryResult and default max distance - result = ScreenToScene(centerClick, camera, rayQuery); + result = screenToScene(centerClick, camera, rayQuery); // No objects currently in the scene, so return a point max distance in // front of camera @@ -98,7 +98,7 @@ void UtilTest::ClickToScene(const std::string &_renderEngine) // Try with different max distance RayQueryResult rayResult; - result = ScreenToScene(centerClick, camera, rayQuery, rayResult, 20.0); + 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); @@ -118,7 +118,7 @@ void UtilTest::ClickToScene(const std::string &_renderEngine) root->AddChild(box); // API without RayQueryResult and default max distance - result = ScreenToScene(centerClick, camera, rayQuery, rayResult); + result = screenToScene(centerClick, camera, rayQuery, rayResult); EXPECT_NEAR(0.5, result.Z(), 1e-10); EXPECT_NEAR(0.0, result.X(), 2e-6); @@ -127,7 +127,7 @@ void UtilTest::ClickToScene(const std::string &_renderEngine) EXPECT_NEAR(14.5 - camera->NearClipPlane(), rayResult.distance, 4e-6); EXPECT_EQ(box->Id(), rayResult.objectId); - result = ScreenToScene(centerClick, camera, rayQuery, rayResult, 20.0); + result = screenToScene(centerClick, camera, rayQuery, rayResult, 20.0); EXPECT_NEAR(0.5, result.Z(), 1e-10); EXPECT_NEAR(0.0, result.X(), 2e-6); @@ -140,7 +140,7 @@ void UtilTest::ClickToScene(const std::string &_renderEngine) camera->SetLocalPosition(0.0, 0.0, 7.0); camera->SetLocalRotation(0.0, IGN_PI / 2, 0.0); - result = ScreenToScene(centerClick, camera, rayQuery, rayResult); + result = screenToScene(centerClick, camera, rayQuery, rayResult); EXPECT_NEAR(0.5, result.Z(), 1e-10); EXPECT_NEAR(0.0, result.X(), 2e-6);