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(); +}