diff --git a/src/candlewick/core/CommandBuffer.h b/src/candlewick/core/CommandBuffer.h index 604a5acf..badf1091 100644 --- a/src/candlewick/core/CommandBuffer.h +++ b/src/candlewick/core/CommandBuffer.h @@ -3,7 +3,6 @@ #include "Core.h" #include #include -#include #include namespace candlewick { diff --git a/src/candlewick/core/Core.h b/src/candlewick/core/Core.h index 7484f8d5..3eb74141 100644 --- a/src/candlewick/core/Core.h +++ b/src/candlewick/core/Core.h @@ -1,6 +1,9 @@ #pragma once #include +#include + +#define CDW_ASSERT(condition, msg) assert(((condition) && (msg))) namespace candlewick { struct Camera; diff --git a/src/candlewick/core/GuiSystem.h b/src/candlewick/core/GuiSystem.h index e8c771f1..9a20755e 100644 --- a/src/candlewick/core/GuiSystem.h +++ b/src/candlewick/core/GuiSystem.h @@ -1,7 +1,6 @@ /// \defgroup gui_util GUI utilities /// Tools, render systems, etc... for the Candlewick GUI. #include "Core.h" -#include "Tags.h" #include #include @@ -14,17 +13,17 @@ class GuiSystem { public: using GuiBehavior = std::function; - GuiSystem(NoInitT, GuiBehavior behav) - : m_renderer(nullptr), _callback(behav) {} GuiSystem(const Renderer &renderer, GuiBehavior behav); - bool init(const Renderer &renderer); void render(CommandBuffer &cmdBuf); void release(); bool initialized() const { return m_initialized; } GuiBehavior _callback; + +private: + bool init(const Renderer &renderer); }; /// \ingroup gui_util diff --git a/src/candlewick/core/Mesh.cpp b/src/candlewick/core/Mesh.cpp index 08ae24db..2fdbed74 100644 --- a/src/candlewick/core/Mesh.cpp +++ b/src/candlewick/core/Mesh.cpp @@ -4,8 +4,6 @@ #include "MeshLayout.h" #include "./errors.h" -#include - namespace candlewick { MeshView::MeshView(const MeshView &parent, Uint32 subVertexOffset, @@ -17,9 +15,10 @@ MeshView::MeshView(const MeshView &parent, Uint32 subVertexOffset, indexOffset(parent.indexOffset + subIndexOffset), indexCount(subIndexCount) { // assumption: parent MeshView is validated - assert(validateMeshView(*this)); - assert(subVertexOffset + subVertexCount <= parent.vertexCount); - assert(subIndexOffset + subIndexCount <= parent.indexCount); + CDW_ASSERT(validateMeshView(*this), "MeshView failed validation."); + CDW_ASSERT(subVertexOffset + subVertexCount <= parent.vertexCount && + subIndexOffset + subIndexCount <= parent.indexCount, + ""); } Mesh::Mesh(NoInitT) {} diff --git a/src/candlewick/core/Renderer.cpp b/src/candlewick/core/Renderer.cpp index 62a8741e..a11f3dc8 100644 --- a/src/candlewick/core/Renderer.cpp +++ b/src/candlewick/core/Renderer.cpp @@ -3,7 +3,6 @@ #include #include -#include #include #include diff --git a/src/candlewick/core/Texture.cpp b/src/candlewick/core/Texture.cpp index 875067e4..9dbd5092 100644 --- a/src/candlewick/core/Texture.cpp +++ b/src/candlewick/core/Texture.cpp @@ -3,7 +3,6 @@ #include "errors.h" #include -#include #include namespace candlewick { @@ -45,7 +44,8 @@ Texture &Texture::operator=(Texture &&other) noexcept { SDL_GPUBlitRegion Texture::blitRegion(Uint32 x, Uint32 y, Uint32 layer_or_depth_plane) const { - assert(layer_or_depth_plane < layerCount()); + CDW_ASSERT(layer_or_depth_plane < layerCount(), + "layer is higher than layerCount!"); return { .texture = _texture, .mip_level = 0, diff --git a/src/candlewick/multibody/RobotScene.cpp b/src/candlewick/multibody/RobotScene.cpp index af039a30..810f83e5 100644 --- a/src/candlewick/multibody/RobotScene.cpp +++ b/src/candlewick/multibody/RobotScene.cpp @@ -35,6 +35,16 @@ invalid_enum(const char *msg, T type, std::format("{:s} - {:s}", msg, magic_enum::enum_name(type)), location); } +void updateRobotTransforms(entt::registry ®istry, + const pin::GeometryData &geom_data) { + auto robot_view = + registry.view(); + for (auto [ent, geom_id, tr] : robot_view.each()) { + SE3f pose = geom_data.oMg[geom_id].cast(); + tr = pose.toHomogeneousMatrix(); + } +} + auto RobotScene::pinGeomToPipeline(const coal::CollisionGeometry &geom) -> PipelineType { using enum coal::OBJECT_TYPE; @@ -102,9 +112,17 @@ void RobotScene::clearRobotGeometries() { RobotScene::RobotScene(entt::registry ®istry, const Renderer &renderer, const pin::GeometryModel &geom_model, const pin::GeometryData &geom_data, Config config) - : // screenSpaceShadows{.sampler = nullptr, .pass{NoInit}}, - m_registry(registry), m_config(config), m_renderer(renderer), - m_geomModel(geom_model), m_geomData(geom_data) { + : RobotScene(registry, renderer) { + m_config = config; + this->loadModels(geom_model, geom_data); +} + +void RobotScene::loadModels(const pin::GeometryModel &geom_model, + const pin::GeometryData &geom_data) { + SDL_assert(!initialized()); + m_geomModel = &geom_model; + m_geomData = &geom_data; + SDL_assert(initialized()); for (size_t i = 0; i < kNumPipelineTypes; i++) { renderPipelines[i] = NULL; @@ -120,8 +138,8 @@ RobotScene::RobotScene(entt::registry ®istry, const Renderer &renderer, } // initialize render target for GBuffer - const auto [width, height] = renderer.window.size(); - gBuffer.normalMap = Texture{renderer.device, + const auto [width, height] = m_renderer.window.size(); + gBuffer.normalMap = Texture{m_renderer.device, { .type = SDL_GPU_TEXTURETYPE_2D, .format = SDL_GPU_TEXTUREFORMAT_R16G16_FLOAT, @@ -143,29 +161,29 @@ RobotScene::RobotScene(entt::registry ®istry, const Renderer &renderer, auto meshDatas = loadGeometryObject(geom_obj); PipelineType pipeline_type = pinGeomToPipeline(*geom_obj.geometry); auto mesh = createMeshFromBatch(device(), meshDatas, true); - assert(validateMesh(mesh)); + SDL_assert(validateMesh(mesh)); // local copy for use const auto layout = mesh.layout(); // add entity for this geometry - entt::entity entity = registry.create(); - registry.emplace(entity, geom_id); - registry.emplace(entity); + entt::entity entity = m_registry.create(); + m_registry.emplace(entity, geom_id); + m_registry.emplace(entity); if (pipeline_type != PIPELINE_POINTCLOUD) - registry.emplace(entity); - registry.emplace(entity, std::move(mesh), - extractMaterials(meshDatas)); + m_registry.emplace(entity); + m_registry.emplace(entity, std::move(mesh), + extractMaterials(meshDatas)); add_pipeline_tag_component(m_registry, entity, pipeline_type); if (pipeline_type == PIPELINE_TRIANGLEMESH) { if (!ssaoPass.pipeline) { - ssaoPass = ssao::SsaoPass(renderer, layout, gBuffer.normalMap); + ssaoPass = ssao::SsaoPass(m_renderer, layout, gBuffer.normalMap); } // configure shadow pass if (enable_shadows && !shadowPass.pipeline) { shadowPass = - ShadowPassInfo::create(renderer, layout, config.shadow_config); + ShadowPassInfo::create(m_renderer, layout, m_config.shadow_config); } } @@ -173,26 +191,16 @@ RobotScene::RobotScene(entt::registry ®istry, const Renderer &renderer, SDL_Log("Building pipeline for type %s", magic_enum::enum_name(pipeline_type).data()); SDL_GPUGraphicsPipeline *pipeline = - createPipeline(layout, renderer.getSwapchainTextureFormat(), - renderer.depthFormat(), pipeline_type); - assert(pipeline); + createPipeline(layout, m_renderer.getSwapchainTextureFormat(), + m_renderer.depthFormat(), pipeline_type); + SDL_assert(pipeline); renderPipelines[pipeline_type] = pipeline; } } } -void updateRobotTransforms(entt::registry ®istry, - const pin::GeometryData &geom_data) { - auto robot_view = - registry.view(); - for (auto [ent, geom_id, tr] : robot_view.each()) { - SE3f pose = geom_data.oMg[geom_id].cast(); - tr = pose.toHomogeneousMatrix(); - } -} - void RobotScene::updateTransforms() { - ::candlewick::multibody::updateRobotTransforms(m_registry, m_geomData); + ::candlewick::multibody::updateRobotTransforms(m_registry, *m_geomData); } void RobotScene::collectOpaqueCastables() { @@ -312,7 +320,7 @@ void RobotScene::renderPBRTriangleGeometry(CommandBuffer &command_buffer, .pushFragmentUniform(2, &_useSsao, sizeof(_useSsao)); auto *pipeline = renderPipelines[PIPELINE_TRIANGLEMESH]; - assert(pipeline); + SDL_assert(pipeline); SDL_BindGPUGraphicsPipeline(render_pass, pipeline); auto all_view = diff --git a/src/candlewick/multibody/RobotScene.h b/src/candlewick/multibody/RobotScene.h index ad82962b..b329be53 100644 --- a/src/candlewick/multibody/RobotScene.h +++ b/src/candlewick/multibody/RobotScene.h @@ -16,14 +16,24 @@ #include namespace candlewick { - namespace multibody { void updateRobotTransforms(entt::registry ®istry, const pin::GeometryData &geom_data); - /// \brief A scene/render system for robot geometries using Pinocchio. - class RobotScene { + /// \brief A render system for Pinocchio robot geometries using Pinocchio. + /// + /// This internally stores references to pinocchio::GeometryModel and + /// pinocchio::GeometryData objects. + class RobotScene final { + [[nodiscard]] bool initialized() const { return m_geomModel && m_geomData; } + + void renderPBRTriangleGeometry(CommandBuffer &command_buffer, + const Camera &camera); + + void renderOtherGeometry(CommandBuffer &command_buffer, + const Camera &camera); + public: enum PipelineType { PIPELINE_TRIANGLEMESH, @@ -83,10 +93,32 @@ namespace multibody { ShadowPassConfig shadow_config; }; + /// \brief Non-initializing constructor. + RobotScene(entt::registry ®istry, const Renderer &renderer) + : m_registry(registry), m_renderer(renderer), m_config() {} + + /// \brief Constructor which initializes the system. + /// + /// loadModels() will be called. RobotScene(entt::registry ®istry, const Renderer &renderer, const pin::GeometryModel &geom_model, const pin::GeometryData &geom_data, Config config); + RobotScene(const RobotScene &) = delete; + + void setConfig(const Config &config) { + CDW_ASSERT( + !initialized(), + "Cannot call setConfig() after render system was initialized."); + m_config = config; + } + + /// \brief Set the internal geometry model and data pointers, and load the + /// corresponding models. + void loadModels(const pin::GeometryModel &geom_model, + const pin::GeometryData &geom_data); + + /// \brief Update the transform component of the GeometryObject entities. void updateTransforms(); void collectOpaqueCastables(); @@ -112,12 +144,8 @@ namespace multibody { /// \warning Call updateRobotTransforms() before rendering the objects with /// this function. void render(CommandBuffer &command_buffer, const Camera &camera); - /// \brief PBR render pass for triangle meshes. - void renderPBRTriangleGeometry(CommandBuffer &command_buffer, - const Camera &camera); - /// \brief Render pass for other geometry. - void renderOtherGeometry(CommandBuffer &command_buffer, - const Camera &camera); + + /// \brief Release all resources. void release(); Config &config() { return m_config; } @@ -125,11 +153,11 @@ namespace multibody { inline bool pbrHasPrepass() const { return m_config.triangle_has_prepass; } inline bool shadowsEnabled() const { return m_config.enable_shadows; } - /// \brief Getter for the referenced pinocchio GeometryModel object. - const pin::GeometryModel &geomModel() const { return m_geomModel; } + /// \brief Getter for the pinocchio GeometryModel object. + const pin::GeometryModel &geomModel() const { return *m_geomModel; } - /// \brief Getter for the referenced pinocchio GeometryData object. - const pin::GeometryData &geomData() const { return m_geomData; } + /// \brief Getter for the pinocchio GeometryData object. + const pin::GeometryData &geomData() const { return *m_geomData; } const entt::registry ®istry() const { return m_registry; } @@ -146,10 +174,10 @@ namespace multibody { private: entt::registry &m_registry; - Config m_config; const Renderer &m_renderer; - std::reference_wrapper m_geomModel; - std::reference_wrapper m_geomData; + Config m_config; + const pin::GeometryModel *m_geomModel; + const pin::GeometryData *m_geomData; std::vector m_castables; }; static_assert(Scene); diff --git a/src/candlewick/multibody/Visualizer.cpp b/src/candlewick/multibody/Visualizer.cpp index a80970d8..3e2499b7 100644 --- a/src/candlewick/multibody/Visualizer.cpp +++ b/src/candlewick/multibody/Visualizer.cpp @@ -8,40 +8,46 @@ namespace candlewick::multibody { -Visualizer::Visualizer(const Config &config, const pin::Model &model, - const pin::GeometryModel &visual_model, - GuiSystem::GuiBehavior gui_callback) - : BaseVisualizer{model, visual_model}, registry{}, renderer{NoInit}, - guiSystem{NoInit, std::move(gui_callback)} { +static Renderer _create_renderer(const Visualizer::Config &config) { if (!SDL_Init(SDL_INIT_VIDEO)) { throw std::runtime_error( std::format("Failed to init video: {}", SDL_GetError())); } - ::new (&renderer) Renderer{Device{auto_detect_shader_format_subset()}, - Window{"Candlewick Pinocchio visualizer", - int(config.width), int(config.height), 0}, - config.depth_stencil_format}; + return Renderer{Device{auto_detect_shader_format_subset()}, + Window{"Candlewick Pinocchio visualizer", int(config.width), + int(config.height), 0}, + config.depth_stencil_format}; +} + +Visualizer::Visualizer(const Config &config, const pin::Model &model, + const pin::GeometryModel &visual_model, + GuiSystem::GuiBehavior gui_callback) + : BaseVisualizer{model, visual_model}, registry{}, + renderer{_create_renderer(config)}, + guiSystem{renderer, std::move(gui_callback)}, + robotScene{registry, renderer} { RobotScene::Config rconfig; rconfig.enable_shadows = true; - robotScene.emplace(registry, renderer, visualModel(), visualData(), rconfig); + robotScene.setConfig(rconfig); + robotScene.loadModels(visualModel(), visualData()); + debugScene.emplace(registry, renderer); debugScene->addSystem(this->model(), data()); std::tie(m_triad, std::ignore) = debugScene->addTriad(); std::tie(m_grid, std::ignore) = debugScene->addLineGrid(); - robotScene->directionalLight = { + robotScene.directionalLight = { .direction = {0., -1., -1.}, .color = {1.0, 1.0, 1.0}, .intensity = 8.0, }; - guiSystem.init(renderer); - robotScene->worldSpaceBounds.update({-1., -1., 0.}, {+1., +1., 1.}); + robotScene.worldSpaceBounds.update({-1., -1., 0.}, {+1., +1., 1.}); const Uint32 prepeat = 25; - m_plane = robotScene->addEnvironmentObject( + m_plane = robotScene.addEnvironmentObject( loadPlaneTiled(0.5f, prepeat, prepeat), Mat4f::Identity()); if (!envStatus.show_plane) { registry.emplace(m_plane); @@ -80,7 +86,7 @@ void Visualizer::setCameraPose(const Eigen::Ref &pose) { } Visualizer::~Visualizer() { - robotScene->release(); + robotScene.release(); debugScene->release(); guiSystem.release(); renderer.destroy(); @@ -91,7 +97,7 @@ void Visualizer::displayImpl() { this->processEvents(); debugScene->update(); - robotScene->updateTransforms(); + robotScene.updateTransforms(); render(); } @@ -99,14 +105,14 @@ void Visualizer::render() { CommandBuffer cmdBuf = renderer.acquireCommandBuffer(); if (renderer.waitAndAcquireSwapchain(cmdBuf)) { - robotScene->collectOpaqueCastables(); - std::span castables = robotScene->castables(); - renderShadowPassFromAABB(cmdBuf, robotScene->shadowPass, - robotScene->directionalLight, castables, - robotScene->worldSpaceBounds); + robotScene.collectOpaqueCastables(); + std::span castables = robotScene.castables(); + renderShadowPassFromAABB(cmdBuf, robotScene.shadowPass, + robotScene.directionalLight, castables, + robotScene.worldSpaceBounds); auto &camera = controller.camera; - robotScene->render(cmdBuf, camera); + robotScene.render(cmdBuf, camera); debugScene->render(cmdBuf, camera); guiSystem.render(cmdBuf); } diff --git a/src/candlewick/multibody/Visualizer.h b/src/candlewick/multibody/Visualizer.h index 096e7651..779b80d6 100644 --- a/src/candlewick/multibody/Visualizer.h +++ b/src/candlewick/multibody/Visualizer.h @@ -55,7 +55,7 @@ class Visualizer final : public BaseVisualizer { entt::registry registry; Renderer renderer; GuiSystem guiSystem; - std::optional robotScene; + RobotScene robotScene; std::optional debugScene; CylindricalCamera controller; CameraControlParams cameraParams; @@ -102,8 +102,8 @@ class Visualizer final : public BaseVisualizer { /// \brief Clear objects void clean() override { - robotScene->clearEnvironment(); - robotScene->clearRobotGeometries(); + robotScene.clearEnvironment(); + robotScene.clearRobotGeometries(); debugScene->registry().clear(); } diff --git a/src/candlewick/multibody/visualizer_events_gui.cpp b/src/candlewick/multibody/visualizer_events_gui.cpp index e6c005de..5ab0e37b 100644 --- a/src/candlewick/multibody/visualizer_events_gui.cpp +++ b/src/candlewick/multibody/visualizer_events_gui.cpp @@ -167,7 +167,7 @@ void Visualizer::default_gui_exec() { if (envStatus.show_our_about) ::candlewick::showCandlewickAboutWindow(&envStatus.show_our_about); - auto &light = robotScene->directionalLight; + auto &light = robotScene.directionalLight; ImGuiWindowFlags window_flags = 0; window_flags |= ImGuiWindowFlags_AlwaysAutoResize; window_flags |= ImGuiWindowFlags_MenuBar;