diff --git a/examples/tutorials/colabs/replay_tutorial.ipynb b/examples/tutorials/colabs/replay_tutorial.ipynb index 5431fe77cc..bb873f9c78 100644 --- a/examples/tutorials/colabs/replay_tutorial.ipynb +++ b/examples/tutorials/colabs/replay_tutorial.ipynb @@ -49,6 +49,7 @@ "import numpy as np\n", "\n", "import habitat_sim\n", + "from habitat_sim.gfx import LightInfo, LightPositionModel\n", "from habitat_sim.utils import gfx_replay_utils\n", "from habitat_sim.utils import viz_utils as vut\n", "\n", @@ -116,7 +117,9 @@ { "cell_type": "code", "execution_count": null, - "metadata": {}, + "metadata": { + "lines_to_next_cell": 2 + }, "outputs": [], "source": [ "\n", @@ -180,6 +183,28 @@ "metadata": {}, "outputs": [], "source": [ + "\n", + "\n", + "def configure_lighting(sim):\n", + " light_setup = [\n", + " LightInfo(\n", + " vector=[1.0, 1.0, 0.0, 1.0],\n", + " color=[18.0, 18.0, 18.0],\n", + " model=LightPositionModel.Global,\n", + " ),\n", + " LightInfo(\n", + " vector=[0.0, -1.0, 0.0, 1.0],\n", + " color=[5.0, 5.0, 5.0],\n", + " model=LightPositionModel.Global,\n", + " ),\n", + " LightInfo(\n", + " vector=[-1.0, 1.0, 1.0, 1.0],\n", + " color=[18.0, 18.0, 18.0],\n", + " model=LightPositionModel.Global,\n", + " ),\n", + " ]\n", + " sim.set_light_setup(light_setup)\n", + "\n", "\n", "if __name__ == \"__main__\":\n", " import argparse\n", @@ -207,6 +232,8 @@ "else:\n", " sim.reconfigure(cfg)\n", "\n", + "configure_lighting(sim)\n", + "\n", "agent_state = habitat_sim.AgentState()\n", "agent = sim.initialize_agent(0, agent_state)" ] @@ -403,6 +430,8 @@ "metadata": {}, "outputs": [], "source": [ + "\n", + "sim.close()\n", "\n", "# use same agents/sensors from earlier, with different backend config\n", "playback_cfg = habitat_sim.Configuration(\n", @@ -412,13 +441,13 @@ " cfg.agents,\n", ")\n", "\n", - "sim.close()\n", - "\n", "if not sim:\n", " sim = habitat_sim.Simulator(playback_cfg)\n", "else:\n", " sim.reconfigure(playback_cfg)\n", "\n", + "configure_lighting(sim)\n", + "\n", "agent_state = habitat_sim.AgentState()\n", "sim.initialize_agent(0, agent_state)\n", "\n", @@ -563,32 +592,59 @@ "sensor_node.translation = [-1.1, -0.9, -0.2]\n", "sensor_node.rotation = mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3(0.0, 1.0, 0))\n", "\n", - "prim_attr_mgr = sim.get_asset_template_manager()\n", - "# get the rigid object manager, which provides direct\n", - "# access to objects\n", - "rigid_obj_mgr = sim.get_rigid_object_manager()\n", - "# visualize the recorded agent transform as a cylinder\n", - "agent_viz_handle = prim_attr_mgr.get_template_handles(\"cylinderSolid\")[0]\n", - "agent_viz_obj = rigid_obj_mgr.add_object_by_template_handle(agent_viz_handle)\n", - "agent_viz_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC\n", - "agent_viz_obj.collidable = False\n", - "\n", - "# visualize the recorded sensor transform as a cube\n", - "sensor_viz_handle = prim_attr_mgr.get_template_handles(\"cubeSolid\")[0]\n", - "sensor_viz_obj = rigid_obj_mgr.add_object_by_template_handle(sensor_viz_handle)\n", - "sensor_viz_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC\n", - "sensor_viz_obj.collidable = False\n", + "# gather the agent trajectory for later visualization\n", + "agent_trajectory_points = []\n", + "for frame in range(player.get_num_keyframes()):\n", + " player.set_keyframe_index(frame)\n", + " (agent_translation, _) = player.get_user_transform(\"agent\")\n", + " agent_trajectory_points.append(agent_translation)\n", + "\n", + "debug_line_render = sim.get_debug_line_render()\n", + "debug_line_render.set_line_width(2.0)\n", + "agent_viz_box = mn.Range3D(mn.Vector3(-0.1, 0.0, -0.1), mn.Vector3(0.1, 0.4, 0.1))\n", + "sensor_viz_box = mn.Range3D(mn.Vector3(-0.1, -0.1, -0.1), mn.Vector3(0.1, 0.1, 0.1))\n", "\n", "for frame in range(player.get_num_keyframes()):\n", " player.set_keyframe_index(frame)\n", "\n", " (agent_translation, agent_rotation) = player.get_user_transform(\"agent\")\n", - " agent_viz_obj.translation = agent_translation\n", - " agent_viz_obj.rotation = agent_rotation\n", "\n", + " rot_mat = agent_rotation.to_matrix()\n", + " full_mat = mn.Matrix4.from_(rot_mat, agent_translation)\n", + "\n", + " # draw a box in the agent body's local space\n", + " debug_line_render.push_transform(full_mat)\n", + " debug_line_render.draw_box(\n", + " agent_viz_box.min, agent_viz_box.max, mn.Color4(1.0, 0.0, 0.0, 1.0)\n", + " )\n", + " debug_line_render.pop_transform()\n", + "\n", + " for (radius, opacity) in [(0.2, 0.6), (0.25, 0.4), (0.3, 0.2)]:\n", + " debug_line_render.draw_circle(\n", + " agent_translation, radius, mn.Color4(0.0, 1.0, 1.0, opacity)\n", + " )\n", + "\n", + " # draw a box in the sensor's local space\n", " (sensor_translation, sensor_rotation) = player.get_user_transform(\"sensor\")\n", - " sensor_viz_obj.translation = sensor_translation\n", - " sensor_viz_obj.rotation = sensor_rotation\n", + " debug_line_render.push_transform(\n", + " mn.Matrix4.from_(sensor_rotation.to_matrix(), sensor_translation)\n", + " )\n", + " debug_line_render.draw_box(\n", + " sensor_viz_box.min, sensor_viz_box.max, mn.Color4(1.0, 0.0, 0.0, 1.0)\n", + " )\n", + " # draw a line in the sensor look direction (-z in local space)\n", + " debug_line_render.draw_transformed_line(\n", + " mn.Vector3.zero_init(),\n", + " mn.Vector3(0.0, 0.0, -0.5),\n", + " mn.Color4(1.0, 0.0, 0.0, 1.0),\n", + " mn.Color4(1.0, 1.0, 1.0, 1.0),\n", + " )\n", + " debug_line_render.pop_transform()\n", + "\n", + " # draw the agent trajectory\n", + " debug_line_render.draw_path_with_endpoint_circles(\n", + " agent_trajectory_points, 0.07, mn.Color4(1.0, 1.0, 1.0, 1.0)\n", + " )\n", "\n", " observations.append(sim.get_sensor_observations())\n", "\n", @@ -601,8 +657,6 @@ " open_vid=show_video,\n", " )\n", "\n", - "rigid_obj_mgr.remove_object_by_id(agent_viz_obj.object_id)\n", - "rigid_obj_mgr.remove_object_by_id(sensor_viz_obj.object_id)\n", "\n", "# clean up the player\n", "player.close()" diff --git a/examples/tutorials/nb_python/replay_tutorial.py b/examples/tutorials/nb_python/replay_tutorial.py index b198a79fc4..f2e28c664d 100644 --- a/examples/tutorials/nb_python/replay_tutorial.py +++ b/examples/tutorials/nb_python/replay_tutorial.py @@ -50,6 +50,7 @@ import numpy as np import habitat_sim +from habitat_sim.gfx import LightInfo, LightPositionModel from habitat_sim.utils import gfx_replay_utils from habitat_sim.utils import viz_utils as vut @@ -144,6 +145,28 @@ def simulate_with_moving_agent( # ## More tutorial setup # %% + +def configure_lighting(sim): + light_setup = [ + LightInfo( + vector=[1.0, 1.0, 0.0, 1.0], + color=[18.0, 18.0, 18.0], + model=LightPositionModel.Global, + ), + LightInfo( + vector=[0.0, -1.0, 0.0, 1.0], + color=[5.0, 5.0, 5.0], + model=LightPositionModel.Global, + ), + LightInfo( + vector=[-1.0, 1.0, 1.0, 1.0], + color=[18.0, 18.0, 18.0], + model=LightPositionModel.Global, + ), + ] + sim.set_light_setup(light_setup) + + if __name__ == "__main__": import argparse @@ -170,6 +193,8 @@ def simulate_with_moving_agent( else: sim.reconfigure(cfg) +configure_lighting(sim) + agent_state = habitat_sim.AgentState() agent = sim.initialize_agent(0, agent_state) @@ -276,6 +301,8 @@ def simulate_with_moving_agent( # Note call to gfx_replay_utils.make_backend_configuration_for_playback. Note that we don't specify a scene or stage when reconfiguring for replay playback. need_separate_semantic_scene_graph is generally set to False. If you're using a semantic sensor and replaying a scene that uses a separate semantic mesh (like an MP3D scene), set this to True. If in doubt, be aware there's a Habitat runtime warning that will always catch incorrect usage of this flag. # %% +sim.close() + # use same agents/sensors from earlier, with different backend config playback_cfg = habitat_sim.Configuration( gfx_replay_utils.make_backend_configuration_for_playback( @@ -284,13 +311,13 @@ def simulate_with_moving_agent( cfg.agents, ) -sim.close() - if not sim: sim = habitat_sim.Simulator(playback_cfg) else: sim.reconfigure(playback_cfg) +configure_lighting(sim) + agent_state = habitat_sim.AgentState() sim.initialize_agent(0, agent_state) @@ -370,32 +397,59 @@ def simulate_with_moving_agent( sensor_node.translation = [-1.1, -0.9, -0.2] sensor_node.rotation = mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3(0.0, 1.0, 0)) -prim_attr_mgr = sim.get_asset_template_manager() -# get the rigid object manager, which provides direct -# access to objects -rigid_obj_mgr = sim.get_rigid_object_manager() -# visualize the recorded agent transform as a cylinder -agent_viz_handle = prim_attr_mgr.get_template_handles("cylinderSolid")[0] -agent_viz_obj = rigid_obj_mgr.add_object_by_template_handle(agent_viz_handle) -agent_viz_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC -agent_viz_obj.collidable = False - -# visualize the recorded sensor transform as a cube -sensor_viz_handle = prim_attr_mgr.get_template_handles("cubeSolid")[0] -sensor_viz_obj = rigid_obj_mgr.add_object_by_template_handle(sensor_viz_handle) -sensor_viz_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC -sensor_viz_obj.collidable = False +# gather the agent trajectory for later visualization +agent_trajectory_points = [] +for frame in range(player.get_num_keyframes()): + player.set_keyframe_index(frame) + (agent_translation, _) = player.get_user_transform("agent") + agent_trajectory_points.append(agent_translation) + +debug_line_render = sim.get_debug_line_render() +debug_line_render.set_line_width(2.0) +agent_viz_box = mn.Range3D(mn.Vector3(-0.1, 0.0, -0.1), mn.Vector3(0.1, 0.4, 0.1)) +sensor_viz_box = mn.Range3D(mn.Vector3(-0.1, -0.1, -0.1), mn.Vector3(0.1, 0.1, 0.1)) for frame in range(player.get_num_keyframes()): player.set_keyframe_index(frame) (agent_translation, agent_rotation) = player.get_user_transform("agent") - agent_viz_obj.translation = agent_translation - agent_viz_obj.rotation = agent_rotation + rot_mat = agent_rotation.to_matrix() + full_mat = mn.Matrix4.from_(rot_mat, agent_translation) + + # draw a box in the agent body's local space + debug_line_render.push_transform(full_mat) + debug_line_render.draw_box( + agent_viz_box.min, agent_viz_box.max, mn.Color4(1.0, 0.0, 0.0, 1.0) + ) + debug_line_render.pop_transform() + + for (radius, opacity) in [(0.2, 0.6), (0.25, 0.4), (0.3, 0.2)]: + debug_line_render.draw_circle( + agent_translation, radius, mn.Color4(0.0, 1.0, 1.0, opacity) + ) + + # draw a box in the sensor's local space (sensor_translation, sensor_rotation) = player.get_user_transform("sensor") - sensor_viz_obj.translation = sensor_translation - sensor_viz_obj.rotation = sensor_rotation + debug_line_render.push_transform( + mn.Matrix4.from_(sensor_rotation.to_matrix(), sensor_translation) + ) + debug_line_render.draw_box( + sensor_viz_box.min, sensor_viz_box.max, mn.Color4(1.0, 0.0, 0.0, 1.0) + ) + # draw a line in the sensor look direction (-z in local space) + debug_line_render.draw_transformed_line( + mn.Vector3.zero_init(), + mn.Vector3(0.0, 0.0, -0.5), + mn.Color4(1.0, 0.0, 0.0, 1.0), + mn.Color4(1.0, 1.0, 1.0, 1.0), + ) + debug_line_render.pop_transform() + + # draw the agent trajectory + debug_line_render.draw_path_with_endpoint_circles( + agent_trajectory_points, 0.07, mn.Color4(1.0, 1.0, 1.0, 1.0) + ) observations.append(sim.get_sensor_observations()) @@ -408,8 +462,6 @@ def simulate_with_moving_agent( open_vid=show_video, ) -rigid_obj_mgr.remove_object_by_id(agent_viz_obj.object_id) -rigid_obj_mgr.remove_object_by_id(sensor_viz_obj.object_id) # clean up the player player.close() diff --git a/src/esp/bindings/GfxBindings.cpp b/src/esp/bindings/GfxBindings.cpp index 3d7b546427..f3ce59623c 100644 --- a/src/esp/bindings/GfxBindings.cpp +++ b/src/esp/bindings/GfxBindings.cpp @@ -14,6 +14,7 @@ #include "python/corrade/EnumOperators.h" #include "esp/assets/ResourceManager.h" +#include "esp/gfx/DebugLineRender.h" #include "esp/gfx/LightSetup.h" #include "esp/gfx/RenderCamera.h" #include "esp/gfx/RenderTarget.h" @@ -205,6 +206,46 @@ void initGfxBindings(py::module& m) { .def(py::self == py::self) .def(py::self != py::self); + py::class_>( + m, "DebugLineRender") + .def( + "set_line_width", &DebugLineRender::setLineWidth, + R"(Set global line width for all lines rendered by DebugLineRender.)") + .def( + "push_transform", &DebugLineRender::pushTransform, + R"(Push (multiply) a transform onto the transform stack, affecting all line-drawing until popped. Must be paired with popTransform().)") + .def("pop_transform", &DebugLineRender::popTransform, + R"(See push_transform.)") + .def("draw_box", &DebugLineRender::drawBox, + R"(Draw a box in world-space or local-space (see pushTransform).)") + .def( + "draw_circle", &DebugLineRender::drawCircle, "translation"_a, + "radius"_a, "color"_a, "num_segments"_a = 24, + "normal"_a = Magnum::Vector3{0.0, 1.0, 0.0}, + R"(Draw a circle in world-space or local-space (see pushTransform). The circle is an approximation; see numSegments.)") + .def( + "draw_transformed_line", + py::overload_cast( + &DebugLineRender::drawTransformedLine), + "from"_a, "to"_a, "from_color"_a, "to_color"_a, + R"(Draw a line segment in world-space or local-space (see pushTransform) with interpolated color.)") + .def( + "draw_transformed_line", + py::overload_cast( + &DebugLineRender::drawTransformedLine), + "from"_a, "to"_a, "color"_a, + R"(Draw a line segment in world-space or local-space (see pushTransform).)") + .def( + "draw_path_with_endpoint_circles", + py::overload_cast&, float, + const Magnum::Color4&, int, const Magnum::Vector3&>( + &DebugLineRender::drawPathWithEndpointCircles), + "points"_a, "radius"_a, "color"_a, "num_segments"_a = 24, + "normal"_a = Magnum::Vector3{0.0, 1.0, 0.0}, + R"(Draw a sequence of line segments with circles at the two endpoints. In world-space or local-space (see pushTransform).)"); + m.attr("DEFAULT_LIGHTING_KEY") = DEFAULT_LIGHTING_KEY; m.attr("NO_LIGHT_KEY") = NO_LIGHT_KEY; } diff --git a/src/esp/bindings/SimBindings.cpp b/src/esp/bindings/SimBindings.cpp index 37a301269e..31ea7aa7c7 100644 --- a/src/esp/bindings/SimBindings.cpp +++ b/src/esp/bindings/SimBindings.cpp @@ -453,7 +453,10 @@ void initSimBindings(py::module& m) { &Simulator::getRigidConstraintSettings, "constraint_id"_a, R"(Get a copy of the settings for an existing rigid constraint.)") .def("remove_rigid_constraint", &Simulator::removeRigidConstraint, - "constraint_id"_a, R"(Remove a rigid constraint by id.)"); + "constraint_id"_a, R"(Remove a rigid constraint by id.)") + .def("get_debug_line_render", &Simulator::getDebugLineRender, + pybind11::return_value_policy::reference, + R"(Get visualization helper for rendering lines.)"); } } // namespace sim diff --git a/src/esp/gfx/CMakeLists.txt b/src/esp/gfx/CMakeLists.txt index 67bfbcee91..0d1163ecb2 100644 --- a/src/esp/gfx/CMakeLists.txt +++ b/src/esp/gfx/CMakeLists.txt @@ -24,6 +24,8 @@ set( CubeMapCamera.h CubeMap.cpp CubeMap.h + DebugLineRender.cpp + DebugLineRender.h Renderer.cpp Renderer.h replay/Keyframe.h diff --git a/src/esp/gfx/DebugLineRender.cpp b/src/esp/gfx/DebugLineRender.cpp new file mode 100644 index 0000000000..8925f09376 --- /dev/null +++ b/src/esp/gfx/DebugLineRender.cpp @@ -0,0 +1,309 @@ +// Copyright (c) Facebook, Inc. and its affiliates. +// This source code is licensed under the MIT license found in the +// LICENSE file in the root directory of this source tree. + +#include "DebugLineRender.h" + +#include +#include +#include + +#include "esp/core/Check.h" +#include "esp/core/logging.h" + +namespace Cr = Corrade; +namespace Mn = Magnum; + +namespace esp { +namespace gfx { + +namespace { + +Mn::Color4 remapAlpha(const Mn::Color4& src) { + // Because we render lines multiple times additively in flushLines, we need to + // remap alpha (opacity). This is an approximation. + constexpr float exponent = 2.f; + return {src.rgb(), std::pow(src.a(), exponent)}; +} + +// return false if segment is entirely clipped +bool scissorSegmentToOutsideCircle(Mn::Vector3* pt0, + Mn::Vector3* pt1, + const Mn::Vector3& center, + float radius) { + float dist0 = (*pt0 - center).length(); + float dist1 = (*pt1 - center).length(); + if (dist0 < radius) { + if (dist1 < radius) { + return false; + } + const float lerpFraction = Mn::Math::lerpInverted(dist0, dist1, radius); + CORRADE_INTERNAL_ASSERT(lerpFraction >= 0.f && lerpFraction <= 1.f); + Mn::Vector3 clippedPt = Mn::Math::lerp(*pt0, *pt1, lerpFraction); + *pt0 = clippedPt; + return true; + } + + if (dist1 < radius) { + const float lerpFraction = Mn::Math::lerpInverted(dist1, dist0, radius); + CORRADE_INTERNAL_ASSERT(lerpFraction >= 0.f && lerpFraction <= 1.f); + Mn::Vector3 clippedPt = Mn::Math::lerp(*pt1, *pt0, lerpFraction); + *pt1 = clippedPt; + return true; + } + + return true; +} + +} // namespace + +DebugLineRender::DebugLineRender() + : _glResources{std::make_unique()} { + _glResources->mesh.addVertexBuffer(_glResources->buffer, 0, + Mn::Shaders::FlatGL3D::Position{}, + Mn::Shaders::FlatGL3D::Color4{}); +} + +void DebugLineRender::releaseGLResources() { + _glResources = nullptr; +} + +void DebugLineRender::drawLine(const Mn::Vector3& from, + const Mn::Vector3& to, + const Mn::Color4& fromColor, + const Mn::Color4& toColor) { + VertexRecord v1{from, remapAlpha(fromColor)}; + VertexRecord v2{to, remapAlpha(toColor)}; + arrayAppend(_verts, {v1, v2}); +} + +void DebugLineRender::setLineWidth(float lineWidth) { + // This is derived from experiments with glLineWidth on Nvidia hardware. + const float maxLineWidth = 20.f; + if (lineWidth > maxLineWidth) { + LOG(WARNING) << "DebugLineRender::setLineWidth: requested lineWidth of " + << lineWidth + << " is greater than " + "max supported width of " + << maxLineWidth; + lineWidth = maxLineWidth; + } + _internalLineWidth = lineWidth / 2; // see also DebugLineRender::flushLines +} + +void DebugLineRender::flushLines(const Magnum::Matrix4& camMatrix, + const Magnum::Matrix4& projMatrix, + const Magnum::Vector2i& viewport) { + CORRADE_ASSERT(_glResources, + "DebugLineRender::flushLines: no GL resources; see " + "also releaseGLResources", ); + + bool doToggleBlend = !glIsEnabled(GL_BLEND); + + if (doToggleBlend) { + Mn::GL::Renderer::enable(Mn::GL::Renderer::Feature::Blending); + Mn::GL::Renderer::setBlendFunction( + Mn::GL::Renderer::BlendFunction::SourceAlpha, + Mn::GL::Renderer::BlendFunction::OneMinusSourceAlpha); + Mn::GL::Renderer::setBlendEquation(Mn::GL::Renderer::BlendEquation::Add); + } + + // Update buffer with new data + _glResources->buffer.setData(_verts, Mn::GL::BufferUsage::DynamicDraw); + + // Update shader + _glResources->mesh.setCount(_verts.size()); + + Mn::GL::Renderer::setLineWidth(_internalLineWidth); + + // We draw lines multiple times, with pixel offsets, to produce a single + // thick, visually-appealing line. Todo: implement thick lines using + // triangles, for example https://mattdesl.svbtle.com/drawing-lines-is-hard. + // 1.2 is hand-tuned for Nvidia hardware to be just small enough so we don't + // see gaps between the individual offset lines. + const float x = _internalLineWidth * 1.2f; + constexpr float sqrtOfTwo = Mn::Constants::sqrt2(); + // hard-coding 8 points around a circle + const Mn::Vector3 offsets[] = {Mn::Vector3(x, x, 0), + Mn::Vector3(-x, x, 0), + Mn::Vector3(x, -x, 0), + Mn::Vector3(-x, -x, 0), + Mn::Vector3(x * sqrtOfTwo, 0, 0), + Mn::Vector3(-x * sqrtOfTwo, 0, 0), + Mn::Vector3(0, x * sqrtOfTwo, 0), + Mn::Vector3(0, -x * sqrtOfTwo, 0)}; + + Mn::Matrix4 projCam = projMatrix * camMatrix; + + auto submitLinesWithOffsets = [&]() { + for (const auto& offset : offsets) { + // We want to offset in viewport (pixel) space, but we need to specify our + // offset transform in projection space. So divide by viewport dim. + Magnum::Matrix4 offset0Matrix = Mn::Matrix4::translation( + offset * Mn::Vector3(1.f / viewport.x(), 1.f / viewport.y(), 0.f)); + Magnum::Matrix4 transProj = offset0Matrix * projCam; + _glResources->shader.setTransformationProjectionMatrix(transProj); + _glResources->shader.draw(_glResources->mesh); + } + }; + + _glResources->shader.setColor({1.0f, 1.0f, 1.0f, 1.0}); + + submitLinesWithOffsets(); + + // modify all colors to be semi-transparent + static float opacity = 0.1; + _glResources->shader.setColor({1.0f, 1.0f, 1.0f, opacity}); + + // Here, we re-draw lines with a reversed depth function. This causes + // occluded lines to be visualized as semi-transparent, which is useful for + // UX and visually-appealing. + Mn::GL::Renderer::setDepthFunction(Mn::GL::Renderer::DepthFunction::Greater); + submitLinesWithOffsets(); + + // restore to a reasonable default + Mn::GL::Renderer::setDepthFunction(Mn::GL::Renderer::DepthFunction::Less); + + // Clear _verts to receive new data + arrayResize(_verts, 0); + + // restore blending state if necessary + if (doToggleBlend) { + Mn::GL::Renderer::disable(Mn::GL::Renderer::Feature::Blending); + // Note: because we are disabling blending, we don't need to restore + // BlendFunction or BlendEquation + } + + // restore to a reasonable default + Mn::GL::Renderer::setLineWidth(1.0); +} + +void DebugLineRender::pushTransform(const Magnum::Matrix4& transform) { + _inputTransformStack.push_back(transform); + updateCachedInputTransform(); +} + +void DebugLineRender::popTransform() { + CORRADE_INTERNAL_ASSERT(!_inputTransformStack.empty()); + _inputTransformStack.pop_back(); + updateCachedInputTransform(); +} + +void DebugLineRender::drawTransformedLine(const Magnum::Vector3& from, + const Magnum::Vector3& to, + const Magnum::Color4& fromColor, + const Magnum::Color4& toColor) { + Mn::Vector3 fromTransformed = _cachedInputTransform.transformPoint(from); + Mn::Vector3 toTransformed = _cachedInputTransform.transformPoint(to); + drawLine(fromTransformed, toTransformed, fromColor, toColor); +} + +void DebugLineRender::drawBox(const Magnum::Vector3& min, + const Magnum::Vector3& max, + const Magnum::Color4& color) { + // 4 lines along x axis + drawTransformedLine(Mn::Vector3(min.x(), min.y(), min.z()), + Mn::Vector3(max.x(), min.y(), min.z()), color); + drawTransformedLine(Mn::Vector3(min.x(), min.y(), max.z()), + Mn::Vector3(max.x(), min.y(), max.z()), color); + drawTransformedLine(Mn::Vector3(min.x(), max.y(), min.z()), + Mn::Vector3(max.x(), max.y(), min.z()), color); + drawTransformedLine(Mn::Vector3(min.x(), max.y(), max.z()), + Mn::Vector3(max.x(), max.y(), max.z()), color); + + // 4 lines along y axis + drawTransformedLine(Mn::Vector3(min.x(), min.y(), min.z()), + Mn::Vector3(min.x(), max.y(), min.z()), color); + drawTransformedLine(Mn::Vector3(max.x(), min.y(), min.z()), + Mn::Vector3(max.x(), max.y(), min.z()), color); + drawTransformedLine(Mn::Vector3(min.x(), min.y(), max.z()), + Mn::Vector3(min.x(), max.y(), max.z()), color); + drawTransformedLine(Mn::Vector3(max.x(), min.y(), max.z()), + Mn::Vector3(max.x(), max.y(), max.z()), color); + + // 4 lines along z axis + drawTransformedLine(Mn::Vector3(min.x(), min.y(), min.z()), + Mn::Vector3(min.x(), min.y(), max.z()), color); + drawTransformedLine(Mn::Vector3(max.x(), min.y(), min.z()), + Mn::Vector3(max.x(), min.y(), max.z()), color); + drawTransformedLine(Mn::Vector3(min.x(), max.y(), min.z()), + Mn::Vector3(min.x(), max.y(), max.z()), color); + drawTransformedLine(Mn::Vector3(max.x(), max.y(), min.z()), + Mn::Vector3(max.x(), max.y(), max.z()), color); +} + +void DebugLineRender::drawCircle(const Magnum::Vector3& pos, + float radius, + const Magnum::Color4& color, + int numSegments, + const Magnum::Vector3& normal) { + // https://stackoverflow.com/questions/11132681/what-is-a-formula-to-get-a-vector-perpendicular-to-another-vector + auto randomPerpVec = normal.z() < normal.x() + ? Mn::Vector3(normal.y(), -normal.x(), 0) + : Mn::Vector3(0, -normal.z(), normal.y()); + + pushTransform(Mn::Matrix4::lookAt(pos, pos + normal, randomPerpVec) * + Mn::Matrix4::scaling(Mn::Vector3(radius, radius, 0.f))); + + Mn::Vector3 prevPt; + for (int seg = 0; seg <= numSegments; seg++) { + Mn::Deg angle = Mn::Deg(360.f * float(seg) / numSegments); + Mn::Vector3 pt(Mn::Math::cos(angle), Mn::Math::sin(angle), 0.f); + if (seg > 0) { + drawTransformedLine(prevPt, pt, color); + } + prevPt = pt; + } + + popTransform(); +} + +void DebugLineRender::drawPathWithEndpointCircles( + Mn::Containers::ArrayView points, + float radius, + const Magnum::Color4& color, + int numSegments, + const Magnum::Vector3& normal) { + ESP_CHECK(points.size() >= 2, + "drawPathWithEndpointCircles requires at least two points"); + + const auto& end0 = points.front(); + const auto& end1 = points.back(); + + drawCircle(end0, radius, color, numSegments, normal); + drawCircle(end1, radius, color, numSegments, normal); + + Mn::Vector3 prevPos; + for (int i = 0; i < points.size(); i++) { + const auto& pos = points[i]; + if (i > 0) { + if ((prevPos - end0).length() > radius && + (prevPos - end1).length() > radius && + (pos - end0).length() > radius && (pos - end1).length() > radius) { + drawTransformedLine(prevPos, pos, color); + } else { + // we must scissor one or both points to lie on the end circles + auto pt0 = prevPos; + auto pt1 = pos; + if (!scissorSegmentToOutsideCircle(&pt0, &pt1, end0, radius)) { + continue; + } + if (!scissorSegmentToOutsideCircle(&pt0, &pt1, end1, radius)) { + continue; + } + drawTransformedLine(pt0, pt1, color); + } + } + prevPos = pos; + } +} + +void DebugLineRender::updateCachedInputTransform() { + _cachedInputTransform = Mn::Matrix4{Magnum::Math::IdentityInit}; + for (const auto& item : _inputTransformStack) { + _cachedInputTransform = _cachedInputTransform * item; + } +} + +} // namespace gfx +} // namespace esp diff --git a/src/esp/gfx/DebugLineRender.h b/src/esp/gfx/DebugLineRender.h new file mode 100644 index 0000000000..9c95f0171a --- /dev/null +++ b/src/esp/gfx/DebugLineRender.h @@ -0,0 +1,187 @@ +// Copyright (c) Facebook, Inc. and its affiliates. +// This source code is licensed under the MIT license found in the +// LICENSE file in the root directory of this source tree. + +#ifndef ESP_GFX_DEBUGLINERENDER_H_ +#define ESP_GFX_DEBUGLINERENDER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace esp { +namespace gfx { + +/** + * @brief Singleton utility class for on-the-fly rendering of lines (e.g. every + * frame). This is intended for debugging or simple UX for prototype apps. The + * API prioritizes ease-of-use over maximum runtime performance. + * + * It's easy to add new primitives here; see drawCircle as a reference. In + * addition, if you're interested to integrate Magnum's line-based primitives, + * see src/deps/magnum/doc/generated/primitives.cpp and also this discussion: + * https://github.com/facebookresearch/habitat-sim/pull/1349#discussion_r660092144 + */ +class DebugLineRender { + public: + /** + * @brief Constructor. This allocates GPU resources so it should persist + * frame-to-frame (don't recreate it every frame). + */ + DebugLineRender(); + + /** @brief Release GPU resources */ + void releaseGLResources(); + + /** @brief Copying is not allowed */ + DebugLineRender(const DebugLineRender&) = delete; + + /** @brief Copying is not allowed */ + DebugLineRender& operator=(const DebugLineRender&) = delete; + + /** + * @brief Set line width in pixels (approximate). + */ + void setLineWidth(float lineWidth); + + /** + * @brief Draw a line segment in world-space (ignores pushTransform). + */ + void drawLine(const Magnum::Vector3& from, + const Magnum::Vector3& to, + const Magnum::Color4& color) { + drawLine(from, to, color, color); + } + + /** + * @brief Draw a line segment in world-space. + */ + void drawLine(const Magnum::Vector3& from, + const Magnum::Vector3& to, + const Magnum::Color4& fromColor, + const Magnum::Color4& toColor); + + /** + * @brief Submit lines to the GL renderer. Call this once per frame. + * Because this uses transparency, you should ideally call this *after* + * submitting opaque scene geometry. + */ + void flushLines(const Magnum::Matrix4& camMatrix, + const Magnum::Matrix4& projMatrix, + const Magnum::Vector2i& viewport); + + /** + * @brief Push (multiply) a transform onto the transform stack, affecting all + * line-drawing until popped. Must be paired with popTransform(). For example, + * push an object's local-to-world transform and then use drawTransformedLine + * to draw lines in the object's local space. + */ + void pushTransform(const Magnum::Matrix4& transform); + + /** + * @brief See also pushTransform. + */ + void popTransform(); + + /** + * @brief Draw a line segment in world-space or local-space (see + * pushTransform). + */ + void drawTransformedLine(const Magnum::Vector3& from, + const Magnum::Vector3& to, + const Magnum::Color4& color) { + drawTransformedLine(from, to, color, color); + } + + /** + * @brief Draw a line segment in world-space or local-space (see + * pushTransform). + */ + void drawTransformedLine(const Magnum::Vector3& from, + const Magnum::Vector3& to, + const Magnum::Color4& fromColor, + const Magnum::Color4& toColor); + + /** + * @brief Draw a box in world-space or local-space (see pushTransform). + */ + void drawBox(const Magnum::Vector3& min, + const Magnum::Vector3& max, + const Magnum::Color4& color); + + /** + * @brief Draw a circle in world-space or local-space (see pushTransform). + * The circle is an approximation; see numSegments. + */ + void drawCircle(const Magnum::Vector3& pos, + float radius, + const Magnum::Color4& color, + int numSegments = 24, + const Magnum::Vector3& normal = Magnum::Vector3(0.0, + 1.0, + 0.0)); + + /** + * @brief Draw a sequence of line segments with circles at the two endpoints. + * In world-space or local-space (see pushTransform). + */ + void drawPathWithEndpointCircles( + const std::vector& points, + float radius, + const Magnum::Color4& color, + int numSegments = 24, + const Magnum::Vector3& normal = Magnum::Vector3(0.0, 1.0, 0.0)) { + drawPathWithEndpointCircles( + Magnum::Containers::ArrayView(points), radius, + color, numSegments, normal); + } + + /** + * @brief Draw a sequence of line segments with circles at the two endpoints. + * In world-space or local-space (see pushTransform). + * + * @param points Note that std::vector, std::array, or c-style array can be + * passed here. + * + */ + void drawPathWithEndpointCircles( + Magnum::Containers::ArrayView points, + float radius, + const Magnum::Color4& color, + int numSegments = 24, + const Magnum::Vector3& normal = Magnum::Vector3(0.0, 1.0, 0.0)); + + private: + void updateCachedInputTransform(); + + struct VertexRecord { + Magnum::Vector3 pos; + Magnum::Color4 color; + }; + + struct GLResourceSet { + Magnum::GL::Buffer buffer; + Magnum::GL::Mesh mesh{Magnum::GL::MeshPrimitive::Lines}; + Magnum::Shaders::FlatGL3D shader{ + Magnum::Shaders::FlatGL3D::Flag::VertexColor}; + }; + + std::vector _inputTransformStack; + Magnum::Matrix4 _cachedInputTransform{Magnum::Math::IdentityInit}; + float _internalLineWidth = 1.0f; + std::unique_ptr _glResources; + Magnum::Containers::Array _verts; +}; + +} // namespace gfx +} // namespace esp + +#endif diff --git a/src/esp/sensor/CameraSensor.cpp b/src/esp/sensor/CameraSensor.cpp index f1e34ac787..7d66a58d9a 100644 --- a/src/esp/sensor/CameraSensor.cpp +++ b/src/esp/sensor/CameraSensor.cpp @@ -147,6 +147,17 @@ bool CameraSensor::drawObservation(sim::Simulator& sim) { } else { // SensorType is Color, Depth or any other type draw(sim.getActiveSceneGraph(), flags); + + // include DebugLineRender in Color sensors + if (cameraSensorSpec_->sensorType == SensorType::Color) { + const auto debugLineRender = sim.getDebugLineRender(); + // debugLineRender is generally null (unless the user drew lines) + if (debugLineRender) { + debugLineRender->flushLines(renderCamera_->cameraMatrix(), + renderCamera_->projectionMatrix(), + renderCamera_->viewport()); + } + } } renderTarget().renderExit(); diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index 1de2679080..eaa62e9bca 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -72,6 +72,13 @@ void Simulator::close(const bool destroy) { resourceManager_ = nullptr; + if (debugLineRender_) { + // Python may keep around other shared_ptrs to this object, but we need + // to release GL resources here. + debugLineRender_->releaseGLResources(); + debugLineRender_ = nullptr; + } + // Keeping the renderer and the context only matters when the // background renderer was initialized. if (destroy || !renderer_->wasBackgroundRendererInitialized()) { diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index 409b1a827a..f91eaebd44 100644 --- a/src/esp/sim/Simulator.h +++ b/src/esp/sim/Simulator.h @@ -12,6 +12,7 @@ #include "esp/assets/ResourceManager.h" #include "esp/core/esp.h" #include "esp/core/random.h" +#include "esp/gfx/DebugLineRender.h" #include "esp/gfx/RenderTarget.h" #include "esp/gfx/WindowlessContext.h" #include "esp/metadata/MetadataMediator.h" @@ -915,6 +916,14 @@ class Simulator { return Magnum::Vector3(); } + std::shared_ptr getDebugLineRender() { + // We only create this if/when used (lazy creation) + if (!debugLineRender_) { + debugLineRender_ = std::make_shared(); + } + return debugLineRender_; + } + /** * @brief Compute the navmesh for the simulator's current active scene and * assign it to the referenced @ref nav::PathFinder. @@ -1460,6 +1469,8 @@ class Simulator { */ Corrade::Containers::Optional requiresTextures_; + std::shared_ptr debugLineRender_; + ESP_SMART_POINTERS(Simulator) };