├── .gitignore ├── CMakeLists.txt ├── README.md ├── include ├── app.h ├── command.h ├── commands.h ├── imgui_impl_raylib.h ├── import_mesh.h ├── raygizmo.h ├── rlImGui.h ├── rlImGuiColors.h ├── rlights.h └── robot.h ├── resources ├── r2d2.urdf ├── screenshot.png ├── shaders │ ├── lighting.fs │ └── lighting.vs └── simple.urdf └── src ├── app.cc ├── command.cc ├── import_mesh.cc ├── main.cc ├── raygizmo.cc ├── rlImGui.cc └── robot.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | .cache/clangd/index 3 | compile_commands.json 4 | imgui.ini 5 | .DS_Store 6 | .nvim.lua 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | 4 | project(urdf-editor) 5 | 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | include(FetchContent) 10 | FetchContent_Declare( 11 | raylib 12 | GIT_REPOSITORY https://github.com/raysan5/raylib.git 13 | GIT_TAG 5.0 14 | ) 15 | FetchContent_Declare( 16 | pugixml 17 | GIT_REPOSITORY https://github.com/zeux/pugixml.git 18 | GIT_TAG v1.15 19 | ) 20 | FetchContent_Declare( 21 | loguru 22 | GIT_REPOSITORY https://github.com/emilk/loguru.git 23 | GIT_TAG 4adaa185883e3c04da25913579c451d3c32cfac1 24 | ) 25 | FetchContent_Declare( 26 | imgui 27 | GIT_REPOSITORY https://github.com/ocornut/imgui.git 28 | GIT_TAG v1.90 29 | ) 30 | FetchContent_Declare( 31 | nfd 32 | GIT_REPOSITORY https://github.com/btzy/nativefiledialog-extended.git 33 | GIT_TAG v1.2.1 34 | ) 35 | FetchContent_Declare( 36 | fmt 37 | GIT_REPOSITORY https://github.com/fmtlib/fmt.git 38 | GIT_TAG 11.1.3 39 | ) 40 | # set(ASSIMP_BUILD_ASSIMP_TOOLS OFF) 41 | # set(ASSIMP_BUILD_TESTS OFF) 42 | # set(ASSIMP_INSTALL OFF) 43 | FetchContent_Declare( 44 | assimp 45 | GIT_REPOSITORY https://github.com/assimp/assimp.git 46 | GIT_TAG v5.4.3 47 | ) 48 | set(LOGURU_WITH_STREAMS TRUE) 49 | FetchContent_MakeAvailable(raylib imgui pugixml loguru nfd fmt assimp) 50 | 51 | add_library(imgui STATIC 52 | ${imgui_SOURCE_DIR}/imgui.cpp 53 | ${imgui_SOURCE_DIR}/imgui_draw.cpp 54 | ${imgui_SOURCE_DIR}/imgui_widgets.cpp 55 | ${imgui_SOURCE_DIR}/imgui_tables.cpp 56 | ${imgui_SOURCE_DIR}/misc/cpp/imgui_stdlib.cpp 57 | ${imgui_SOURCE_DIR}/backends/imgui_impl_glfw.cpp 58 | ${imgui_SOURCE_DIR}/backends/imgui_impl_opengl3.cpp 59 | ) 60 | 61 | target_include_directories(imgui PUBLIC 62 | ${imgui_SOURCE_DIR} 63 | ${imgui_SOURCE_DIR}/backends 64 | ${raylib_SOURCE_DIR}/src/external/glfw/include 65 | ) 66 | 67 | target_compile_definitions(imgui PUBLIC IMGUI_DEFINE_MATH_OPERATORS) 68 | 69 | file(GLOB SRC_FILES "src/*.cc") 70 | add_executable(${PROJECT_NAME} ${SRC_FILES}) 71 | 72 | target_include_directories(${PROJECT_NAME} PRIVATE include) 73 | 74 | target_link_libraries(${PROJECT_NAME} PRIVATE raylib imgui pugixml loguru nfd fmt assimp) 75 | 76 | target_compile_options(${PROJECT_NAME} PUBLIC -Wall -Wfloat-conversion -pedantic) 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # URDF Editor 2 | 3 | This is a work-in-progress WYSIWYG editor that allows you to create, modify and 4 | visualize robots defined with [URDF](http://wiki.ros.org/urdf). The main goal of 5 | this project is to create an editor that does not depend on the ROS software 6 | stack and is platform agnostic, you only need a C++17 compiler. 7 | 8 | ![Editor screenshot](./resources/screenshot.png) 9 | 10 | # Install 11 | 12 | Make sure that the following X dependencies are installed: 13 | 14 | ```bash 15 | sudo apt install pkg-config libgtk-3-dev libxi-dev libxcursor-dev libxinerama-dev libxrandr-dev 16 | ``` 17 | 18 | All the dependencies are automatically downloaded and compiled with CMake. To 19 | install, simply run: 20 | 21 | ```bash 22 | git clone https://github.com/noctrog/urdf-editor && cd urdf-editor 23 | cmake -B build -S . -DCMAKE_BUILD_TYPE=Release # or RelWithDebInfo for debugging 24 | cmake --build build -j$(nproc) 25 | ./build/urdf-editor 26 | ``` 27 | 28 | # Roadmap 29 | 30 | - [x] Load and save URDF files 31 | - [x] Do/Undo system 32 | - [x] GUI (with ImGui) 33 | - [x] Drag and drop link (GUI URDF Tree) 34 | - [x] 3D Gizmo for visual editing 35 | - [ ] Visualize joint movements 36 | - [ ] Material editor 37 | - [ ] Mesh geometry support (in the works through Assimp) 38 | 39 | # Known bugs 40 | 41 | - [ ] Changing name of collision looses focus on the collision tab. 42 | - [x] When changing the geometry type, the forward kinematics have to be refreshed. 43 | - [ ] Crash when changing geometry type to Mesh. 44 | 45 | # Contributing 46 | 47 | Contributions are always welcome! Feel free to open an issue or create a merge 48 | request :) 49 | -------------------------------------------------------------------------------- /include/app.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define RAYGIZMO_IMPLEMENTATION 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class App 13 | { 14 | public: 15 | App(int argc, char* argv[]); 16 | ~App() = default; 17 | 18 | void run(); 19 | 20 | private: 21 | 22 | void setup(); 23 | void update(); 24 | void draw(); 25 | void drawMenu(); 26 | void drawScene(); 27 | void cleanup(); 28 | 29 | void drawToolbar(); 30 | void drawRobotTree(); 31 | void drawNodeProperties(); 32 | void drawSideMenu(); 33 | 34 | void originGui(urdf::Origin& origin); 35 | 36 | void menuName(std::optional& name, const char *label = ""); 37 | void menuOrigin(std::optional& origin); 38 | void menuMaterial(std::optional& material_name, const char *label = ""); 39 | void menuGeometry(urdf::Geometry& geometry, Model& model); 40 | void menuAxis(std::optional& axis); 41 | void menuDynamics(std::optional& dynamics); 42 | void menuLimit(std::optional& limit); 43 | 44 | void menuPropertiesInertial(urdf::LinkNodePtr link_node); 45 | void menuPropertiesVisual(urdf::LinkNodePtr link_node); 46 | void menuPropertiesCollisions(urdf::LinkNodePtr link_node, int i); 47 | 48 | CommandBuffer command_buffer_; 49 | 50 | Camera camera_; 51 | Shader shader_; 52 | std::shared_ptr robot_; 53 | 54 | NFD::Guard nfdguard_; 55 | 56 | bool bShowGrid_; 57 | bool bWindowShouldClose_; 58 | int menubar_height_; 59 | 60 | urdf::TreeNodePtr hovered_node_; 61 | urdf::TreeNodePtr selected_node_; 62 | urdf::OriginRawPtr selected_link_origin_; 63 | 64 | RGizmo gizmo_; 65 | }; 66 | -------------------------------------------------------------------------------- /include/command.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | class Command { 9 | public: 10 | virtual ~Command() = default; 11 | virtual void execute() = 0; 12 | virtual void undo() = 0; 13 | }; 14 | 15 | using CommandPtr = std::shared_ptr; 16 | 17 | class CommandBuffer 18 | { 19 | public: 20 | CommandBuffer(); 21 | 22 | void add(CommandPtr command); 23 | void execute(); 24 | void undo(); 25 | void redo(); 26 | 27 | bool canUndo(); 28 | bool canRedo(); 29 | private: 30 | std::vector executed_commands_; 31 | std::vector new_commands_; 32 | size_t current_command_; 33 | }; 34 | 35 | class CreateRobotCommand : public Command { 36 | public: 37 | CreateRobotCommand(std::shared_ptr& robot, 38 | const Shader& shader); 39 | void execute() override; 40 | void undo() override; 41 | private: 42 | std::shared_ptr& robot_; 43 | std::shared_ptr old_robot_; 44 | const Shader& shader_; 45 | }; 46 | 47 | class LoadRobotCommand : public Command { 48 | public: 49 | LoadRobotCommand(std::string filename, 50 | std::shared_ptr& robot, 51 | Shader& shader); 52 | void execute() override; 53 | void undo() override; 54 | private: 55 | const std::string filename_; 56 | std::shared_ptr& robot_; 57 | std::shared_ptr old_robot_; 58 | std::shared_ptr future_robot_; 59 | const Shader& shader_; 60 | }; 61 | 62 | class JointChangeParentCommand : public Command { 63 | public: 64 | JointChangeParentCommand(const urdf::JointNodePtr& node, 65 | const urdf::LinkNodePtr& new_parent, 66 | const urdf::RobotPtr& robot); 67 | void execute() override; 68 | void undo() override; 69 | private: 70 | urdf::JointNodePtr joint_; 71 | urdf::LinkNodePtr new_parent_; 72 | urdf::LinkNodePtr old_parent_; 73 | urdf::RobotPtr robot_; 74 | std::vector::iterator old_position_; 75 | }; 76 | 77 | class CreateJointCommand : public Command { 78 | public: 79 | CreateJointCommand(const char *joint_name, 80 | urdf::LinkNodePtr& parent, 81 | urdf::RobotPtr& robot); 82 | void execute() override; 83 | void undo() override; 84 | private: 85 | std::string joint_name_; 86 | urdf::LinkNodePtr parent_; 87 | urdf::JointNodePtr new_joint_; 88 | urdf::RobotPtr& robot_; 89 | }; 90 | 91 | class ChangeNameCommand : public Command { 92 | public: 93 | ChangeNameCommand(const std::string& old_name, 94 | const std::string& new_name, 95 | std::string& target); 96 | void execute() override; 97 | void undo() override; 98 | private: 99 | const std::string old_name_; 100 | const std::string new_name_; 101 | std::string& target_; 102 | }; 103 | 104 | class CreateNameCommand : public Command { 105 | public: 106 | explicit CreateNameCommand(std::optional& target); 107 | void execute() override; 108 | void undo() override; 109 | 110 | private: 111 | std::optional& target_; 112 | }; 113 | 114 | class CreateOriginCommand : public Command { 115 | public: 116 | explicit CreateOriginCommand(std::optional& target, urdf::OriginRawPtr& selected_origin); 117 | void execute() override; 118 | void undo() override; 119 | private: 120 | std::optional& target_; 121 | urdf::OriginRawPtr& selected_origin_; 122 | }; 123 | 124 | class UpdateOriginCommand : public Command { 125 | public: 126 | UpdateOriginCommand(urdf::Origin& old_origin, 127 | urdf::Origin& new_origin, 128 | urdf::Origin& target, 129 | urdf::RobotPtr& robot); 130 | void execute() override; 131 | void undo() override; 132 | private: 133 | urdf::Origin old_origin_; 134 | urdf::Origin new_origin_; 135 | urdf::Origin& target_; 136 | const urdf::RobotPtr robot_; 137 | }; 138 | 139 | class CreateAxisCommand : public Command { 140 | public: 141 | explicit CreateAxisCommand(std::optional& target); 142 | void execute() override; 143 | void undo() override; 144 | private: 145 | std::optional& target_; 146 | }; 147 | 148 | class CreateDynamicsCommand : public Command { 149 | public: 150 | explicit CreateDynamicsCommand(std::optional& target); 151 | void execute() override; 152 | void undo() override; 153 | private: 154 | std::optional& target_; 155 | }; 156 | 157 | class CreateLimitCommand : public Command { 158 | public: 159 | explicit CreateLimitCommand(std::optional& target); 160 | void execute() override; 161 | void undo() override; 162 | private: 163 | std::optional& target_; 164 | }; 165 | 166 | class ChangeGeometryCommand : public Command { 167 | public: 168 | ChangeGeometryCommand(urdf::GeometryTypePtr old_geometry, 169 | urdf::GeometryTypePtr new_geometry, 170 | urdf::Geometry& target, 171 | Model& model, 172 | urdf::RobotPtr& robot); 173 | void execute() override; 174 | void undo() override; 175 | private: 176 | urdf::GeometryTypePtr old_geometry_; 177 | urdf::GeometryTypePtr new_geometry_; 178 | urdf::Geometry& target_; 179 | Model& model_; 180 | urdf::RobotPtr robot_; 181 | }; 182 | 183 | class CreateInertialCommand : public Command { 184 | public: 185 | explicit CreateInertialCommand(urdf::LinkNodePtr& link, urdf::OriginRawPtr& selected_origin); 186 | void execute() override; 187 | void undo() override; 188 | 189 | private: 190 | urdf::LinkNodePtr link_; 191 | urdf::OriginRawPtr& selected_origin_; 192 | }; 193 | 194 | class CreateVisualCommand : public Command { 195 | public: 196 | CreateVisualCommand(urdf::LinkNodePtr& link, 197 | const urdf::RobotPtr& robot, 198 | const Shader& shader_); 199 | void execute() override; 200 | void undo() override; 201 | private: 202 | urdf::LinkNodePtr link_; 203 | const urdf::RobotPtr robot_; 204 | const Shader shader_; 205 | }; 206 | 207 | class DeleteVisualCommand : public Command { 208 | public: 209 | DeleteVisualCommand(urdf::LinkNodePtr& link, 210 | urdf::RobotPtr& robot); 211 | void execute() override; 212 | void undo() override; 213 | private: 214 | urdf::LinkNodePtr link_; 215 | Shader shader_; 216 | urdf::Visual old_visual_; 217 | const urdf::RobotPtr robot_; 218 | }; 219 | 220 | class AddCollisionCommand : public Command { 221 | public: 222 | explicit AddCollisionCommand(urdf::LinkNodePtr& link); 223 | void execute() override; 224 | void undo() override; 225 | private: 226 | urdf::LinkNodePtr link_; 227 | }; 228 | 229 | class DeleteCollisionCommand : public Command { 230 | public: 231 | DeleteCollisionCommand(urdf::LinkNodePtr& link, int i); 232 | void execute() override; 233 | void undo() override; 234 | private: 235 | urdf::LinkNodePtr link_; 236 | int i_; 237 | urdf::Collision old_collision_; 238 | }; 239 | 240 | class UpdateGeometryBoxCommand : public Command { 241 | public: 242 | UpdateGeometryBoxCommand(std::shared_ptr& box, 243 | const Vector3& old_size, 244 | Model& model, 245 | const Shader& shader); 246 | void execute() override; 247 | void undo() override; 248 | private: 249 | Vector3 new_size_; 250 | Vector3 old_size_; 251 | std::shared_ptr box_; 252 | Model& model_; 253 | const Shader& shader_; 254 | }; 255 | 256 | class UpdateGeometryCylinderCommand : public Command { 257 | public: 258 | UpdateGeometryCylinderCommand(std::shared_ptr& cylinder, 259 | float old_radius, 260 | float old_height, 261 | Model& model, 262 | const Shader& shader); 263 | void execute() override; 264 | void undo() override; 265 | private: 266 | float new_radius_; 267 | float new_length_; 268 | float old_radius_; 269 | float old_length_; 270 | std::shared_ptr cylinder_; 271 | urdf::LinkNodePtr link_; 272 | Model& model_; 273 | const Shader& shader_; 274 | }; 275 | 276 | class UpdateGeometrySphereCommand : public Command { 277 | public: 278 | UpdateGeometrySphereCommand(std::shared_ptr& sphere, 279 | float old_radius, 280 | Model& model, 281 | const Shader& shader); 282 | void execute() override; 283 | void undo() override; 284 | private: 285 | float new_radius_; 286 | float old_radius_; 287 | std::shared_ptr sphere_; 288 | Model& model_; 289 | const Shader& shader_; 290 | }; 291 | 292 | class UpdateGeometryMeshCommand : public Command { 293 | public: 294 | UpdateGeometryMeshCommand(std::shared_ptr& mesh, 295 | const std::string& new_filename, 296 | Model& model, 297 | const Shader& shader); 298 | void execute() override; 299 | void undo() override; 300 | private: 301 | std::string new_filename_; 302 | std::string old_filename_; 303 | std::shared_ptr mesh_; 304 | Model& model_; 305 | const Shader& shader_; 306 | }; 307 | -------------------------------------------------------------------------------- /include/commands.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | class Command 5 | { 6 | virtual ~Command() = default; 7 | virtual void execute() = 0; 8 | virtual void undo() = 0; 9 | }; 10 | -------------------------------------------------------------------------------- /include/imgui_impl_raylib.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * 3 | * raylibExtras * Utilities and Shared Components for Raylib 4 | * 5 | * rlImGui * basic ImGui integration 6 | * 7 | * LICENSE: ZLIB 8 | * 9 | * Copyright (c) 2023 Jeffery Myers 10 | * 11 | * Permission is hereby granted, free of charge, to any person obtaining a copy 12 | * of this software and associated documentation files (the "Software"), to deal 13 | * in the Software without restriction, including without limitation the rights 14 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | * copies of the Software, and to permit persons to whom the Software is 16 | * furnished to do so, subject to the following conditions: 17 | * 18 | * The above copyright notice and this permission notice shall be included in all 19 | * copies or substantial portions of the Software. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | * SOFTWARE. 28 | * 29 | **********************************************************************************************/ 30 | 31 | // dear imgui: Platform Backend for Raylib 32 | // (Info: Raylib is a cross-platform general purpose library for handling windows, inputs, graphics context creation, etc. using OpenGL) 33 | // This is is the low level ImGui backend for raylib, a higher level API that matches the raylib API can be found in rlImGui.h 34 | 35 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 36 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 37 | // Learn about Dear ImGui: 38 | // - FAQ https://dearimgui.com/faq 39 | // - Getting Started https://dearimgui.com/getting-started 40 | // - Documentation https://dearimgui.com/docs (same as your local docs/ folder). 41 | // - Introduction, links and more at the top of imgui.cpp 42 | 43 | #pragma once 44 | #include "imgui.h" // IMGUI_IMPL_API 45 | #ifndef IMGUI_DISABLE 46 | 47 | IMGUI_IMPL_API bool ImGui_ImplRaylib_Init(); 48 | IMGUI_IMPL_API void Imgui_ImplRaylib_BuildFontAtlas(); 49 | IMGUI_IMPL_API void ImGui_ImplRaylib_Shutdown(); 50 | IMGUI_IMPL_API void ImGui_ImplRaylib_NewFrame(); 51 | IMGUI_IMPL_API void ImGui_ImplRaylib_RenderDrawData(ImDrawData* draw_data); 52 | IMGUI_IMPL_API bool ImGui_ImplRaylib_ProcessEvents(); 53 | 54 | #endif // #ifndef IMGUI_DISABLE 55 | -------------------------------------------------------------------------------- /include/import_mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef IMPORT_MESH_H 2 | #define IMPORT_MESH_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | Mesh GenMeshCenteredCylinder(float radius, float height, int slices); 10 | 11 | // Function to create a raylib Mesh from an Assimp mesh 12 | Mesh LoadMeshFromAssimp(const aiMesh* mesh); 13 | 14 | // Function to load all meshes from a Collada file 15 | std::vector LoadColladaMeshes(const std::string& filename); 16 | 17 | Model LoadModelFromCollada(const std::string& filename); 18 | 19 | 20 | #endif // IMPORT_MESH_H 21 | -------------------------------------------------------------------------------- /include/raygizmo.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYGIZMO_H 2 | #define RAYGIZMO_H 3 | 4 | #include 5 | 6 | typedef enum RGizmoState { 7 | RGIZMO_STATE_COLD, 8 | 9 | RGIZMO_STATE_HOT, 10 | 11 | RGIZMO_STATE_HOT_ROT, 12 | RGIZMO_STATE_HOT_AXIS, 13 | RGIZMO_STATE_HOT_PLANE, 14 | 15 | RGIZMO_STATE_ACTIVE, 16 | 17 | RGIZMO_STATE_ACTIVE_ROT, 18 | RGIZMO_STATE_ACTIVE_AXIS, 19 | RGIZMO_STATE_ACTIVE_PLANE, 20 | } RGizmoState; 21 | 22 | typedef struct RGizmo { 23 | struct { 24 | Vector3 translation; 25 | Vector3 axis; 26 | float angle; 27 | } update; 28 | 29 | struct { 30 | float size; 31 | float handle_draw_thickness; 32 | float active_axis_draw_thickness; 33 | float axis_handle_length; 34 | float axis_handle_tip_length; 35 | float axis_handle_tip_radius; 36 | float plane_handle_offset; 37 | float plane_handle_size; 38 | } view; 39 | 40 | RGizmoState state; 41 | } RGizmo; 42 | 43 | void rgizmo_unload(void); 44 | 45 | RGizmo rgizmo_create(void); 46 | 47 | bool rgizmo_update(RGizmo *gizmo, Camera3D camera, Vector3 position); 48 | void rgizmo_draw(RGizmo gizmo, Camera3D camera, Vector3 position); 49 | Matrix rgizmo_get_transform(RGizmo gizmo, Vector3 position); 50 | 51 | #endif // RAYGIZMO_H 52 | -------------------------------------------------------------------------------- /include/rlImGui.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * 3 | * raylibExtras * Utilities and Shared Components for Raylib 4 | * 5 | * rlImGui * basic ImGui integration 6 | * 7 | * LICENSE: ZLIB 8 | * 9 | * Copyright (c) 2023 Jeffery Myers 10 | * 11 | * Permission is hereby granted, free of charge, to any person obtaining a copy 12 | * of this software and associated documentation files (the "Software"), to deal 13 | * in the Software without restriction, including without limitation the rights 14 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | * copies of the Software, and to permit persons to whom the Software is 16 | * furnished to do so, subject to the following conditions: 17 | * 18 | * The above copyright notice and this permission notice shall be included in all 19 | * copies or substantial portions of the Software. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | * SOFTWARE. 28 | * 29 | **********************************************************************************************/ 30 | 31 | #pragma once 32 | 33 | #include 34 | 35 | #define NO_FONT_AWESOME 36 | 37 | #ifndef NO_FONT_AWESOME 38 | #include "extras/IconsFontAwesome6.h" 39 | #define FONT_AWESOME_ICON_SIZE 11 40 | #endif 41 | 42 | #ifdef __cplusplus 43 | extern "C" { 44 | #endif 45 | 46 | // High level API. This API is designed in the style of raylib and meant to work with reaylib code. 47 | // It will manage it's own ImGui context and call common ImGui functions (like NewFrame and Render) for you 48 | // for a lower level API that matches the other ImGui platforms, please see imgui_impl_raylib.h 49 | 50 | /// 51 | /// Sets up ImGui, loads fonts and themes 52 | /// Calls ImGui_ImplRaylib_Init and sets the theme. Will install Font awesome by default 53 | /// 54 | /// when true(default) the dark theme is used, when false the light theme is used 55 | void rlImGuiSetup(bool darkTheme); 56 | 57 | /// 58 | /// Starts a new ImGui Frame 59 | /// Calls ImGui_ImplRaylib_NewFrame, ImGui_ImplRaylib_ProcessEvents, and ImGui::NewFrame together 60 | /// 61 | void rlImGuiBegin(); 62 | 63 | /// 64 | /// Ends an ImGui frame and submits all ImGui drawing to raylib for processing. 65 | /// Calls ImGui:Render, an d ImGui_ImplRaylib_RenderDrawData to draw to the current raylib render target 66 | /// 67 | void rlImGuiEnd(); 68 | 69 | /// 70 | /// Cleanup ImGui and unload font atlas 71 | /// Calls ImGui_ImplRaylib_Shutdown 72 | /// 73 | void rlImGuiShutdown(); 74 | 75 | // Advanced StartupAPI 76 | 77 | /// 78 | /// Custom initialization. Not needed if you call rlImGuiSetup. Only needed if you want to add custom setup code. 79 | /// must be followed by rlImGuiEndInitImGui 80 | /// Called by ImGui_ImplRaylib_Init, and does the first part of setup, before fonts are rendered 81 | /// 82 | void rlImGuiBeginInitImGui(); 83 | 84 | /// 85 | /// End Custom initialization. Not needed if you call rlImGuiSetup. Only needed if you want to add custom setup code. 86 | /// must be proceeded by rlImGuiBeginInitImGui 87 | /// Called by ImGui_ImplRaylib_Init and does the second part of setup, and renders fonts. 88 | /// 89 | void rlImGuiEndInitImGui(); 90 | 91 | /// 92 | /// Forces the font texture atlas to be recomputed and re-cached 93 | /// 94 | void rlImGuiReloadFonts(); 95 | 96 | // Advanced Update API 97 | 98 | /// 99 | /// Starts a new ImGui Frame with a specified delta time 100 | /// 101 | /// delta time, any value < 0 will use raylib GetFrameTime 102 | void rlImGuiBeginDelta(float deltaTime); 103 | 104 | // ImGui Image API extensions 105 | // Purely for convenience in working with raylib textures as images. 106 | // If you want to call ImGui image functions directly, simply pass them the pointer to the texture. 107 | 108 | /// 109 | /// Draw a texture as an image in an ImGui Context 110 | /// Uses the current ImGui Cursor position and the full texture size. 111 | /// 112 | /// The raylib texture to draw 113 | void rlImGuiImage(const Texture *image); 114 | 115 | /// 116 | /// Draw a texture as an image in an ImGui Context at a specific size 117 | /// Uses the current ImGui Cursor position and the specified width and height 118 | /// The image will be scaled up or down to fit as needed 119 | /// 120 | /// The raylib texture to draw 121 | /// The width of the drawn image 122 | /// The height of the drawn image 123 | void rlImGuiImageSize(const Texture *image, int width, int height); 124 | 125 | /// 126 | /// Draw a texture as an image in an ImGui Context at a specific size 127 | /// Uses the current ImGui Cursor position and the specified size 128 | /// The image will be scaled up or down to fit as needed 129 | /// 130 | /// The raylib texture to draw 131 | /// The size of drawn image 132 | void rlImGuiImageSizeV(const Texture* image, Vector2 size); 133 | 134 | /// 135 | /// Draw a portion texture as an image in an ImGui Context at a defined size 136 | /// Uses the current ImGui Cursor position and the specified size 137 | /// The image will be scaled up or down to fit as needed 138 | /// 139 | /// The raylib texture to draw 140 | /// The width of the drawn image 141 | /// The height of the drawn image 142 | /// The portion of the texture to draw as an image. Negative values for the width and height will flip the image 143 | void rlImGuiImageRect(const Texture* image, int destWidth, int destHeight, Rectangle sourceRect); 144 | 145 | /// 146 | /// Draws a render texture as an image an ImGui Context, automatically flipping the Y axis so it will show correctly on screen 147 | /// 148 | /// The render texture to draw 149 | void rlImGuiImageRenderTexture(const RenderTexture* image); 150 | 151 | /// 152 | /// Draws a render texture as an image an ImGui Context, automatically flipping the Y axis so it will show correctly on screen 153 | /// Fits the render texture to the available content area 154 | /// 155 | /// The render texture to draw 156 | /// When true the image will be centered in the content area 157 | void rlImGuiImageRenderTextureFit(const RenderTexture* image, bool center); 158 | 159 | /// 160 | /// Draws a texture as an image button in an ImGui context. Uses the current ImGui cursor position and the full size of the texture 161 | /// 162 | /// The display name and ImGui ID for the button 163 | /// The texture to draw 164 | /// True if the button was clicked 165 | bool rlImGuiImageButton(const char* name, const Texture* image); 166 | 167 | /// 168 | /// Draws a texture as an image button in an ImGui context. Uses the current ImGui cursor position and the specified size. 169 | /// 170 | /// The display name and ImGui ID for the button 171 | /// The texture to draw 172 | /// The size of the button/param> 173 | /// True if the button was clicked 174 | bool rlImGuiImageButtonSize(const char* name, const Texture* image, struct ImVec2 size); 175 | 176 | #ifdef __cplusplus 177 | } 178 | #endif 179 | -------------------------------------------------------------------------------- /include/rlImGuiColors.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * 3 | * raylibExtras * Utilities and Shared Components for Raylib 4 | * 5 | * rlImGui * basic ImGui integration 6 | * 7 | * LICENSE: ZLIB 8 | * 9 | * Copyright (c) 2020 Jeffery Myers 10 | * 11 | * Permission is hereby granted, free of charge, to any person obtaining a copy 12 | * of this software and associated documentation files (the "Software"), to deal 13 | * in the Software without restriction, including without limitation the rights 14 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | * copies of the Software, and to permit persons to whom the Software is 16 | * furnished to do so, subject to the following conditions: 17 | * 18 | * The above copyright notice and this permission notice shall be included in all 19 | * copies or substantial portions of the Software. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | * SOFTWARE. 28 | * 29 | **********************************************************************************************/ 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | 36 | namespace rlImGuiColors 37 | { 38 | inline ImVec4 Convert(::Color color) 39 | { 40 | return ImVec4(color.r / 255.0f, color.g / 255.0f, color.b / 255.0f, color.a / 255.0f); 41 | } 42 | 43 | inline ::Color Convert(ImVec4 color) 44 | { 45 | return ::Color{ (unsigned char)(color.x * 255.0f), (unsigned char)(color.y * 255.0f), (unsigned char)(color.z * 255.0f), (unsigned char)(color.w * 255.0f) }; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /include/rlights.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * 3 | * raylib.lights - Some useful functions to deal with lights data 4 | * 5 | * CONFIGURATION: 6 | * 7 | * #define RLIGHTS_IMPLEMENTATION 8 | * Generates the implementation of the library into the included file. 9 | * If not defined, the library is in header only mode and can be included in other headers 10 | * or source files without problems. But only ONE file should hold the implementation. 11 | * 12 | * LICENSE: zlib/libpng 13 | * 14 | * Copyright (c) 2017-2023 Victor Fisac (@victorfisac) and Ramon Santamaria (@raysan5) 15 | * 16 | * This software is provided "as-is", without any express or implied warranty. In no event 17 | * will the authors be held liable for any damages arising from the use of this software. 18 | * 19 | * Permission is granted to anyone to use this software for any purpose, including commercial 20 | * applications, and to alter it and redistribute it freely, subject to the following restrictions: 21 | * 22 | * 1. The origin of this software must not be misrepresented; you must not claim that you 23 | * wrote the original software. If you use this software in a product, an acknowledgment 24 | * in the product documentation would be appreciated but is not required. 25 | * 26 | * 2. Altered source versions must be plainly marked as such, and must not be misrepresented 27 | * as being the original software. 28 | * 29 | * 3. This notice may not be removed or altered from any source distribution. 30 | * 31 | **********************************************************************************************/ 32 | 33 | #ifndef RLIGHTS_H 34 | #define RLIGHTS_H 35 | #define RLIGHTS_IMPLEMENTATION 36 | 37 | //---------------------------------------------------------------------------------- 38 | // Defines and Macros 39 | //---------------------------------------------------------------------------------- 40 | #define MAX_LIGHTS 4 // Max dynamic lights supported by shader 41 | 42 | //---------------------------------------------------------------------------------- 43 | // Types and Structures Definition 44 | //---------------------------------------------------------------------------------- 45 | 46 | // Light data 47 | typedef struct { 48 | int type; 49 | bool enabled; 50 | Vector3 position; 51 | Vector3 target; 52 | Color color; 53 | float attenuation; 54 | 55 | // Shader locations 56 | int enabledLoc; 57 | int typeLoc; 58 | int positionLoc; 59 | int targetLoc; 60 | int colorLoc; 61 | int attenuationLoc; 62 | } Light; 63 | 64 | // Light type 65 | typedef enum { 66 | LIGHT_DIRECTIONAL = 0, 67 | LIGHT_POINT 68 | } LightType; 69 | 70 | #ifdef __cplusplus 71 | extern "C" { // Prevents name mangling of functions 72 | #endif 73 | 74 | //---------------------------------------------------------------------------------- 75 | // Module Functions Declaration 76 | //---------------------------------------------------------------------------------- 77 | Light CreateLight(int type, Vector3 position, Vector3 target, Color color, Shader shader); // Create a light and get shader locations 78 | void UpdateLightValues(Shader shader, Light light); // Send light properties to shader 79 | 80 | #ifdef __cplusplus 81 | } 82 | #endif 83 | 84 | #endif // RLIGHTS_H 85 | 86 | 87 | /*********************************************************************************** 88 | * 89 | * RLIGHTS IMPLEMENTATION 90 | * 91 | ************************************************************************************/ 92 | 93 | #if defined(RLIGHTS_IMPLEMENTATION) 94 | 95 | #include "raylib.h" 96 | 97 | //---------------------------------------------------------------------------------- 98 | // Defines and Macros 99 | //---------------------------------------------------------------------------------- 100 | // ... 101 | 102 | //---------------------------------------------------------------------------------- 103 | // Types and Structures Definition 104 | //---------------------------------------------------------------------------------- 105 | // ... 106 | 107 | //---------------------------------------------------------------------------------- 108 | // Global Variables Definition 109 | //---------------------------------------------------------------------------------- 110 | static int lightsCount = 0; // Current amount of created lights 111 | 112 | //---------------------------------------------------------------------------------- 113 | // Module specific Functions Declaration 114 | //---------------------------------------------------------------------------------- 115 | // ... 116 | 117 | //---------------------------------------------------------------------------------- 118 | // Module Functions Definition 119 | //---------------------------------------------------------------------------------- 120 | 121 | // Create a light and get shader locations 122 | Light CreateLight(int type, Vector3 position, Vector3 target, Color color, Shader shader) 123 | { 124 | Light light = { 0 }; 125 | 126 | if (lightsCount < MAX_LIGHTS) 127 | { 128 | light.enabled = true; 129 | light.type = type; 130 | light.position = position; 131 | light.target = target; 132 | light.color = color; 133 | 134 | // NOTE: Lighting shader naming must be the provided ones 135 | light.enabledLoc = GetShaderLocation(shader, TextFormat("lights[%i].enabled", lightsCount)); 136 | light.typeLoc = GetShaderLocation(shader, TextFormat("lights[%i].type", lightsCount)); 137 | light.positionLoc = GetShaderLocation(shader, TextFormat("lights[%i].position", lightsCount)); 138 | light.targetLoc = GetShaderLocation(shader, TextFormat("lights[%i].target", lightsCount)); 139 | light.colorLoc = GetShaderLocation(shader, TextFormat("lights[%i].color", lightsCount)); 140 | 141 | UpdateLightValues(shader, light); 142 | 143 | lightsCount++; 144 | } 145 | 146 | return light; 147 | } 148 | 149 | // Send light properties to shader 150 | // NOTE: Light shader locations should be available 151 | void UpdateLightValues(Shader shader, Light light) 152 | { 153 | // Send to shader light enabled state and type 154 | SetShaderValue(shader, light.enabledLoc, &light.enabled, SHADER_UNIFORM_INT); 155 | SetShaderValue(shader, light.typeLoc, &light.type, SHADER_UNIFORM_INT); 156 | 157 | // Send to shader light position values 158 | float position[3] = { light.position.x, light.position.y, light.position.z }; 159 | SetShaderValue(shader, light.positionLoc, position, SHADER_UNIFORM_VEC3); 160 | 161 | // Send to shader light target position values 162 | float target[3] = { light.target.x, light.target.y, light.target.z }; 163 | SetShaderValue(shader, light.targetLoc, target, SHADER_UNIFORM_VEC3); 164 | 165 | // Send to shader light color values 166 | float color[4] = { (float)light.color.r/(float)255, (float)light.color.g/(float)255, 167 | (float)light.color.b/(float)255, (float)light.color.a/(float)255 }; 168 | SetShaderValue(shader, light.colorLoc, color, SHADER_UNIFORM_VEC4); 169 | } 170 | 171 | #endif // RLIGHTS_IMPLEMENTATION 172 | -------------------------------------------------------------------------------- /include/robot.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace urdf { 14 | 15 | inline Vector3 MatrixToXYZ(const Matrix& m) 16 | { 17 | Vector3 xyz {Vector3Zero()}; 18 | 19 | xyz.x = std::atan2(-m.m9, m.m10); 20 | xyz.y = std::atan2(m.m8, std::sqrt(m.m0 * m.m0 + m.m4 * m.m4)); 21 | xyz.z = std::atan2(-m.m4, m.m0); 22 | 23 | return xyz; 24 | } 25 | 26 | inline Vector3 PosFromMatrix(const Matrix& m) 27 | { 28 | return Vector3 {m.m12, m.m13, m.m14}; 29 | } 30 | 31 | struct Origin { 32 | Origin(); 33 | Origin(const char *xyz, const char *rpy); 34 | Origin(const Vector3& xyz, const Vector3& rpy); 35 | 36 | Matrix toMatrix(void) const; 37 | 38 | Vector3 xyz; 39 | Vector3 rpy; 40 | }; 41 | 42 | using OriginRawPtr = Origin*; 43 | 44 | struct GeometryType { 45 | virtual Model generateGeometry() = 0; 46 | }; 47 | 48 | struct Box : public GeometryType { 49 | Box(); 50 | Box(const char *size); 51 | Box(const Vector3& size); 52 | virtual ~Box() = default; 53 | 54 | Vector3 size; 55 | 56 | virtual Model generateGeometry() override; 57 | }; 58 | 59 | struct Cylinder : public GeometryType { 60 | Cylinder(); 61 | Cylinder(const char *radius, const char *length); 62 | Cylinder(float radius, float length); 63 | virtual ~Cylinder() = default; 64 | 65 | float radius; 66 | float length; 67 | 68 | virtual Model generateGeometry() override; 69 | }; 70 | 71 | struct Sphere : public GeometryType { 72 | Sphere(); 73 | Sphere(const char *radius); 74 | Sphere(float radius); 75 | virtual ~Sphere() = default; 76 | 77 | float radius; 78 | 79 | virtual Model generateGeometry() override; 80 | }; 81 | 82 | struct Mesh : public GeometryType { 83 | Mesh(const char *filename); 84 | virtual ~Mesh() = default; 85 | 86 | std::string filename; 87 | 88 | virtual Model generateGeometry() override; 89 | }; 90 | 91 | // TODO: implement mesh geometry 92 | 93 | using GeometryTypePtr = std::shared_ptr; 94 | 95 | struct Geometry { 96 | GeometryTypePtr type; 97 | }; 98 | 99 | struct Material { 100 | std::string name; 101 | std::optional rgba; 102 | std::optional texture_file; 103 | }; 104 | 105 | struct Inertia { 106 | Inertia(float ixx, float iyy, float izz, float ixy, float ixz, float iyz); 107 | 108 | float ixx, iyy, izz, ixy, ixz, iyz; 109 | }; 110 | 111 | struct Inertial { 112 | explicit Inertial(const char *xyz = nullptr, const char *rpy = nullptr, 113 | float mass = 0.0F, 114 | float ixx = 0.0F, float iyy = 0.0F, float izz = 0.0F, 115 | float ixy = 0.0F, float ixz = 0.0F, float iyz = 0.0F); 116 | 117 | Origin origin; 118 | float mass; 119 | Inertia inertia; 120 | }; 121 | 122 | struct Visual { 123 | std::optional name; 124 | std::optional origin; 125 | Geometry geometry; 126 | std::optional material_name; 127 | }; 128 | 129 | struct Collision { 130 | std::optional name; 131 | std::optional origin; 132 | Geometry geometry; 133 | }; 134 | 135 | struct Axis { 136 | Axis(); 137 | explicit Axis(const Vector3& _xyz); 138 | explicit Axis(const char *s_xyz); 139 | 140 | Vector3 xyz; 141 | }; 142 | 143 | struct Dynamics { 144 | Dynamics(); 145 | Dynamics(float _damping, float _friction); 146 | 147 | float damping; 148 | float friction; 149 | }; 150 | 151 | struct Limit 152 | { 153 | Limit(float lower, float upper, float effort, float velocity); 154 | 155 | float lower; 156 | float upper; 157 | float effort; 158 | float velocity; 159 | }; 160 | 161 | struct Link { 162 | Link() = default; 163 | explicit Link(std::string name); 164 | 165 | std::string name; 166 | std::optional inertial; 167 | std::optional visual; 168 | std::vector collision; 169 | }; 170 | 171 | struct Joint { 172 | Joint(const char *name, const char *parent, const char *child, const char *type); 173 | 174 | enum Type : int { 175 | kRevolute = 0, kContinuous, kPrismatic, kFixed, kFloating, kPlanar, kNumJointTypes 176 | }; 177 | 178 | std::string name; 179 | Type type; 180 | std::string parent; 181 | std::string child; 182 | std::optional origin; 183 | std::optional axis; 184 | // TODO: calibration 185 | std::optional dynamics; 186 | std::optional limit; // required only for prismatic and revolute 187 | // TODO: mimic 188 | }; 189 | 190 | struct TreeNode { 191 | TreeNode() = default; 192 | virtual ~TreeNode() = default; 193 | }; 194 | 195 | struct LinkNode; 196 | struct JointNode; 197 | 198 | using TreeNodePtr = std::shared_ptr; 199 | using LinkNodePtr = std::shared_ptr; 200 | using JointNodePtr = std::shared_ptr; 201 | 202 | struct LinkNode : TreeNode 203 | { 204 | LinkNode(); 205 | LinkNode(Link link, JointNodePtr parent_joint); 206 | 207 | void addCollision(); 208 | void deleteCollision(int i); 209 | 210 | Link link; 211 | JointNodePtr parent; 212 | std::vector children; 213 | 214 | Model visual_model; 215 | std::vector collision_models; 216 | 217 | Matrix w_T_l; // Transform matrix from world to current link 218 | }; 219 | 220 | struct JointNode : TreeNode 221 | { 222 | JointNode(Joint joint, LinkNodePtr parent, LinkNodePtr child); 223 | 224 | Joint joint; 225 | LinkNodePtr parent; 226 | LinkNodePtr child; 227 | }; 228 | 229 | class Robot { 230 | public: 231 | explicit Robot(LinkNodePtr root, const std::map& materials = {}); 232 | ~Robot(); 233 | 234 | void forwardKinematics(); 235 | static void forwardKinematics(LinkNodePtr& link); 236 | 237 | void buildGeometry(); 238 | void updateMaterial(const LinkNodePtr& link); 239 | 240 | void printTree() const; 241 | 242 | void setShader(const Shader& shader); 243 | 244 | void draw(const LinkNodePtr& highlighted = nullptr, 245 | const LinkNodePtr& selected = nullptr) const; 246 | 247 | LinkNodePtr getRoot() const; 248 | 249 | LinkNodePtr getLink(const Ray& ray); 250 | 251 | void forEveryLink(const std::function& func) const; 252 | void forEveryJoint(const std::function& func) const; 253 | 254 | const std::map& getMaterials() const; 255 | 256 | private: 257 | static Matrix originToMatrix(std::optional& origin); 258 | 259 | LinkNodePtr root_; 260 | 261 | Shader visual_shader_; 262 | 263 | // std::map> q; // vector motivation: joints with multiple dof 264 | 265 | std::map materials_; 266 | }; 267 | 268 | using RobotPtr = std::shared_ptr; 269 | 270 | RobotPtr buildRobot(const char *urdf_file); 271 | 272 | void exportRobot(const Robot& robot, std::string out_filename); 273 | 274 | pugi::xml_node findRoot(const pugi::xml_document& doc); 275 | 276 | // XML to data structures 277 | Link xmlNodeToLink(const pugi::xml_node& xml_node); 278 | Joint xmlNodeToJoint(const pugi::xml_node& xml_node); 279 | 280 | std::optional xmlNodeToInertial(const pugi::xml_node& xml_node); 281 | std::optional xmlNodeToOrigin(const pugi::xml_node& xml_node); 282 | Geometry xmlNodeToGeometry(const pugi::xml_node& xml_node); 283 | std::optional xmlNodeToMaterial(const pugi::xml_node& xml_node); 284 | 285 | // Data structures to XML 286 | void linkToXmlNode(pugi::xml_node& xml_node, const Link& link); 287 | void jointToXmlNode(pugi::xml_node& xml_node, const Joint& joint); 288 | 289 | void inertialToXmlNode(pugi::xml_node& xml_node, const Inertial& inertial); 290 | void originToXmlNode(pugi::xml_node& xml_node, const Origin& origin); 291 | void geometryToXmlNode(pugi::xml_node& xml_node, const Geometry& geometry); 292 | void materialToXmlNode(pugi::xml_node& xml_node, const Material& material); 293 | 294 | } // namespace urdf 295 | -------------------------------------------------------------------------------- /resources/r2d2.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | -------------------------------------------------------------------------------- /resources/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/noctrog/urdf-editor/1967315d9fcdf853883dea927c2b787ffce179d3/resources/screenshot.png -------------------------------------------------------------------------------- /resources/shaders/lighting.fs: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | // Input vertex attributes (from vertex shader) 4 | in vec3 fragPosition; 5 | in vec2 fragTexCoord; 6 | //in vec4 fragColor; 7 | in vec3 fragNormal; 8 | 9 | // Input uniform values 10 | uniform sampler2D texture0; 11 | uniform vec4 colDiffuse; 12 | 13 | // Output fragment color 14 | out vec4 finalColor; 15 | 16 | // NOTE: Add here your custom variables 17 | 18 | #define MAX_LIGHTS 4 19 | #define LIGHT_DIRECTIONAL 0 20 | #define LIGHT_POINT 1 21 | 22 | struct Light { 23 | int enabled; 24 | int type; 25 | vec3 position; 26 | vec3 target; 27 | vec4 color; 28 | }; 29 | 30 | // Input lighting values 31 | uniform Light lights[MAX_LIGHTS]; 32 | uniform vec4 ambient; 33 | uniform vec3 viewPos; 34 | 35 | void main() 36 | { 37 | // Texel color fetching from texture sampler 38 | // vec4 texelColor = texture(texture0, fragTexCoord); 39 | vec3 lightDot = vec3(0.0); 40 | vec3 normal = normalize(fragNormal); 41 | vec3 viewD = normalize(viewPos - fragPosition); 42 | vec3 specular = vec3(0.0); 43 | 44 | // NOTE: Implement here your fragment shader code 45 | 46 | for (int i = 0; i < MAX_LIGHTS; i++) 47 | { 48 | if (lights[i].enabled == 1) 49 | { 50 | vec3 light = vec3(0.0); 51 | 52 | if (lights[i].type == LIGHT_DIRECTIONAL) 53 | { 54 | light = -normalize(lights[i].target - lights[i].position); 55 | } 56 | 57 | if (lights[i].type == LIGHT_POINT) 58 | { 59 | light = normalize(lights[i].position - fragPosition); 60 | } 61 | 62 | float NdotL = max(dot(normal, light), 0.0); 63 | lightDot += lights[i].color.rgb*NdotL; 64 | 65 | float specCo = 0.0; 66 | if (NdotL > 0.0) specCo = pow(max(0.0, dot(viewD, reflect(-(light), normal))), 16.0); // 16 refers to shine 67 | specular += specCo; 68 | } 69 | } 70 | 71 | // finalColor = (texelColor*((colDiffuse + vec4(specular, 1.0))*vec4(lightDot, 1.0))); 72 | // finalColor += texelColor*(ambient/10.0)*colDiffuse; 73 | 74 | finalColor = (colDiffuse + vec4(specular, 1.0))*vec4(lightDot, 1.0); 75 | finalColor += (ambient/10.0)*colDiffuse; 76 | 77 | // Gamma correction 78 | finalColor = pow(finalColor, vec4(1.0/2.2)); 79 | } 80 | -------------------------------------------------------------------------------- /resources/shaders/lighting.vs: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | // Input vertex attributes 4 | in vec3 vertexPosition; 5 | in vec2 vertexTexCoord; 6 | in vec3 vertexNormal; 7 | in vec4 vertexColor; 8 | 9 | // Input uniform values 10 | uniform mat4 mvp; 11 | uniform mat4 matModel; 12 | uniform mat4 matNormal; 13 | 14 | // Output vertex attributes (to fragment shader) 15 | out vec3 fragPosition; 16 | out vec2 fragTexCoord; 17 | out vec4 fragColor; 18 | out vec3 fragNormal; 19 | 20 | // NOTE: Add here your custom variables 21 | 22 | void main() 23 | { 24 | // Send vertex attributes to fragment shader 25 | fragPosition = vec3(matModel*vec4(vertexPosition, 1.0)); 26 | fragTexCoord = vertexTexCoord; 27 | fragColor = vertexColor; 28 | fragNormal = normalize(vec3(matNormal*vec4(vertexNormal, 1.0))); 29 | 30 | // Calculate final vertex position 31 | gl_Position = mvp*vec4(vertexPosition, 1.0); 32 | } 33 | -------------------------------------------------------------------------------- /resources/simple.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /src/app.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | 26 | void drawGridZUp(int slices, float spacing) { 27 | int half_slices = slices / 2; 28 | 29 | rlBegin(RL_LINES); 30 | for (int i = -half_slices; i <= half_slices; i++) { 31 | if (i == 0) { 32 | rlColor3f(0.5F, 0.5F, 0.5F); 33 | rlColor3f(0.5F, 0.5F, 0.5F); 34 | rlColor3f(0.5F, 0.5F, 0.5F); 35 | rlColor3f(0.5F, 0.5F, 0.5F); 36 | } else { 37 | rlColor3f(0.75F, 0.75F, 0.75F); 38 | rlColor3f(0.75F, 0.75F, 0.75F); 39 | rlColor3f(0.75F, 0.75F, 0.75F); 40 | rlColor3f(0.75F, 0.75F, 0.75F); 41 | } 42 | 43 | rlVertex3f(static_cast(i) * spacing, 44 | static_cast(-half_slices) * spacing, 0.0F); 45 | rlVertex3f(static_cast(i) * spacing, 46 | static_cast(half_slices) * spacing, 0.0F); 47 | 48 | rlVertex3f(static_cast(-half_slices) * spacing, 49 | static_cast(i) * spacing, 0.0F); 50 | rlVertex3f(static_cast(half_slices) * spacing, 51 | static_cast(i) * spacing, 0.0F); 52 | } 53 | rlEnd(); 54 | } 55 | 56 | #define CAMERA_ROT_SPEED 0.003f 57 | #define CAMERA_MOVE_SPEED 0.01f 58 | #define CAMERA_ZOOM_SPEED 1.0f 59 | 60 | static void updateCamera(Camera3D *camera) { 61 | bool is_mmb_down = IsMouseButtonDown(MOUSE_BUTTON_MIDDLE); 62 | bool is_shift_down = IsKeyDown(KEY_LEFT_SHIFT); 63 | Vector2 mouse_delta = GetMouseDelta(); 64 | 65 | if (is_mmb_down && is_shift_down) { 66 | CameraMoveRight(camera, -CAMERA_MOVE_SPEED * mouse_delta.x, true); 67 | 68 | Vector3 right = GetCameraRight(camera); 69 | Vector3 up = Vector3CrossProduct( 70 | Vector3Subtract(camera->position, camera->target), right); 71 | up = Vector3Scale(Vector3Normalize(up), -CAMERA_MOVE_SPEED * mouse_delta.y); 72 | camera->position = Vector3Add(camera->position, up); 73 | camera->target = Vector3Add(camera->target, up); 74 | } else if (is_mmb_down) { 75 | CameraYaw(camera, -CAMERA_ROT_SPEED * mouse_delta.x, true); 76 | CameraPitch(camera, -CAMERA_ROT_SPEED * mouse_delta.y, true, true, false); 77 | } 78 | 79 | CameraMoveToTarget(camera, -GetMouseWheelMove() * CAMERA_ZOOM_SPEED); 80 | } 81 | 82 | App::App(int argc, char *argv[]) 83 | : bShowGrid_(true), selected_link_origin_{nullptr} { 84 | loguru::init(argc, argv); 85 | } 86 | 87 | void App::run() { 88 | setup(); 89 | 90 | while (not bWindowShouldClose_) { 91 | update(); 92 | draw(); 93 | bWindowShouldClose_ |= WindowShouldClose(); 94 | } 95 | 96 | cleanup(); 97 | } 98 | 99 | void App::setup() { 100 | const int screen_width = 1200; 101 | const int screen_height = 800; 102 | SetConfigFlags(FLAG_MSAA_4X_HINT | FLAG_WINDOW_RESIZABLE | 103 | FLAG_WINDOW_HIGHDPI); 104 | InitWindow(screen_width, screen_height, "URDF Editor"); 105 | 106 | shader_ = LoadShader("./resources/shaders/lighting.vs", 107 | "./resources/shaders/lighting.fs"); 108 | shader_.locs[SHADER_LOC_VECTOR_VIEW] = GetShaderLocation(shader_, "viewPos"); 109 | shader_.locs[SHADER_LOC_COLOR_DIFFUSE] = 110 | GetShaderLocation(shader_, "colDiffuse"); 111 | shader_.locs[SHADER_LOC_COLOR_AMBIENT] = 112 | GetShaderLocation(shader_, "ambient"); 113 | 114 | SetShaderValue(shader_, shader_.locs[SHADER_LOC_COLOR_AMBIENT], 115 | std::array({3.0F, 3.0F, 3.0F, 1.0F}).data(), 116 | SHADER_UNIFORM_VEC4); 117 | 118 | // Create lights 119 | std::array lights{}; 120 | lights[0] = CreateLight(LIGHT_DIRECTIONAL, Vector3{1, 1, 1}, Vector3Zero(), 121 | WHITE, shader_); 122 | 123 | // Define the camera to look into our 3d world 124 | camera_ = { 125 | {0.0F, 1.5F, 2.5F}, {0.0F, 0.0F, 0.0F}, {0.0F, 0.0F, 1.0F}, 45.0F, 0}; 126 | bWindowShouldClose_ = false; 127 | 128 | gizmo_ = rgizmo_create(); 129 | 130 | SetTargetFPS(120); 131 | rlImGuiSetup(true); 132 | } 133 | 134 | void App::update() { 135 | command_buffer_.execute(); 136 | 137 | // Update 138 | ImGuiIO &io = ImGui::GetIO(); 139 | if (not io.WantCaptureMouse) { 140 | updateCamera(&camera_); 141 | } 142 | 143 | //---------------------------------------------------------------------------------- 144 | 145 | // Input 146 | if (IsKeyDown(KEY_LEFT_CONTROL) && IsKeyPressed(KEY_Z)) { 147 | command_buffer_.undo(); 148 | } 149 | if (IsKeyDown(KEY_LEFT_CONTROL) && IsKeyPressed(KEY_Y)) { 150 | command_buffer_.redo(); 151 | } 152 | } 153 | 154 | void App::drawMenu() { 155 | rlImGuiBegin(); 156 | 157 | drawToolbar(); 158 | drawSideMenu(); 159 | 160 | rlImGuiEnd(); 161 | } 162 | 163 | void App::drawScene() { 164 | ClearBackground(LIGHTGRAY); 165 | 166 | // TODO(ramon): links can't be selected. Select instead visual, collision, 167 | // inertial and modify it's origin, if it exists. const auto selected_link = 168 | const auto selected_link = 169 | std::dynamic_pointer_cast(selected_node_); 170 | if (selected_link and selected_link_origin_) { 171 | // Create matrix w_T_o 172 | const Matrix &w_t_l = selected_link->w_T_l; 173 | Matrix l_t_o = selected_link_origin_->toMatrix(); 174 | Matrix w_t_o = MatrixMultiply(l_t_o, w_t_l); 175 | 176 | const Vector3 position{urdf::PosFromMatrix(w_t_o)}; 177 | if (rgizmo_update(&gizmo_, camera_, position)) { 178 | // Update matrix with gizmo 179 | w_t_o = MatrixMultiply(w_t_o, rgizmo_get_transform(gizmo_, position)); 180 | 181 | // Get matrix l_t_o 182 | l_t_o = MatrixMultiply(w_t_o, MatrixInvert(w_t_l)); 183 | 184 | // Save new origin 185 | selected_link_origin_->xyz.x = l_t_o.m12; 186 | selected_link_origin_->xyz.y = l_t_o.m13; 187 | selected_link_origin_->xyz.z = l_t_o.m14; 188 | selected_link_origin_->rpy = urdf::MatrixToXYZ(l_t_o); 189 | 190 | // Update the robot 191 | robot_->forwardKinematics(); 192 | } 193 | } 194 | 195 | const auto selected_joint = 196 | std::dynamic_pointer_cast(selected_node_); 197 | if (selected_joint and selected_joint->joint.origin) { 198 | urdf::Origin &joint_origin = *selected_joint->joint.origin; 199 | 200 | // Create matrix w_T_j 201 | const Matrix &w_t_p = selected_joint->parent->w_T_l; 202 | Matrix p_t_j = joint_origin.toMatrix(); 203 | Matrix w_t_j = MatrixMultiply(p_t_j, w_t_p); 204 | 205 | const Vector3 position{urdf::PosFromMatrix(w_t_j)}; 206 | if (rgizmo_update(&gizmo_, camera_, position)) { 207 | // Update matrix with gizmo 208 | w_t_j = MatrixMultiply(w_t_j, rgizmo_get_transform(gizmo_, position)); 209 | 210 | // Get matrix p_T_j 211 | p_t_j = MatrixMultiply(w_t_j, MatrixInvert(w_t_p)); 212 | 213 | // Save new origin {xyz, rpy} 214 | joint_origin.xyz.x = p_t_j.m12; 215 | joint_origin.xyz.y = p_t_j.m13; 216 | joint_origin.xyz.z = p_t_j.m14; 217 | joint_origin.rpy = urdf::MatrixToXYZ(p_t_j); 218 | 219 | // Update the robot 220 | robot_->forwardKinematics(); 221 | } 222 | } 223 | 224 | BeginMode3D(camera_); 225 | 226 | if (bShowGrid_) 227 | drawGridZUp(10, 1.0F); 228 | 229 | if (robot_) { 230 | const auto hovered_node = 231 | std::dynamic_pointer_cast(hovered_node_); 232 | robot_->draw(hovered_node, selected_link); 233 | } 234 | 235 | if (selected_link and selected_link_origin_) { 236 | const Matrix &w_t_l = selected_link->w_T_l; 237 | Matrix l_t_o = selected_link_origin_->toMatrix(); 238 | Matrix w_t_o = MatrixMultiply(l_t_o, w_t_l); 239 | 240 | const Vector3 position{urdf::PosFromMatrix(w_t_o)}; 241 | rgizmo_draw(gizmo_, camera_, position); 242 | } 243 | 244 | if (selected_joint and selected_joint->joint.origin) { 245 | const Matrix &w_t_p = selected_joint->parent->w_T_l; 246 | const Matrix p_t_j = selected_joint->joint.origin->toMatrix(); 247 | const Matrix w_t_j = MatrixMultiply(p_t_j, w_t_p); 248 | 249 | const Vector3 position{urdf::PosFromMatrix(w_t_j)}; 250 | rgizmo_draw(gizmo_, camera_, position); 251 | } 252 | 253 | EndMode3D(); 254 | } 255 | 256 | void App::draw() { 257 | BeginDrawing(); 258 | drawScene(); // Draw to render texture 259 | drawMenu(); // Draw to screen 260 | EndDrawing(); 261 | } 262 | 263 | void App::drawToolbar() { 264 | if (ImGui::BeginMainMenuBar()) { 265 | if (ImGui::BeginMenu("File")) { 266 | // File menu items go here 267 | if (ImGui::MenuItem("Open", "Ctrl+O")) { 268 | NFD::UniquePath out_path; 269 | 270 | // prepare filters for the dialog 271 | nfdfilteritem_t filter_item[2] = {{"URDF file", "urdf,xml"}}; 272 | 273 | // show the dialog 274 | nfdresult_t result = NFD::OpenDialog(out_path, filter_item, 1); 275 | if (result == NFD_OKAY) { 276 | command_buffer_.add(std::make_shared( 277 | out_path.get(), robot_, shader_)); 278 | } else if (result == NFD_CANCEL) { 279 | LOG_F(INFO, "User pressed cancel."); 280 | } else { 281 | LOG_F(ERROR, "Error: %s", NFD::GetError()); 282 | } 283 | } 284 | 285 | if (ImGui::MenuItem("Save", "Ctrl+S", false, static_cast(robot_))) { 286 | NFD::UniquePath out_path; 287 | nfdfilteritem_t filter_item[2] = {{"URDF file", "urdf"}}; 288 | nfdresult_t result = NFD::SaveDialog(out_path, filter_item, 1); 289 | if (result == NFD_OKAY) { 290 | LOG_F(INFO, "Success! %s", out_path.get()); 291 | exportRobot(*robot_, out_path.get()); 292 | } else if (result == NFD_CANCEL) { 293 | LOG_F(INFO, "User pressed cancel."); 294 | } else { 295 | LOG_F(ERROR, "Error: %s", NFD::GetError()); 296 | } 297 | } 298 | if (ImGui::MenuItem("Exit", "Alt+F4")) { 299 | bWindowShouldClose_ = true; 300 | } 301 | 302 | ImGui::EndMenu(); 303 | } 304 | 305 | if (ImGui::BeginMenu("Edit")) { 306 | if (ImGui::MenuItem("Undo", "Ctrl+Z", false, command_buffer_.canUndo())) { 307 | command_buffer_.undo(); 308 | } 309 | if (ImGui::MenuItem("Redo", "Ctrl+Y", false, command_buffer_.canRedo())) { 310 | command_buffer_.redo(); 311 | } 312 | ImGui::Separator(); 313 | if (ImGui::MenuItem("New Robot", "Ctrl+N")) { 314 | command_buffer_.add( 315 | std::make_shared(robot_, shader_)); 316 | } 317 | auto link_node = 318 | std::dynamic_pointer_cast(selected_node_); 319 | if (ImGui::MenuItem("Create Joint", "Ctrl+J", false, 320 | link_node != nullptr)) { 321 | command_buffer_.add(std::make_shared( 322 | "New Joint", link_node, robot_)); 323 | } 324 | ImGui::EndMenu(); 325 | } 326 | 327 | if (ImGui::BeginMenu("View")) { 328 | ImGui::MenuItem("Show Grid", nullptr, &bShowGrid_); 329 | ImGui::EndMenu(); 330 | } 331 | 332 | if (ImGui::BeginMenu("Help")) { 333 | ImGui::MenuItem("About"); 334 | ImGui::EndMenu(); 335 | } 336 | } 337 | 338 | menubar_height_ = static_cast(ImGui::GetWindowSize().y); 339 | ImGui::EndMainMenuBar(); 340 | } 341 | 342 | void App::drawRobotTree() { 343 | ImGuiTableFlags table_flags = 344 | ImGuiTableFlags_BordersV | ImGuiTableFlags_BordersOuterH | 345 | ImGuiTableFlags_RowBg | ImGuiTableFlags_Resizable | 346 | ImGuiTableFlags_Hideable | ImGuiTableFlags_NoBordersInBody | 347 | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY; 348 | 349 | if (ImGui::BeginTable("robot table", 2, table_flags, ImVec2(0, 300))) { 350 | ImGui::TableSetupColumn("Name", 351 | ImGuiTableColumnFlags_NoHide | 352 | ImGuiTableColumnFlags_WidthStretch, 353 | 0.9F); 354 | ImGui::TableSetupColumn( 355 | "Type", ImGuiTableColumnFlags_NoHide | ImGuiTableColumnFlags_WidthFixed, 356 | 0.1F); 357 | ImGui::TableHeadersRow(); 358 | 359 | if (not robot_) { 360 | ImGui::TableNextRow(); 361 | ImGui::TableNextColumn(); 362 | ImGui::Text("--"); 363 | ImGui::TableNextColumn(); 364 | ImGui::Text("--"); 365 | } else { 366 | ImGuiTreeNodeFlags tree_flags = ImGuiTreeNodeFlags_DefaultOpen | 367 | ImGuiTreeNodeFlags_OpenOnArrow | 368 | ImGuiTreeNodeFlags_SpanAllColumns; 369 | 370 | urdf::LinkNodePtr current_link = robot_->getRoot(); 371 | 372 | std::function recursion = 373 | [&](auto link) { 374 | if (not link) 375 | return; 376 | 377 | // Unique ID for drag and drop 378 | void *node_id = static_cast(link.get()); 379 | 380 | ImGui::TableNextRow(); 381 | ImGui::TableNextColumn(); 382 | 383 | bool open = ImGui::TreeNodeEx( 384 | link->link.name.c_str(), 385 | tree_flags | 386 | (link->children.empty() ? ImGuiTreeNodeFlags_Leaf : 0) | 387 | (link.get() == selected_node_.get() 388 | ? ImGuiTreeNodeFlags_Selected 389 | : 0)); 390 | 391 | if (ImGui::IsItemHovered()) { 392 | hovered_node_ = link; 393 | } 394 | 395 | if (ImGui::IsItemClicked() and selected_node_ != link) { 396 | selected_node_ = link; 397 | selected_link_origin_ = nullptr; 398 | } 399 | 400 | // Drag source for link node 401 | if (ImGui::BeginDragDropSource(ImGuiDragDropFlags_None)) { 402 | ImGui::SetDragDropPayload("LINK_NODE", &node_id, sizeof(void *)); 403 | ImGui::Text("Moving %s", link->link.name.c_str()); 404 | ImGui::EndDragDropSource(); 405 | } 406 | 407 | // Drop target for joint node 408 | if (ImGui::BeginDragDropTarget()) { 409 | if (const ImGuiPayload *payload = 410 | ImGui::AcceptDragDropPayload("JOINT_NODE")) { 411 | urdf::JointNodePtr dropped_joint_node = 412 | *static_cast(payload->Data); 413 | LOG_F(INFO, "Dropped joint %s onto link %s", 414 | dropped_joint_node->joint.name.c_str(), 415 | link->link.name.c_str()); 416 | command_buffer_.add(std::make_shared( 417 | dropped_joint_node, link, robot_)); 418 | } 419 | ImGui::EndDragDropTarget(); 420 | } 421 | 422 | ImGui::TableNextColumn(); 423 | ImGui::Text("Link"); 424 | 425 | if (open) { 426 | for (urdf::JointNodePtr &joint : link->children) { 427 | ImGui::TableNextRow(); 428 | ImGui::TableNextColumn(); 429 | 430 | bool open = ImGui::TreeNodeEx( 431 | joint->joint.name.c_str(), 432 | tree_flags | (joint.get() == selected_node_.get() 433 | ? ImGuiTreeNodeFlags_Selected 434 | : 0)); 435 | 436 | if (ImGui::IsItemHovered()) { 437 | hovered_node_ = nullptr; 438 | } 439 | 440 | if (ImGui::IsItemClicked()) { 441 | selected_node_ = joint; 442 | } 443 | 444 | // Drag source for joint node 445 | if (ImGui::BeginDragDropSource(ImGuiDragDropFlags_None)) { 446 | ImGui::SetDragDropPayload("JOINT_NODE", &joint, 447 | sizeof(urdf::JointNodePtr *)); 448 | ImGui::Text("Moving %s", joint->joint.name.c_str()); 449 | ImGui::EndDragDropSource(); 450 | } 451 | 452 | // Drop target for link node 453 | if (ImGui::BeginDragDropTarget()) { 454 | if (const ImGuiPayload *payload = 455 | ImGui::AcceptDragDropPayload("LINK_NODE")) { 456 | // void* dropped_link_node_id = 457 | // *static_cast(payload->Data); Handle the drop for 458 | // a link node (update parent/child relationships) 459 | } 460 | ImGui::EndDragDropTarget(); 461 | } 462 | 463 | ImGui::TableNextColumn(); 464 | ImGui::Text("Joint"); 465 | 466 | if (open) { 467 | recursion(joint->child); 468 | ImGui::TreePop(); 469 | } 470 | } 471 | ImGui::TreePop(); 472 | } 473 | }; 474 | 475 | recursion(current_link); 476 | } 477 | 478 | ImGui::EndTable(); 479 | } 480 | } 481 | 482 | void App::drawSideMenu() { 483 | ImGui::SetNextWindowPos(ImVec2(0, ImGui::GetFrameHeight()), ImGuiCond_Always); 484 | ImGui::SetNextWindowSize(ImVec2(350, static_cast(GetScreenHeight()) - 485 | ImGui::GetFrameHeight()), 486 | ImGuiCond_Always); 487 | 488 | ImGui::Begin("Robot Tree", nullptr, 489 | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | 490 | ImGuiWindowFlags_NoTitleBar); 491 | 492 | drawRobotTree(); 493 | ImGui::Separator(); 494 | drawNodeProperties(); 495 | 496 | ImGui::End(); 497 | } 498 | 499 | void App::menuPropertiesInertial(urdf::LinkNodePtr link_node) { 500 | if (auto &inertial = link_node->link.inertial) { 501 | selected_link_origin_ = &inertial->origin; 502 | 503 | ImGui::InputFloat("Mass", &inertial->mass); 504 | 505 | originGui(inertial->origin); 506 | 507 | if (ImGui::TreeNode("Inertia")) { 508 | ImGui::InputFloat("ixx", &inertial->inertia.ixx); 509 | ImGui::InputFloat("iyy", &inertial->inertia.iyy); 510 | ImGui::InputFloat("izz", &inertial->inertia.izz); 511 | ImGui::InputFloat("ixy", &inertial->inertia.ixy); 512 | ImGui::InputFloat("ixz", &inertial->inertia.ixz); 513 | ImGui::InputFloat("iyz", &inertial->inertia.iyz); 514 | ImGui::TreePop(); 515 | } 516 | } else { 517 | selected_link_origin_ = nullptr; 518 | if (ImGui::Button("Create inertial component")) { 519 | command_buffer_.add(std::make_shared( 520 | link_node, selected_link_origin_)); 521 | } 522 | } 523 | } 524 | 525 | void App::menuPropertiesVisual(urdf::LinkNodePtr link_node) { 526 | auto &visual = link_node->link.visual; 527 | if (visual.has_value()) { 528 | if (visual->origin.has_value()) { 529 | selected_link_origin_ = &*visual->origin; 530 | } 531 | menuName(visual->name, "visual"); 532 | menuOrigin(visual->origin); 533 | menuGeometry(visual->geometry, link_node->visual_model); 534 | menuMaterial(visual->material_name); 535 | ImGui::Separator(); 536 | if (ImGui::Button("Delete visual component")) { 537 | command_buffer_.add( 538 | std::make_shared(link_node, robot_)); 539 | } 540 | } else { 541 | selected_link_origin_ = nullptr; 542 | if (ImGui::Button("Create visual component")) { 543 | command_buffer_.add( 544 | std::make_shared(link_node, robot_, shader_)); 545 | } 546 | } 547 | } 548 | 549 | void App::menuPropertiesCollisions(urdf::LinkNodePtr link_node, int i) { 550 | urdf::Collision &col = link_node->link.collision[i]; 551 | 552 | selected_link_origin_ = 553 | col.origin.has_value() ? &col.origin.value() : nullptr; 554 | 555 | menuName(col.name, "collision"); 556 | menuOrigin(col.origin); 557 | menuGeometry(col.geometry, link_node->collision_models[i]); 558 | } 559 | 560 | void App::drawNodeProperties() { 561 | if (not selected_node_ or not robot_) 562 | return; 563 | 564 | if (auto link_node = 565 | std::dynamic_pointer_cast(selected_node_)) { 566 | if (ImGui::BeginTabBar("PropertiesBar", 567 | ImGuiTabBarFlags_FittingPolicyScroll | 568 | ImGuiTabBarFlags_NoCloseWithMiddleMouseButton)) { 569 | if (ImGui::BeginTabItem("Inertial##PropMenuInertial")) { 570 | menuPropertiesInertial(link_node); 571 | ImGui::EndTabItem(); 572 | } 573 | 574 | if (ImGui::BeginTabItem("Visual##PropMenuVisual")) { 575 | menuPropertiesVisual(link_node); 576 | ImGui::EndTabItem(); 577 | } 578 | 579 | if (ImGui::TabItemButton("+", ImGuiTabItemFlags_Trailing)) { 580 | command_buffer_.add(std::make_shared(link_node)); 581 | } 582 | 583 | // TODO(ramon) fix bug: when changing name, the tab looses focus 584 | for (size_t i = 0; i < link_node->link.collision.size(); ++i) { 585 | bool open = true; 586 | const urdf::Collision &col = link_node->link.collision[i]; 587 | char name_buffer[256]; 588 | if (col.name.has_value()) { 589 | snprintf(name_buffer, 256, "%s##ColTabItem%zu", 590 | col.name.value().c_str(), i); 591 | } else { 592 | snprintf(name_buffer, 256, "Col %zu##ColTabItem%zu", i, i); 593 | } 594 | if (ImGui::BeginTabItem(name_buffer, &open)) { 595 | menuPropertiesCollisions(link_node, i); 596 | ImGui::EndTabItem(); 597 | } 598 | 599 | if (not open) { 600 | command_buffer_.add( 601 | std::make_shared(link_node, i)); 602 | } 603 | } 604 | 605 | ImGui::EndTabBar(); 606 | } 607 | } else if (auto joint_node = 608 | std::dynamic_pointer_cast(selected_node_)) { 609 | ImGui::InputText("Name##joint", &joint_node->joint.name, 610 | ImGuiInputTextFlags_None); 611 | 612 | static const char *joint_types[] = {"revolute", "continuous", "prismatic", 613 | "fixed", "floating", "planar"}; 614 | int choice = joint_node->joint.type; 615 | if (ImGui::Combo("dropdown", &choice, joint_types, 616 | IM_ARRAYSIZE(joint_types), urdf::Joint::kNumJointTypes)) { 617 | joint_node->joint.type = static_cast(choice); 618 | } 619 | 620 | ImGui::Text("Parent link: %s", joint_node->parent->link.name.c_str()); 621 | ImGui::Text("Child link: %s", joint_node->child->link.name.c_str()); 622 | 623 | menuOrigin(joint_node->joint.origin); 624 | if (choice != urdf::Joint::kFixed) { 625 | menuAxis(joint_node->joint.axis); 626 | menuDynamics(joint_node->joint.dynamics); 627 | menuLimit(joint_node->joint.limit); 628 | } 629 | } 630 | } 631 | 632 | void App::originGui(urdf::Origin &origin) { 633 | urdf::Origin old_origin = origin; 634 | 635 | if (ImGui::TreeNode("Origin")) { 636 | if (ImGui::InputFloat3("Position", &origin.xyz.x)) { 637 | command_buffer_.add(std::make_shared( 638 | old_origin, origin, origin, robot_)); 639 | } 640 | 641 | if (ImGui::InputFloat3("Orientation", &origin.rpy.x)) { 642 | command_buffer_.add(std::make_shared( 643 | old_origin, origin, origin, robot_)); 644 | } 645 | 646 | if (ImGui::IsItemClicked() and selected_node_) { 647 | selected_link_origin_ = &origin; 648 | } 649 | 650 | ImGui::TreePop(); 651 | } 652 | } 653 | 654 | void App::menuName(std::optional &name, const char *label) { 655 | if (name) { 656 | ImGui::InputText("Name", &(*name)); 657 | } else { 658 | if (ImGui::Button("Create name")) { 659 | command_buffer_.add(std::make_shared(name)); 660 | } 661 | } 662 | } 663 | 664 | void App::menuOrigin(std::optional &origin) { 665 | if (origin) { 666 | originGui(*origin); 667 | } else { 668 | if (ImGui::Button("Create origin")) { 669 | command_buffer_.add( 670 | std::make_shared(origin, selected_link_origin_)); 671 | } 672 | } 673 | } 674 | 675 | void App::menuMaterial(std::optional &material_name, 676 | const char *label) { 677 | if (material_name) { 678 | ImGui::InputText("Material name", &(*material_name)); 679 | } else { 680 | if (ImGui::Button("Create material")) { 681 | command_buffer_.add(std::make_shared(material_name)); 682 | } 683 | } 684 | } 685 | 686 | void App::menuAxis(std::optional &axis) { 687 | if (axis) { 688 | if (ImGui::TreeNode("Axis")) { 689 | ImGui::InputFloat3("xyz", &axis->xyz.x); 690 | if (ImGui::Button("Delete axis")) { 691 | axis = std::nullopt; 692 | } 693 | ImGui::TreePop(); 694 | } 695 | } else { 696 | if (ImGui::Button("Create axis")) { 697 | command_buffer_.add(std::make_shared(axis)); 698 | } 699 | } 700 | } 701 | 702 | void App::menuDynamics(std::optional &dynamics) { 703 | if (dynamics) { 704 | if (ImGui::TreeNode("Dynamics")) { 705 | ImGui::InputFloat("Damping", &dynamics->damping); 706 | ImGui::InputFloat("Friction", &dynamics->friction); 707 | if (ImGui::Button("Delete dynamics")) { 708 | dynamics = std::nullopt; 709 | } 710 | ImGui::TreePop(); 711 | } 712 | } else { 713 | if (ImGui::Button("Create dynamics")) { 714 | command_buffer_.add(std::make_shared(dynamics)); 715 | } 716 | } 717 | } 718 | 719 | void App::menuLimit(std::optional &limit) { 720 | if (limit) { 721 | if (ImGui::TreeNode("Limit")) { 722 | ImGui::InputFloat("Lower", &limit->lower); 723 | ImGui::InputFloat("Upper", &limit->upper); 724 | ImGui::InputFloat("Effort", &limit->effort); 725 | ImGui::InputFloat("Velocity", &limit->velocity); 726 | if (ImGui::Button("Delete limit")) { 727 | limit = std::nullopt; 728 | } 729 | ImGui::TreePop(); 730 | } 731 | } else { 732 | if (ImGui::Button("Create limit")) { 733 | command_buffer_.add(std::make_shared(limit)); 734 | } 735 | } 736 | } 737 | 738 | void App::menuGeometry(urdf::Geometry& geometry, Model& model) { 739 | if (ImGui::TreeNode("Geometry")) { 740 | if (urdf::GeometryTypePtr &type = geometry.type) { 741 | static const char *geom_types[] = {"Box", "Cylinder", "Sphere", "Mesh"}; 742 | 743 | int choice = 0; 744 | if (std::dynamic_pointer_cast(type)) { 745 | choice = 0; 746 | } else if (std::dynamic_pointer_cast(type)) { 747 | choice = 1; 748 | } else if (std::dynamic_pointer_cast(type)) { 749 | choice = 2; 750 | } else if (std::dynamic_pointer_cast(type)) { 751 | choice = 3; 752 | } 753 | 754 | if (ImGui::Combo("dropdown", &choice, geom_types, 755 | IM_ARRAYSIZE(geom_types), 4)) { 756 | switch (choice) { 757 | case 0: 758 | command_buffer_.add(std::make_shared( 759 | type, std::make_shared(), geometry, model, robot_)); 760 | break; 761 | case 1: 762 | command_buffer_.add(std::make_shared( 763 | type, std::make_shared(), geometry, model, robot_)); 764 | break; 765 | case 2: 766 | command_buffer_.add(std::make_shared( 767 | type, std::make_shared(), geometry, model, robot_)); 768 | break; 769 | case 3: 770 | command_buffer_.add(std::make_shared( 771 | type, std::make_shared(""), geometry, model, robot_)); 772 | break; 773 | default: 774 | LOG_F(ERROR, "Invalid geometry type"); 775 | } 776 | } 777 | 778 | switch (choice) { 779 | case 0: 780 | if (auto box = std::dynamic_pointer_cast(type)) { 781 | static Vector3 old_size = box->size; 782 | if (ImGui::InputFloat3("Size", &box->size.x, "%.3f")) { 783 | command_buffer_.add(std::make_shared( 784 | box, old_size, model, shader_)); 785 | } 786 | } 787 | break; 788 | case 1: 789 | if (auto cylinder = std::dynamic_pointer_cast(type)) { 790 | float old_radius = cylinder->radius; 791 | float old_length = cylinder->length; 792 | ImGui::InputFloat("Radius", &cylinder->radius, 0.01F, 0.1F, "%.3f"); 793 | ImGui::InputFloat("Length", &cylinder->length, 0.01F, 0.1F, "%.3f"); 794 | 795 | if (old_radius != cylinder->radius or 796 | old_length != cylinder->length) { 797 | command_buffer_.add(std::make_shared( 798 | cylinder, cylinder->radius, cylinder->length, model, shader_)); 799 | } 800 | } 801 | break; 802 | case 2: 803 | if (auto sphere = std::dynamic_pointer_cast(type)) { 804 | float old_radius = sphere->radius; 805 | ImGui::InputFloat("Radius", &sphere->radius, 0.01F, 0.1F, "%.3f"); 806 | if (old_radius != sphere->radius) 807 | command_buffer_.add(std::make_shared( 808 | sphere, sphere->radius, model, shader_)); 809 | } 810 | break; 811 | case 3: 812 | if (auto gmesh = std::dynamic_pointer_cast(type)) { 813 | ImGui::Text("Filename: %s", gmesh->filename.c_str()); 814 | ImGui::SameLine(); 815 | if (ImGui::Button("...")) { 816 | NFD::UniquePath out_path; 817 | 818 | // prepare filters for the dialog 819 | nfdfilteritem_t filter_item[2] = {{"Mesh filename", "dae,stl"}}; 820 | 821 | // show the dialog 822 | nfdresult_t result = NFD::OpenDialog(out_path, filter_item, 1); 823 | if (result == NFD_OKAY) { 824 | command_buffer_.add(std::make_shared( 825 | gmesh, out_path.get(), model, shader_)); 826 | } else if (result == NFD_CANCEL) { 827 | LOG_F(INFO, "User pressed cancel."); 828 | } else { 829 | LOG_F(WARNING, "Warn: %s", NFD::GetError()); 830 | } 831 | } 832 | if (ImGui::InputText("Filename", &gmesh->filename, 833 | ImGuiInputTextFlags_EnterReturnsTrue)) { 834 | // TODO(ramon): try to load mesh and update 835 | } 836 | } 837 | break; 838 | default: 839 | LOG_F(ERROR, "Invalid geometry type"); 840 | } 841 | } else { 842 | if (ImGui::Button("Create geometry")) { 843 | geometry.type = std::make_shared(); 844 | model = geometry.type->generateGeometry(); 845 | } 846 | } 847 | ImGui::TreePop(); 848 | } 849 | } 850 | 851 | void App::cleanup() 852 | { 853 | UnloadShader(shader_); 854 | rgizmo_unload(); 855 | rlImGuiShutdown(); // Close rl gui 856 | CloseWindow(); // Close window and OpenGL context 857 | } 858 | -------------------------------------------------------------------------------- /src/command.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | CommandBuffer::CommandBuffer() 11 | : current_command_(0) 12 | { 13 | 14 | } 15 | 16 | void CommandBuffer::add(CommandPtr command) 17 | { 18 | if (command) { 19 | new_commands_.emplace_back(std::move(command)); 20 | } 21 | } 22 | 23 | void CommandBuffer::execute() 24 | { 25 | if (new_commands_.empty()) return; 26 | 27 | // Delete all commands that were executed after the current command 28 | // This will only happen after undoing some commands 29 | if (executed_commands_.begin() + current_command_ != executed_commands_.end()) { 30 | executed_commands_.erase(executed_commands_.begin() + current_command_, 31 | executed_commands_.end()); 32 | } 33 | 34 | for (const auto& command : new_commands_) { 35 | command->execute(); 36 | executed_commands_.push_back(command); 37 | } 38 | 39 | current_command_ = executed_commands_.size(); 40 | new_commands_.clear(); 41 | } 42 | 43 | void CommandBuffer::undo() 44 | { 45 | if (executed_commands_.begin() + current_command_ == executed_commands_.begin()) return; 46 | 47 | (*(executed_commands_.begin() + current_command_ - 1))->undo(); 48 | current_command_--; 49 | } 50 | 51 | void CommandBuffer::redo() 52 | { 53 | if (executed_commands_.begin() + current_command_ == executed_commands_.end()) return; 54 | 55 | (*(executed_commands_.begin() + current_command_))->execute(); 56 | current_command_++; 57 | } 58 | 59 | bool CommandBuffer::canUndo() 60 | { 61 | return executed_commands_.begin() + current_command_ != executed_commands_.begin(); 62 | } 63 | 64 | bool CommandBuffer::canRedo() 65 | { 66 | return executed_commands_.begin() + current_command_ != executed_commands_.end(); 67 | } 68 | 69 | CreateRobotCommand::CreateRobotCommand(std::shared_ptr& robot, 70 | const Shader& shader) 71 | : robot_(robot), shader_(shader) 72 | { 73 | 74 | } 75 | 76 | void CreateRobotCommand::execute() 77 | { 78 | old_robot_ = robot_; 79 | urdf::LinkNodePtr root = std::make_shared(urdf::Link("root"), nullptr); 80 | robot_ = std::make_shared(root); 81 | robot_->setShader(shader_); 82 | } 83 | 84 | void CreateRobotCommand::undo() 85 | { 86 | robot_ = old_robot_; 87 | } 88 | 89 | LoadRobotCommand::LoadRobotCommand(std::string filename, 90 | std::shared_ptr& robot, 91 | Shader& shader) 92 | : filename_(std::move(filename)), robot_(robot), shader_(shader) 93 | { 94 | 95 | } 96 | 97 | void LoadRobotCommand::execute() 98 | { 99 | if (future_robot_) { 100 | old_robot_ = robot_; 101 | robot_ = future_robot_; 102 | } else { 103 | old_robot_ = robot_; 104 | robot_ = urdf::buildRobot(filename_.c_str()); 105 | robot_->buildGeometry(); 106 | robot_->forwardKinematics(); 107 | robot_->setShader(shader_); 108 | } 109 | } 110 | 111 | void LoadRobotCommand::undo() 112 | { 113 | future_robot_ = robot_; 114 | robot_ = old_robot_; 115 | } 116 | 117 | JointChangeParentCommand::JointChangeParentCommand(const urdf::JointNodePtr& node, 118 | const urdf::LinkNodePtr& new_parent, 119 | const urdf::RobotPtr& robot) 120 | : joint_(node), new_parent_(new_parent), old_parent_(node->parent), robot_(robot) 121 | { 122 | 123 | } 124 | 125 | void JointChangeParentCommand::execute() 126 | { 127 | old_position_ = std::find(joint_->parent->children.begin(), 128 | joint_->parent->children.end(), 129 | joint_); 130 | joint_->parent->children.erase(old_position_); 131 | new_parent_->children.push_back(joint_); 132 | 133 | joint_->parent = new_parent_; 134 | joint_->joint.parent = new_parent_->link.name; 135 | robot_->forwardKinematics(); 136 | } 137 | 138 | void JointChangeParentCommand::undo() 139 | { 140 | joint_->parent->children.erase(std::find(joint_->parent->children.begin(), 141 | joint_->parent->children.end(), 142 | joint_)); 143 | old_parent_->children.insert(old_position_, joint_); 144 | 145 | joint_->parent = old_parent_; 146 | joint_->joint.parent = old_parent_->link.name; 147 | robot_->forwardKinematics(); 148 | } 149 | 150 | CreateJointCommand::CreateJointCommand(const char *joint_name, 151 | urdf::LinkNodePtr& parent, 152 | urdf::RobotPtr& robot) 153 | : joint_name_(joint_name), parent_(parent), robot_(robot) 154 | { 155 | 156 | } 157 | 158 | void CreateJointCommand::execute() 159 | { 160 | // Get number of overlapping link and joint names 161 | int new_link_count = 0; 162 | int new_joint_count = 0; 163 | std::deque deq { robot_->getRoot() }; 164 | while (not deq.empty()) { 165 | const auto& current_link = deq.front(); 166 | deq.pop_front(); 167 | 168 | new_link_count += static_cast(current_link->link.name.find("New link") != std::string::npos); 169 | 170 | for (const auto& joint : current_link->children) { 171 | new_joint_count += static_cast(joint->joint.name.find(joint_name_) != std::string::npos); 172 | deq.push_back(joint->child); 173 | } 174 | } 175 | 176 | // Rename in case of name collisions 177 | if (new_joint_count > 0) joint_name_ += fmt::format(" {}", new_joint_count); 178 | 179 | std::string new_link_name = (new_link_count == 0) ? "New link" : fmt::format("New link {}", new_link_count); 180 | urdf::LinkNodePtr child = std::make_shared(urdf::Link{ new_link_name }, nullptr); 181 | 182 | // Create new joint and link 183 | urdf::JointNodePtr new_joint = std::make_shared(urdf::JointNode { 184 | urdf::Joint(joint_name_.c_str(), parent_->link.name.c_str(), new_link_name.c_str(), "fixed"), 185 | parent_, 186 | child}); 187 | 188 | child->parent = new_joint; 189 | 190 | parent_->children.push_back(new_joint); 191 | } 192 | 193 | void CreateJointCommand::undo() 194 | { 195 | parent_->children.erase(std::find(parent_->children.begin(), 196 | parent_->children.end(), 197 | new_joint_)); 198 | new_joint_.reset(); 199 | } 200 | 201 | ChangeNameCommand::ChangeNameCommand(const std::string& old_name, 202 | const std::string& new_name, 203 | std::string& target) 204 | : old_name_(old_name), new_name_(new_name), target_(target) 205 | { 206 | 207 | } 208 | 209 | void ChangeNameCommand::execute() 210 | { 211 | target_ = new_name_; 212 | } 213 | 214 | void ChangeNameCommand::undo() 215 | { 216 | target_ = old_name_; 217 | } 218 | 219 | CreateNameCommand::CreateNameCommand(std::optional& target) 220 | : target_(target) 221 | { 222 | 223 | } 224 | 225 | void CreateNameCommand::execute() 226 | { 227 | target_ = std::string(); 228 | } 229 | 230 | void CreateNameCommand::undo() 231 | { 232 | target_ = std::nullopt; 233 | } 234 | 235 | CreateOriginCommand::CreateOriginCommand(std::optional& target, 236 | urdf::OriginRawPtr& selected_origin) 237 | : target_(target), selected_origin_(selected_origin) 238 | { 239 | 240 | } 241 | 242 | void CreateOriginCommand::execute() 243 | { 244 | target_ = urdf::Origin(); 245 | selected_origin_ = &*target_; 246 | } 247 | 248 | void CreateOriginCommand::undo() 249 | { 250 | target_ = std::nullopt; 251 | selected_origin_ = nullptr; 252 | } 253 | 254 | UpdateOriginCommand::UpdateOriginCommand(urdf::Origin& old_origin, 255 | urdf::Origin& new_origin, 256 | urdf::Origin& target, 257 | urdf::RobotPtr& robot) 258 | : old_origin_(old_origin), new_origin_(new_origin), target_(target), robot_(robot) 259 | { 260 | 261 | } 262 | 263 | void UpdateOriginCommand::execute() 264 | { 265 | target_ = new_origin_; 266 | robot_->forwardKinematics(); 267 | } 268 | 269 | void UpdateOriginCommand::undo() 270 | { 271 | target_ = old_origin_; 272 | robot_->forwardKinematics(); 273 | } 274 | 275 | CreateAxisCommand::CreateAxisCommand(std::optional& target) 276 | : target_(target) 277 | { 278 | 279 | } 280 | 281 | void CreateAxisCommand::execute() 282 | { 283 | target_ = urdf::Axis(); 284 | } 285 | 286 | void CreateAxisCommand::undo() 287 | { 288 | target_ = std::nullopt; 289 | } 290 | 291 | CreateDynamicsCommand::CreateDynamicsCommand(std::optional& target) 292 | : target_(target) 293 | { 294 | 295 | } 296 | 297 | void CreateDynamicsCommand::execute() 298 | { 299 | target_ = urdf::Dynamics(); 300 | } 301 | 302 | void CreateDynamicsCommand::undo() 303 | { 304 | target_ = std::nullopt; 305 | } 306 | 307 | CreateLimitCommand::CreateLimitCommand(std::optional& target) 308 | : target_(target) 309 | { 310 | 311 | } 312 | 313 | void CreateLimitCommand::execute() 314 | { 315 | target_ = urdf::Limit(0.0F, 1.0F, 1.0F, 1.0F); 316 | } 317 | 318 | void CreateLimitCommand::undo() 319 | { 320 | target_ = std::nullopt; 321 | } 322 | 323 | ChangeGeometryCommand::ChangeGeometryCommand(urdf::GeometryTypePtr old_geometry, 324 | urdf::GeometryTypePtr new_geometry, 325 | urdf::Geometry& target, 326 | Model& model, 327 | urdf::RobotPtr& robot) 328 | : old_geometry_(std::move(old_geometry)), new_geometry_(std::move(new_geometry)), target_(target), model_(model), robot_(robot) 329 | { 330 | 331 | } 332 | 333 | void ChangeGeometryCommand::execute() 334 | { 335 | target_.type = new_geometry_; 336 | 337 | UnloadModel(model_); 338 | model_ = target_.type->generateGeometry(); 339 | // mesh_ = target_.type->generateGeometry(); 340 | // model_.meshes[0] = mesh_; // TODO: might need to update material 341 | 342 | robot_->forwardKinematics(); 343 | } 344 | 345 | void ChangeGeometryCommand::undo() 346 | { 347 | target_.type = old_geometry_; 348 | 349 | UnloadModel(model_); 350 | model_ = target_.type->generateGeometry(); 351 | // model_.meshes[0] = mesh_; 352 | 353 | robot_->forwardKinematics(); 354 | } 355 | 356 | CreateInertialCommand::CreateInertialCommand(urdf::LinkNodePtr& link, 357 | urdf::OriginRawPtr& selected_origin) 358 | : link_(link), selected_origin_(selected_origin) 359 | { 360 | 361 | } 362 | 363 | void CreateInertialCommand::execute() 364 | { 365 | link_->link.inertial = urdf::Inertial(); 366 | selected_origin_ = &link_->link.inertial->origin; 367 | } 368 | 369 | void CreateInertialCommand::undo() 370 | { 371 | link_->link.inertial = std::nullopt; 372 | selected_origin_ = nullptr; 373 | } 374 | 375 | CreateVisualCommand::CreateVisualCommand(urdf::LinkNodePtr& link, 376 | const urdf::RobotPtr& robot, 377 | const Shader& shader) 378 | : link_(link), robot_(robot), shader_(shader) 379 | { 380 | 381 | } 382 | 383 | void CreateVisualCommand::execute() 384 | { 385 | link_->link.visual = urdf::Visual{ 386 | std::nullopt, std::nullopt, 387 | urdf::Geometry{std::make_shared()}, 388 | std::nullopt}; 389 | link_->visual_model = link_->link.visual->geometry.type->generateGeometry(); 390 | link_->visual_model.materials[0].shader = shader_; 391 | robot_->forwardKinematics(); 392 | } 393 | 394 | void CreateVisualCommand::undo() 395 | { 396 | UnloadModel(link_->visual_model); 397 | link_->link.visual = std::nullopt; 398 | } 399 | 400 | DeleteVisualCommand::DeleteVisualCommand(urdf::LinkNodePtr& link, urdf::RobotPtr& robot) 401 | : link_(link), robot_(robot) 402 | { 403 | 404 | } 405 | 406 | void DeleteVisualCommand::execute() 407 | { 408 | old_visual_ = *link_->link.visual; 409 | shader_ = link_->visual_model.materials[0].shader; 410 | 411 | UnloadModel(link_->visual_model); 412 | link_->link.visual = std::nullopt; 413 | } 414 | 415 | void DeleteVisualCommand::undo() 416 | { 417 | link_->link.visual = old_visual_; 418 | old_visual_ = urdf::Visual(); 419 | UnloadModel(link_->visual_model); 420 | link_->visual_model = link_->link.visual->geometry.type->generateGeometry(); 421 | link_->visual_model.materials[0].shader = shader_; 422 | robot_->forwardKinematics(); 423 | robot_->updateMaterial(link_); 424 | } 425 | 426 | AddCollisionCommand::AddCollisionCommand(urdf::LinkNodePtr& link) 427 | : link_(link) 428 | { 429 | 430 | } 431 | 432 | void AddCollisionCommand::execute() 433 | { 434 | link_->addCollision(); 435 | if (link_->parent and link_->parent->parent) { 436 | urdf::Robot::forwardKinematics(link_->parent->parent); 437 | } 438 | } 439 | 440 | void AddCollisionCommand::undo() 441 | { 442 | link_->deleteCollision(link_->link.collision.size() - 1); 443 | } 444 | 445 | DeleteCollisionCommand::DeleteCollisionCommand(urdf::LinkNodePtr& link, int i) 446 | : link_(link), i_(i) 447 | { 448 | 449 | } 450 | 451 | void DeleteCollisionCommand::execute() 452 | { 453 | old_collision_ = link_->link.collision[i_]; 454 | link_->deleteCollision(i_); 455 | } 456 | 457 | void DeleteCollisionCommand::undo() 458 | { 459 | link_->link.collision.insert(link_->link.collision.begin() + i_, old_collision_); 460 | link_->collision_models.insert(link_->collision_models.begin() + i_, 461 | old_collision_.geometry.type->generateGeometry()); 462 | } 463 | 464 | UpdateGeometryBoxCommand::UpdateGeometryBoxCommand(std::shared_ptr& box, 465 | const Vector3& old_size, 466 | Model& model, 467 | const Shader& shader) 468 | : new_size_(box->size), old_size_(old_size), box_(box), model_(model), shader_(shader) 469 | { 470 | 471 | } 472 | 473 | void UpdateGeometryBoxCommand::execute() 474 | { 475 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 476 | const Matrix t = model_.transform; 477 | box_->size = new_size_; 478 | 479 | UnloadModel(model_); 480 | model_ = Model{}; 481 | model_ = box_->generateGeometry(); 482 | 483 | model_.materials[0].shader = shader_; 484 | model_.transform = t; 485 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 486 | } 487 | 488 | void UpdateGeometryBoxCommand::undo() 489 | { 490 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 491 | const Matrix t = model_.transform; 492 | box_->size = old_size_; 493 | 494 | UnloadModel(model_); 495 | model_ = Model{}; 496 | model_ = box_->generateGeometry(); 497 | 498 | model_.materials[0].shader = shader_; 499 | model_.transform = t; 500 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 501 | } 502 | 503 | UpdateGeometryCylinderCommand::UpdateGeometryCylinderCommand(std::shared_ptr& cylinder, 504 | const float old_radius, 505 | const float old_height, 506 | Model& model, 507 | const Shader& shader) 508 | : new_radius_(cylinder->radius), new_length_(cylinder->length), old_radius_(old_radius), old_length_(old_height), 509 | cylinder_(cylinder), model_(model), shader_(shader) 510 | { 511 | 512 | } 513 | 514 | void UpdateGeometryCylinderCommand::execute() 515 | { 516 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 517 | const Matrix t = model_.transform; 518 | cylinder_->radius = new_radius_; 519 | cylinder_->length = new_length_; 520 | 521 | UnloadModel(model_); 522 | model_ = Model{}; 523 | model_ = cylinder_->generateGeometry(); 524 | 525 | model_.materials[0].shader = shader_; 526 | model_.transform = t; 527 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 528 | } 529 | 530 | void UpdateGeometryCylinderCommand::undo() 531 | { 532 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 533 | const Matrix t = model_.transform; 534 | cylinder_->radius = old_radius_; 535 | cylinder_->length = old_length_; 536 | 537 | UnloadModel(model_); 538 | model_ = Model{}; 539 | model_ = cylinder_->generateGeometry(); 540 | 541 | model_.materials[0].shader = shader_; 542 | model_.transform = t; 543 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 544 | } 545 | 546 | UpdateGeometrySphereCommand::UpdateGeometrySphereCommand(std::shared_ptr& sphere, 547 | const float old_radius, 548 | Model& model, 549 | const Shader& shader) 550 | : new_radius_(sphere->radius), old_radius_(old_radius), sphere_(sphere), model_(model), shader_(shader) 551 | { 552 | 553 | } 554 | 555 | void UpdateGeometrySphereCommand::execute() 556 | { 557 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 558 | const Matrix t = model_.transform; 559 | sphere_->radius = new_radius_; 560 | 561 | UnloadModel(model_); 562 | model_ = Model{}; 563 | model_ = sphere_->generateGeometry(); 564 | 565 | model_.materials[0].shader = shader_; 566 | model_.transform = t; 567 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 568 | } 569 | 570 | void UpdateGeometrySphereCommand::undo() 571 | { 572 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 573 | const Matrix t = model_.transform; 574 | 575 | UnloadModel(model_); 576 | model_ = sphere_->generateGeometry(); 577 | 578 | model_.materials[0].shader = shader_; 579 | model_.transform = t; 580 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 581 | } 582 | 583 | UpdateGeometryMeshCommand::UpdateGeometryMeshCommand(std::shared_ptr& mesh, 584 | const std::string& new_filename, 585 | Model& model, 586 | const Shader& shader) 587 | : new_filename_(new_filename), mesh_(mesh), model_(model), shader_(shader) 588 | { 589 | 590 | } 591 | 592 | void UpdateGeometryMeshCommand::execute() 593 | { 594 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 595 | const Matrix t = model_.transform; 596 | old_filename_ = mesh_->filename; 597 | mesh_->filename = new_filename_; 598 | 599 | UnloadModel(model_); 600 | model_ = Model{}; 601 | if (std::filesystem::exists(mesh_->filename)) { 602 | model_ = mesh_->generateGeometry(); 603 | } 604 | 605 | model_.materials[0].shader = shader_; 606 | model_.transform = t; 607 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 608 | } 609 | 610 | void UpdateGeometryMeshCommand::undo() 611 | { 612 | MaterialMap mat_map = model_.materials[0].maps[MATERIAL_MAP_DIFFUSE]; 613 | const Matrix t = model_.transform; 614 | mesh_->filename = old_filename_; 615 | 616 | UnloadModel(model_); 617 | model_ = mesh_->generateGeometry(); 618 | 619 | model_.materials[0].shader = shader_; 620 | model_.transform = t; 621 | model_.materials[0].maps[MATERIAL_MAP_DIFFUSE] = mat_map; 622 | } 623 | 624 | -------------------------------------------------------------------------------- /src/import_mesh.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | ::Mesh GenMeshCenteredCylinder(float radius, float height, int slices) 10 | { 11 | ::Mesh mesh = { 0 }; 12 | 13 | if (slices >= 3) 14 | { 15 | // Create a cylinder along Y-axis 16 | par_shapes_mesh* cylinder = par_shapes_create_cylinder(slices, 8); 17 | par_shapes_scale(cylinder, radius, radius, height); 18 | 19 | // Translate the cylinder to center it at the origin 20 | par_shapes_translate(cylinder, 0, 0, -height / 2.0F); 21 | 22 | // Create top cap 23 | float center_top[] = { 0, 0, height / 2 }; 24 | float normal_top[] = { 0, 0, 1 }; 25 | par_shapes_mesh* cap_top = par_shapes_create_disk(radius, slices, center_top, normal_top); 26 | cap_top->tcoords = PAR_MALLOC(float, 2 * cap_top->npoints); 27 | for (int i = 0; i < 2 * cap_top->npoints; i++) cap_top->tcoords[i] = 0.0F; 28 | 29 | // Create bottom cap 30 | float center_bot[] = { 0, 0, 0 }; 31 | float normal_bot[] = { 0, 0, 1 }; 32 | par_shapes_mesh* cap_bottom = par_shapes_create_disk(radius, slices, center_bot, normal_bot); 33 | float x_vec[] = {1, 0, 0}; 34 | par_shapes_rotate(cap_bottom, -PI, x_vec); 35 | par_shapes_translate(cap_bottom, 0, 0, -height / 2.0F); 36 | cap_bottom->tcoords = PAR_MALLOC(float, 2 * cap_bottom->npoints); 37 | for (int i = 0; i < 2 * cap_bottom->npoints; i++) cap_bottom->tcoords[i] = 0.95F; 38 | 39 | // Merge cylinder and caps 40 | par_shapes_merge_and_free(cylinder, cap_top); 41 | par_shapes_merge_and_free(cylinder, cap_bottom); 42 | 43 | 44 | mesh.vertices = (float *)RL_MALLOC(cylinder->ntriangles*3*3*sizeof(float)); 45 | mesh.texcoords = (float *)RL_MALLOC(cylinder->ntriangles*3*2*sizeof(float)); 46 | mesh.normals = (float *)RL_MALLOC(cylinder->ntriangles*3*3*sizeof(float)); 47 | 48 | mesh.vertexCount = cylinder->ntriangles*3; 49 | mesh.triangleCount = cylinder->ntriangles; 50 | 51 | for (int k = 0; k < mesh.vertexCount; k++) 52 | { 53 | mesh.vertices[k*3] = cylinder->points[cylinder->triangles[k]*3]; 54 | mesh.vertices[k*3 + 1] = cylinder->points[cylinder->triangles[k]*3 + 1]; 55 | mesh.vertices[k*3 + 2] = cylinder->points[cylinder->triangles[k]*3 + 2]; 56 | 57 | mesh.normals[k*3] = cylinder->normals[cylinder->triangles[k]*3]; 58 | mesh.normals[k*3 + 1] = cylinder->normals[cylinder->triangles[k]*3 + 1]; 59 | mesh.normals[k*3 + 2] = cylinder->normals[cylinder->triangles[k]*3 + 2]; 60 | 61 | mesh.texcoords[k*2] = cylinder->tcoords[cylinder->triangles[k]*2]; 62 | mesh.texcoords[k*2 + 1] = cylinder->tcoords[cylinder->triangles[k]*2 + 1]; 63 | } 64 | 65 | par_shapes_free_mesh(cylinder); 66 | 67 | // Upload vertex data to GPU (static mesh) 68 | UploadMesh(&mesh, false); 69 | } 70 | else LOG_F(WARNING, "MESH: Failed to generate mesh: cylinder"); 71 | 72 | return mesh; 73 | } 74 | 75 | 76 | 77 | // Function to create a raylib Mesh from an Assimp mesh 78 | Mesh LoadMeshFromAssimp(const aiMesh* mesh) { 79 | Mesh result = { 0 }; 80 | 81 | // Allocate vertex and index buffers 82 | result.vertexCount = mesh->mNumVertices; 83 | result.vertices = (float*)RL_MALLOC(result.vertexCount * 3 * sizeof(float)); // Assuming 3D vertices 84 | result.triangleCount = mesh->mNumFaces; 85 | result.indices = (unsigned short*)RL_MALLOC(result.triangleCount * 3 * sizeof(unsigned short)); // Assuming triangles 86 | 87 | // Copy vertex positions 88 | memcpy(result.vertices, mesh->mVertices, result.vertexCount * 3 * sizeof(float)); 89 | 90 | // Copy indices 91 | for (unsigned int i = 0; i < mesh->mNumFaces; i++) { 92 | const aiFace* face = &mesh->mFaces[i]; 93 | memcpy(&result.indices[i * 3], face->mIndices, 3 * sizeof(unsigned int)); 94 | } 95 | 96 | return result; 97 | } 98 | 99 | // Function to load all meshes from a Collada file 100 | std::vector LoadColladaMeshes(const std::string& filename) { 101 | Assimp::Importer importer; 102 | const aiScene* scene = importer.ReadFile(filename, aiProcess_Triangulate); 103 | 104 | if (!scene || scene->mFlags & AI_SCENE_FLAGS_INCOMPLETE || !scene->mRootNode) { 105 | LOG_F(WARNING, "ERROR: %s", importer.GetErrorString()); 106 | return std::vector(1, Mesh()); 107 | } 108 | 109 | std::vector meshes; 110 | for (unsigned int i = 0; i < scene->mNumMeshes; i++) { 111 | meshes.push_back(LoadMeshFromAssimp(scene->mMeshes[i])); 112 | } 113 | 114 | importer.FreeScene(); 115 | return meshes; 116 | } 117 | 118 | Model LoadModelFromCollada(const std::string& filename) { 119 | Model model = {}; 120 | std::vector meshes = LoadColladaMeshes(filename); 121 | model.meshCount = meshes.size(); 122 | model.meshes = (Mesh*)RL_MALLOC(model.meshCount * sizeof(Mesh)); 123 | for (size_t i = 0; i < meshes.size(); i++) { 124 | model.meshes[i] = meshes[i]; 125 | } 126 | return model; 127 | } 128 | -------------------------------------------------------------------------------- /src/main.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | auto main (int argc, char* argv[]) -> int 4 | { 5 | App app(argc, argv); 6 | app.run(); 7 | } 8 | -------------------------------------------------------------------------------- /src/raygizmo.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "raygizmo.h" 3 | #include "raylib.h" 4 | #include "raymath.h" 5 | #include "rlgl.h" 6 | #include 7 | #include 8 | 9 | #if defined(PLATFORM_DESKTOP) // Shaders for PLATFORM_DESKTOP 10 | static const char *SHADER_VERT = "\ 11 | #version 330 core\n\ 12 | in vec3 vertexPosition; \ 13 | in vec4 vertexColor; \ 14 | out vec4 fragColor; \ 15 | out vec3 fragPosition; \ 16 | uniform mat4 mvp; \ 17 | void main() \ 18 | { \ 19 | fragColor = vertexColor; \ 20 | fragPosition = vertexPosition; \ 21 | gl_Position = mvp * vec4(vertexPosition, 1.0); \ 22 | } \ 23 | "; 24 | 25 | static const char *SHADER_FRAG = "\ 26 | #version 330 core\n\ 27 | in vec4 fragColor; \ 28 | in vec3 fragPosition; \ 29 | uniform vec3 cameraPosition; \ 30 | uniform vec3 gizmoPosition; \ 31 | out vec4 finalColor; \ 32 | void main() \ 33 | { \ 34 | vec3 r = normalize(fragPosition - gizmoPosition); \ 35 | vec3 c = normalize(fragPosition - cameraPosition); \ 36 | if (dot(r, c) > 0.1) discard; \ 37 | finalColor = fragColor; \ 38 | } \ 39 | "; 40 | 41 | #else // Shaders for PLATFORM_ANDROID, PLATFORM_WEB 42 | 43 | static const char *SHADER_VERT = "\ 44 | #version 100\n\ 45 | attribute vec3 vertexPosition; \ 46 | attribute vec4 vertexColor; \ 47 | varying vec4 fragColor; \ 48 | varying vec3 fragPosition; \ 49 | uniform mat4 mvp; \ 50 | void main() \ 51 | { \ 52 | fragColor = vertexColor; \ 53 | fragPosition = vertexPosition; \ 54 | gl_Position = mvp * vec4(vertexPosition, 1.0); \ 55 | } \ 56 | "; 57 | 58 | static const char *SHADER_FRAG = "\ 59 | #version 100\n\ 60 | precision mediump float; \ 61 | varying vec4 fragColor; \ 62 | varying vec3 fragPosition; \ 63 | uniform vec3 cameraPosition; \ 64 | uniform vec3 gizmoPosition; \ 65 | void main() { \ 66 | vec3 r = normalize(fragPosition - gizmoPosition); \ 67 | vec3 c = normalize(fragPosition - cameraPosition); \ 68 | if (dot(r, c) > 0.1) discard; \ 69 | gl_FragColor = fragColor; \ 70 | } \ 71 | "; 72 | #endif 73 | 74 | #define PICKING_FBO_WIDTH 512 75 | #define PICKING_FBO_HEIGHT 512 76 | 77 | #define X_AXIS \ 78 | (Vector3) { 1.0, 0.0, 0.0 } 79 | #define Y_AXIS \ 80 | (Vector3) { 0.0, 1.0, 0.0 } 81 | #define Z_AXIS \ 82 | (Vector3) { 0.0, 0.0, 1.0 } 83 | 84 | #define SWAP(x, y) \ 85 | do { \ 86 | unsigned char \ 87 | swap_temp[sizeof(x) == sizeof(y) ? (signed)sizeof(x) : -1]; \ 88 | memcpy(swap_temp, &y, sizeof(x)); \ 89 | memcpy(&y, &x, sizeof(x)); \ 90 | memcpy(&x, swap_temp, sizeof(x)); \ 91 | } while (0) 92 | 93 | static bool IS_LOADED = false; 94 | static Shader SHADER; 95 | static int SHADER_CAMERA_POSITION_LOC; 96 | static int SHADER_GIZMO_POSITION_LOC; 97 | 98 | static unsigned int PICKING_FBO; 99 | static unsigned int PICKING_TEXTURE; 100 | 101 | typedef enum HandleId { 102 | HANDLE_X, 103 | 104 | ROT_HANDLE_X, 105 | AXIS_HANDLE_X, 106 | PLANE_HANDLE_X, 107 | 108 | HANDLE_Y, 109 | 110 | ROT_HANDLE_Y, 111 | AXIS_HANDLE_Y, 112 | PLANE_HANDLE_Y, 113 | 114 | HANDLE_Z, 115 | 116 | ROT_HANDLE_Z, 117 | AXIS_HANDLE_Z, 118 | PLANE_HANDLE_Z 119 | } HandleId; 120 | 121 | typedef struct Handle { 122 | Vector3 position; 123 | Vector3 axis; 124 | Color color; 125 | float distToCamera; 126 | } Handle; 127 | 128 | typedef struct XYZColors { 129 | Color x; 130 | Color y; 131 | Color z; 132 | } XYZColors; 133 | 134 | typedef struct HandleColors { 135 | XYZColors rot; 136 | XYZColors axis; 137 | XYZColors plane; 138 | } HandleColors; 139 | 140 | typedef struct Handles { 141 | Handle arr[3]; 142 | } Handles; 143 | 144 | static Handles sort_handles(Handle h0, Handle h1, Handle h2) { 145 | if (h0.distToCamera < h1.distToCamera) SWAP(h0, h1); 146 | if (h1.distToCamera < h2.distToCamera) SWAP(h1, h2); 147 | if (h0.distToCamera < h1.distToCamera) SWAP(h0, h1); 148 | 149 | Handles handles = {.arr = {h0, h1, h2}}; 150 | return handles; 151 | } 152 | 153 | static XYZColors get_xyz_colors(Vector3 current_axis, bool is_hot) { 154 | Color x = is_hot && current_axis.x == 1.0f ? WHITE : RED; 155 | Color y = is_hot && current_axis.y == 1.0f ? WHITE : GREEN; 156 | Color z = is_hot && current_axis.z == 1.0f ? WHITE : BLUE; 157 | XYZColors colors = {x, y, z}; 158 | return colors; 159 | } 160 | 161 | static void draw_gizmo( 162 | RGizmo gizmo, Camera3D camera, Vector3 position, HandleColors colors 163 | ) { 164 | float radius = gizmo.view.size * Vector3Distance(camera.position, position); 165 | 166 | BeginMode3D(camera); 167 | float previous_line_width = rlGetLineWidth(); 168 | rlSetLineWidth(gizmo.view.handle_draw_thickness); 169 | rlDisableDepthTest(); 170 | 171 | // --------------------------------------------------------------- 172 | // Draw plane handles 173 | { 174 | float offset = radius * gizmo.view.plane_handle_offset; 175 | float size = radius * gizmo.view.plane_handle_size; 176 | 177 | Vector3 px = Vector3Add(position, (Vector3){0.0f, offset, offset}); 178 | Vector3 py = Vector3Add(position, (Vector3){offset, 0.0f, offset}); 179 | Vector3 pz = Vector3Add(position, (Vector3){offset, offset, 0.0f}); 180 | 181 | Handle hx = { 182 | px, 183 | Z_AXIS, 184 | colors.plane.x, 185 | Vector3DistanceSqr(px, camera.position)}; 186 | Handle hy = { 187 | py, 188 | Y_AXIS, 189 | colors.plane.y, 190 | Vector3DistanceSqr(py, camera.position)}; 191 | Handle hz = { 192 | pz, 193 | X_AXIS, 194 | colors.plane.z, 195 | Vector3DistanceSqr(pz, camera.position)}; 196 | Handles handles = sort_handles(hx, hy, hz); 197 | 198 | rlDisableBackfaceCulling(); 199 | for (int i = 0; i < 3; ++i) { 200 | Handle *h = &handles.arr[i]; 201 | rlPushMatrix(); 202 | rlTranslatef(h->position.x, h->position.y, h->position.z); 203 | rlRotatef(90.0f, h->axis.x, h->axis.y, h->axis.z); 204 | DrawPlane( 205 | Vector3Zero(), Vector2Scale(Vector2One(), size), h->color 206 | ); 207 | rlPopMatrix(); 208 | } 209 | } 210 | 211 | // --------------------------------------------------------------- 212 | // Draw rotation handles 213 | { 214 | BeginShaderMode(SHADER); 215 | SetShaderValue( 216 | SHADER, 217 | SHADER_CAMERA_POSITION_LOC, 218 | &camera.position, 219 | SHADER_UNIFORM_VEC3 220 | ); 221 | SetShaderValue( 222 | SHADER, SHADER_GIZMO_POSITION_LOC, &position, SHADER_UNIFORM_VEC3 223 | ); 224 | DrawCircle3D(position, radius, Y_AXIS, 90.0f, colors.rot.x); 225 | DrawCircle3D(position, radius, X_AXIS, 90.0f, colors.rot.y); 226 | DrawCircle3D(position, radius, X_AXIS, 0.0f, colors.rot.z); 227 | EndShaderMode(); 228 | } 229 | 230 | // --------------------------------------------------------------- 231 | // Draw axis handles 232 | { 233 | float length = radius * gizmo.view.axis_handle_length; 234 | float tip_length = radius * gizmo.view.axis_handle_tip_length; 235 | float tip_radius = radius * gizmo.view.axis_handle_tip_radius; 236 | 237 | Vector3 px = Vector3Add(position, Vector3Scale(X_AXIS, length)); 238 | Vector3 py = Vector3Add(position, Vector3Scale(Y_AXIS, length)); 239 | Vector3 pz = Vector3Add(position, Vector3Scale(Z_AXIS, length)); 240 | 241 | Handle hx = { 242 | px, X_AXIS, colors.axis.x, Vector3DistanceSqr(px, camera.position)}; 243 | Handle hy = { 244 | py, Y_AXIS, colors.axis.y, Vector3DistanceSqr(py, camera.position)}; 245 | Handle hz = { 246 | pz, Z_AXIS, colors.axis.z, Vector3DistanceSqr(pz, camera.position)}; 247 | Handles handles = sort_handles(hx, hy, hz); 248 | 249 | for (int i = 0; i < 3; ++i) { 250 | Handle *h = &handles.arr[i]; 251 | Vector3 tip_end = Vector3Add( 252 | h->position, Vector3Scale(h->axis, tip_length) 253 | ); 254 | DrawLine3D(position, h->position, h->color); 255 | DrawCylinderEx( 256 | h->position, tip_end, tip_radius, 0.0f, 16, h->color 257 | ); 258 | } 259 | } 260 | EndMode3D(); 261 | 262 | // --------------------------------------------------------------- 263 | // Draw long white line which represents current active axis 264 | if (gizmo.state == RGIZMO_STATE_ACTIVE_ROT 265 | || gizmo.state == RGIZMO_STATE_ACTIVE_AXIS) { 266 | BeginMode3D(camera); 267 | rlSetLineWidth(gizmo.view.active_axis_draw_thickness); 268 | Vector3 halfAxisLine = Vector3Scale(gizmo.update.axis, 1000.0f); 269 | DrawLine3D( 270 | Vector3Subtract(position, halfAxisLine), 271 | Vector3Add(position, halfAxisLine), 272 | WHITE 273 | ); 274 | EndMode3D(); 275 | } 276 | 277 | // --------------------------------------------------------------- 278 | // Draw white line from the gizmo's center to the mouse cursor when rotating 279 | if (gizmo.state == RGIZMO_STATE_ACTIVE_ROT) { 280 | rlSetLineWidth(gizmo.view.active_axis_draw_thickness); 281 | DrawLineV( 282 | GetWorldToScreen(position, camera), GetMousePosition(), WHITE 283 | ); 284 | } 285 | 286 | rlSetLineWidth(previous_line_width); 287 | } 288 | 289 | static void rgizmo_load(void) { 290 | if (IS_LOADED) { 291 | TraceLog(LOG_WARNING, "RAYGIZMO: Gizmo is already loaded, skip"); 292 | return; 293 | } 294 | 295 | // ------------------------------------------------------------------- 296 | // Load shader 297 | SHADER = LoadShaderFromMemory(SHADER_VERT, SHADER_FRAG); 298 | SHADER_CAMERA_POSITION_LOC = GetShaderLocation(SHADER, "cameraPosition"); 299 | SHADER_GIZMO_POSITION_LOC = GetShaderLocation(SHADER, "gizmoPosition"); 300 | 301 | // ------------------------------------------------------------------- 302 | // Load picking fbo 303 | PICKING_FBO = rlLoadFramebuffer(PICKING_FBO_WIDTH, PICKING_FBO_HEIGHT); 304 | if (!PICKING_FBO) { 305 | TraceLog(LOG_ERROR, "RAYGIZMO: Failed to create picking fbo"); 306 | exit(1); 307 | } 308 | rlEnableFramebuffer(PICKING_FBO); 309 | 310 | PICKING_TEXTURE = rlLoadTexture( 311 | NULL, 312 | PICKING_FBO_WIDTH, 313 | PICKING_FBO_HEIGHT, 314 | RL_PIXELFORMAT_UNCOMPRESSED_R8G8B8A8, 315 | 1 316 | ); 317 | rlActiveDrawBuffers(1); 318 | rlFramebufferAttach( 319 | PICKING_FBO, 320 | PICKING_TEXTURE, 321 | RL_ATTACHMENT_COLOR_CHANNEL0, 322 | RL_ATTACHMENT_TEXTURE2D, 323 | 0 324 | ); 325 | if (!rlFramebufferComplete(PICKING_FBO)) { 326 | TraceLog(LOG_ERROR, "RAYGIZMO: Picking fbo is not complete"); 327 | exit(1); 328 | } 329 | 330 | IS_LOADED = true; 331 | TraceLog(LOG_INFO, "RAYGIZMO: Gizmo loaded"); 332 | } 333 | 334 | void rgizmo_unload(void) { 335 | if (!IS_LOADED) { 336 | TraceLog(LOG_WARNING, "RAYGIZMO: Gizmo is already unloaded, skip"); 337 | return; 338 | } 339 | 340 | UnloadShader(SHADER); 341 | rlUnloadFramebuffer(PICKING_FBO); 342 | rlUnloadTexture(PICKING_TEXTURE); 343 | 344 | IS_LOADED = false; 345 | TraceLog(LOG_INFO, "RAYGIZMO: Gizmo unloaded"); 346 | } 347 | 348 | RGizmo rgizmo_create(void) { 349 | if (!IS_LOADED) rgizmo_load(); 350 | RGizmo gizmo = {0}; 351 | gizmo.view.size = 0.12f; 352 | gizmo.view.handle_draw_thickness = 5.0f; 353 | gizmo.view.active_axis_draw_thickness = 2.0f; 354 | gizmo.view.axis_handle_length = 1.2f; 355 | gizmo.view.axis_handle_tip_length = 0.3f; 356 | gizmo.view.axis_handle_tip_radius = 0.1f; 357 | gizmo.view.plane_handle_offset = 0.4f; 358 | gizmo.view.plane_handle_size = 0.2f; 359 | 360 | return gizmo; 361 | } 362 | 363 | bool rgizmo_update(RGizmo *gizmo, Camera3D camera, Vector3 position) { 364 | bool updated = false; 365 | 366 | if (!IS_LOADED) { 367 | TraceLog(LOG_ERROR, "RAYGIZMO: Gizmo is not loaded"); 368 | exit(1); 369 | } 370 | 371 | // ------------------------------------------------------------------- 372 | // Draw gizmo into the picking fbo for the mouse pixel-picking 373 | rlEnableFramebuffer(PICKING_FBO); 374 | rlViewport(0, 0, PICKING_FBO_WIDTH, PICKING_FBO_HEIGHT); 375 | rlClearColor(0, 0, 0, 0); 376 | rlClearScreenBuffers(); 377 | rlDisableColorBlend(); 378 | 379 | HandleColors colors = { 380 | {(Color){ROT_HANDLE_X, 0, 0, 0}, 381 | (Color){ROT_HANDLE_Y, 0, 0, 0}, 382 | (Color){ROT_HANDLE_Z, 0, 0, 0}}, 383 | {(Color){AXIS_HANDLE_X, 0, 0, 0}, 384 | (Color){AXIS_HANDLE_Y, 0, 0, 0}, 385 | (Color){AXIS_HANDLE_Z, 0, 0, 0}}, 386 | {(Color){PLANE_HANDLE_X, 0, 0, 0}, 387 | (Color){PLANE_HANDLE_Y, 0, 0, 0}, 388 | (Color){PLANE_HANDLE_Z, 0, 0, 0}}}; 389 | 390 | draw_gizmo(*gizmo, camera, position, colors); 391 | 392 | rlDisableFramebuffer(); 393 | rlEnableColorBlend(); 394 | Vector2 dpi = GetWindowScaleDPI(); 395 | rlViewport(0, 0, (int)(GetScreenWidth() * dpi.x), (int)(GetScreenHeight() * dpi.y)); 396 | 397 | // ------------------------------------------------------------------- 398 | // Pick the pixel under the mouse cursor 399 | Vector2 mouse_position = GetMousePosition(); 400 | unsigned char *pixels = (unsigned char *)rlReadTexturePixels( 401 | PICKING_TEXTURE, 402 | PICKING_FBO_WIDTH, 403 | PICKING_FBO_HEIGHT, 404 | RL_PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 405 | ); 406 | 407 | float x_fract = Clamp(mouse_position.x / (float)GetScreenWidth(), 0.0, 1.0); 408 | float y_fract = Clamp( 409 | 1.0 - (mouse_position.y / (float)GetScreenHeight()), 0.0, 1.0 410 | ); 411 | int x = (int)(PICKING_FBO_WIDTH * x_fract); 412 | int y = (int)(PICKING_FBO_HEIGHT * y_fract); 413 | int idx = 4 * (y * PICKING_FBO_WIDTH + x); 414 | unsigned char picked_id = pixels[idx]; 415 | 416 | free(pixels); 417 | 418 | // ------------------------------------------------------------------- 419 | // Update gizmo 420 | gizmo->update.angle = 0.0; 421 | gizmo->update.translation = Vector3Zero(); 422 | 423 | bool is_lmb_down = IsMouseButtonDown(0); 424 | if (!is_lmb_down) gizmo->state = RGIZMO_STATE_COLD; 425 | 426 | if (gizmo->state < RGIZMO_STATE_ACTIVE) { 427 | if (picked_id < HANDLE_Y) gizmo->update.axis = X_AXIS; 428 | else if (picked_id < HANDLE_Z) gizmo->update.axis = Y_AXIS; 429 | else gizmo->update.axis = Z_AXIS; 430 | 431 | if (picked_id % 4 == 1) 432 | gizmo->state = is_lmb_down ? RGIZMO_STATE_ACTIVE_ROT 433 | : RGIZMO_STATE_HOT_ROT; 434 | else if (picked_id % 4 == 2) 435 | gizmo->state = is_lmb_down ? RGIZMO_STATE_ACTIVE_AXIS 436 | : RGIZMO_STATE_HOT_AXIS; 437 | else if (picked_id % 4 == 3) 438 | gizmo->state = is_lmb_down ? RGIZMO_STATE_ACTIVE_PLANE 439 | : RGIZMO_STATE_HOT_PLANE; 440 | } 441 | 442 | Vector2 delta = GetMouseDelta(); 443 | bool is_mouse_moved = (fabs(delta.x) + fabs(delta.y)) > EPSILON; 444 | if (!is_mouse_moved) return updated; 445 | 446 | switch (gizmo->state) { 447 | case RGIZMO_STATE_ACTIVE_ROT: { 448 | Vector2 p1 = Vector2Subtract( 449 | GetMousePosition(), GetWorldToScreen(position, camera) 450 | ); 451 | Vector2 p0 = Vector2Subtract(p1, GetMouseDelta()); 452 | 453 | // Get angle between two vectors: 454 | float angle = 0.0f; 455 | float dot = Vector2DotProduct( 456 | Vector2Normalize(p1), Vector2Normalize(p0) 457 | ); 458 | if (1.0 - fabs(dot) > EPSILON) { 459 | angle = acos(dot); 460 | float z = p1.x * p0.y - p1.y * p0.x; 461 | 462 | if (fabs(z) < EPSILON) angle = 0.0; 463 | else if (z <= 0) angle *= -1.0; 464 | } 465 | 466 | // If we look at the gizmo from behind, we should flip the rotation 467 | if (Vector3DotProduct(gizmo->update.axis, position) 468 | > Vector3DotProduct(gizmo->update.axis, camera.position)) { 469 | angle *= -1; 470 | } 471 | 472 | gizmo->update.angle = angle; 473 | updated = true; 474 | break; 475 | }; 476 | case RGIZMO_STATE_ACTIVE_AXIS: { 477 | Vector2 p = Vector2Add( 478 | GetWorldToScreen(position, camera), GetMouseDelta() 479 | ); 480 | Ray r = GetMouseRay(p, camera); 481 | 482 | // Get two lines nearest point 483 | Vector3 line0point0 = camera.position; 484 | Vector3 line0point1 = Vector3Add(line0point0, r.direction); 485 | Vector3 line1point0 = position; 486 | Vector3 line1point1 = Vector3Add(line1point0, gizmo->update.axis); 487 | Vector3 vec0 = Vector3Subtract(line0point1, line0point0); 488 | Vector3 vec1 = Vector3Subtract(line1point1, line1point0); 489 | Vector3 plane_vec = Vector3Normalize(Vector3CrossProduct(vec0, vec1) 490 | ); 491 | Vector3 plane_normal = Vector3Normalize( 492 | Vector3CrossProduct(vec0, plane_vec) 493 | ); 494 | 495 | // Intersect line and plane 496 | float dot = Vector3DotProduct(plane_normal, vec1); 497 | if (fabs(dot) > EPSILON) { 498 | Vector3 w = Vector3Subtract(line1point0, line0point0); 499 | float k = -Vector3DotProduct(plane_normal, w) / dot; 500 | Vector3 isect = Vector3Add(line1point0, Vector3Scale(vec1, k)); 501 | gizmo->update.translation = Vector3Subtract(isect, position); 502 | } 503 | updated = true; 504 | break; 505 | } 506 | case RGIZMO_STATE_ACTIVE_PLANE: { 507 | Vector2 p = Vector2Add( 508 | GetWorldToScreen(position, camera), GetMouseDelta() 509 | ); 510 | Ray r = GetMouseRay(p, camera); 511 | 512 | // Collide ray and plane 513 | float denominator = r.direction.x * gizmo->update.axis.x 514 | + r.direction.y * gizmo->update.axis.y 515 | + r.direction.z * gizmo->update.axis.z; 516 | 517 | if (fabs(denominator) > EPSILON) { 518 | float t = ((position.x - r.position.x) * gizmo->update.axis.x 519 | + (position.y - r.position.y) * gizmo->update.axis.y 520 | + (position.z - r.position.z) * gizmo->update.axis.z) 521 | / denominator; 522 | 523 | if (t > 0) { 524 | Vector3 c = Vector3Add( 525 | r.position, Vector3Scale(r.direction, t) 526 | ); 527 | gizmo->update.translation = Vector3Subtract(c, position); 528 | } 529 | } 530 | updated = true; 531 | break; 532 | } 533 | default: break; 534 | } 535 | 536 | return updated; 537 | } 538 | 539 | void rgizmo_draw(RGizmo gizmo, Camera3D camera, Vector3 position) { 540 | if (!IS_LOADED) { 541 | TraceLog(LOG_ERROR, "RAYGIZMO: Gizmo is not loaded"); 542 | exit(1); 543 | } 544 | 545 | HandleColors colors; 546 | colors.rot = get_xyz_colors( 547 | gizmo.update.axis, 548 | gizmo.state == RGIZMO_STATE_HOT_ROT 549 | || gizmo.state == RGIZMO_STATE_ACTIVE_ROT 550 | ); 551 | colors.axis = get_xyz_colors( 552 | gizmo.update.axis, 553 | gizmo.state == RGIZMO_STATE_HOT_AXIS 554 | || gizmo.state == RGIZMO_STATE_ACTIVE_AXIS 555 | ); 556 | colors.plane = get_xyz_colors( 557 | gizmo.update.axis, 558 | gizmo.state == RGIZMO_STATE_HOT_PLANE 559 | || gizmo.state == RGIZMO_STATE_ACTIVE_PLANE 560 | ); 561 | 562 | draw_gizmo(gizmo, camera, position, colors); 563 | } 564 | 565 | Matrix rgizmo_get_transform(RGizmo gizmo, Vector3 position) { 566 | Matrix translation = MatrixTranslate( 567 | gizmo.update.translation.x, 568 | gizmo.update.translation.y, 569 | gizmo.update.translation.z 570 | ); 571 | 572 | Matrix rotation = MatrixMultiply( 573 | MatrixMultiply( 574 | MatrixTranslate(-position.x, -position.y, -position.z), 575 | MatrixRotate(gizmo.update.axis, gizmo.update.angle) 576 | ), 577 | MatrixTranslate(position.x, position.y, position.z) 578 | ); 579 | 580 | Matrix transform = MatrixMultiply(translation, rotation); 581 | 582 | return transform; 583 | } 584 | -------------------------------------------------------------------------------- /src/rlImGui.cc: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * 3 | * raylibExtras * Utilities and Shared Components for Raylib 4 | * 5 | * rlImGui * basic ImGui integration 6 | * 7 | * LICENSE: ZLIB 8 | * 9 | * Copyright (c) 2020 Jeffery Myers 10 | * 11 | * Permission is hereby granted, free of charge, to any person obtaining a copy 12 | * of this software and associated documentation files (the "Software"), to deal 13 | * in the Software without restriction, including without limitation the rights 14 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | * copies of the Software, and to permit persons to whom the Software is 16 | * furnished to do so, subject to the following conditions: 17 | * 18 | * The above copyright notice and this permission notice shall be included in all 19 | * copies or substantial portions of the Software. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | * SOFTWARE. 28 | * 29 | **********************************************************************************************/ 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #ifdef PLATFORM_DESKTOP 38 | #include 39 | #endif 40 | 41 | #include 42 | #include 43 | 44 | #ifndef NO_FONT_AWESOME 45 | #include "extras/FA6FreeSolidFontData.h" 46 | #endif 47 | 48 | static ImGuiMouseCursor CurrentMouseCursor = ImGuiMouseCursor_COUNT; 49 | static MouseCursor MouseCursorMap[ImGuiMouseCursor_COUNT]; 50 | 51 | ImGuiContext* GlobalContext = nullptr; 52 | 53 | static std::map RaylibKeyMap; 54 | 55 | static bool LastFrameFocused = false; 56 | 57 | static bool LastControlPressed = false; 58 | static bool LastShiftPressed = false; 59 | static bool LastAltPressed = false; 60 | static bool LastSuperPressed = false; 61 | 62 | bool rlImGuiIsControlDown() { return IsKeyDown(KEY_RIGHT_CONTROL) || IsKeyDown(KEY_LEFT_CONTROL); } 63 | bool rlImGuiIsShiftDown() { return IsKeyDown(KEY_RIGHT_SHIFT) || IsKeyDown(KEY_LEFT_SHIFT); } 64 | bool rlImGuiIsAltDown() { return IsKeyDown(KEY_RIGHT_ALT) || IsKeyDown(KEY_LEFT_ALT); } 65 | bool rlImGuiIsSuperDown() { return IsKeyDown(KEY_RIGHT_SUPER) || IsKeyDown(KEY_LEFT_SUPER); } 66 | 67 | void ReloadFonts() 68 | { 69 | ImGuiIO& io = ImGui::GetIO(); 70 | unsigned char* pixels = nullptr; 71 | 72 | int width; 73 | int height; 74 | io.Fonts->GetTexDataAsRGBA32(&pixels, &width, &height, nullptr); 75 | Image image = GenImageColor(width, height, BLANK); 76 | memcpy(image.data, pixels, width * height * 4); 77 | 78 | Texture2D* fontTexture = (Texture2D*)io.Fonts->TexID; 79 | if (fontTexture && fontTexture->id != 0) 80 | { 81 | UnloadTexture(*fontTexture); 82 | MemFree(fontTexture); 83 | } 84 | 85 | fontTexture = (Texture2D*)MemAlloc(sizeof(Texture2D)); 86 | *fontTexture = LoadTextureFromImage(image); 87 | UnloadImage(image); 88 | io.Fonts->TexID = fontTexture; 89 | } 90 | 91 | static const char* GetClipTextCallback(void*) 92 | { 93 | return GetClipboardText(); 94 | } 95 | 96 | static void SetClipTextCallback(void*, const char* text) 97 | { 98 | SetClipboardText(text); 99 | } 100 | 101 | static void ImGuiNewFrame(float deltaTime) 102 | { 103 | ImGuiIO& io = ImGui::GetIO(); 104 | 105 | if (IsWindowFullscreen()) 106 | { 107 | int monitor = GetCurrentMonitor(); 108 | io.DisplaySize.x = float(GetMonitorWidth(monitor)); 109 | io.DisplaySize.y = float(GetMonitorHeight(monitor)); 110 | } 111 | else 112 | { 113 | io.DisplaySize.x = float(GetScreenWidth()); 114 | io.DisplaySize.y = float(GetScreenHeight()); 115 | } 116 | 117 | int width = int(io.DisplaySize.x), height = int(io.DisplaySize.y); 118 | #ifdef PLATFORM_DESKTOP 119 | glfwGetFramebufferSize(glfwGetCurrentContext(), &width, &height); 120 | #endif 121 | if (width > 0 && height > 0) 122 | { 123 | io.DisplayFramebufferScale = ImVec2(width / io.DisplaySize.x, height / io.DisplaySize.y); 124 | } 125 | else 126 | { 127 | io.DisplayFramebufferScale = ImVec2(1.0f, 1.0f); 128 | } 129 | 130 | io.DeltaTime = deltaTime; 131 | 132 | if (io.WantSetMousePos) 133 | { 134 | SetMousePosition((int)io.MousePos.x, (int)io.MousePos.y); 135 | } 136 | else 137 | { 138 | io.AddMousePosEvent((float)GetMouseX(), (float)GetMouseY()); 139 | } 140 | 141 | auto setMouseEvent = [&io](int rayMouse, int imGuiMouse) 142 | { 143 | if (IsMouseButtonPressed(rayMouse)) 144 | io.AddMouseButtonEvent(imGuiMouse, true); 145 | else if (IsMouseButtonReleased(rayMouse)) 146 | io.AddMouseButtonEvent(imGuiMouse, false); 147 | }; 148 | 149 | setMouseEvent(MOUSE_BUTTON_LEFT, ImGuiMouseButton_Left); 150 | setMouseEvent(MOUSE_BUTTON_RIGHT, ImGuiMouseButton_Right); 151 | setMouseEvent(MOUSE_BUTTON_MIDDLE, ImGuiMouseButton_Middle); 152 | setMouseEvent(MOUSE_BUTTON_FORWARD, ImGuiMouseButton_Middle+1); 153 | setMouseEvent(MOUSE_BUTTON_BACK, ImGuiMouseButton_Middle+2); 154 | 155 | { 156 | Vector2 mouseWheel = GetMouseWheelMoveV(); 157 | io.AddMouseWheelEvent(mouseWheel.x, mouseWheel.y); 158 | } 159 | 160 | if ((io.ConfigFlags & ImGuiConfigFlags_NoMouseCursorChange) == 0) 161 | { 162 | ImGuiMouseCursor imgui_cursor = ImGui::GetMouseCursor(); 163 | if (imgui_cursor != CurrentMouseCursor || io.MouseDrawCursor) 164 | { 165 | CurrentMouseCursor = imgui_cursor; 166 | if (io.MouseDrawCursor || imgui_cursor == ImGuiMouseCursor_None) 167 | { 168 | HideCursor(); 169 | } 170 | else 171 | { 172 | ShowCursor(); 173 | 174 | if (!(io.ConfigFlags & ImGuiConfigFlags_NoMouseCursorChange)) 175 | { 176 | SetMouseCursor((imgui_cursor > -1 && imgui_cursor < ImGuiMouseCursor_COUNT) ? MouseCursorMap[imgui_cursor] : MOUSE_CURSOR_DEFAULT); 177 | } 178 | } 179 | } 180 | } 181 | } 182 | 183 | static void ImGuiTriangleVert(ImDrawVert& idx_vert) 184 | { 185 | Color* c; 186 | c = (Color*)&idx_vert.col; 187 | rlColor4ub(c->r, c->g, c->b, c->a); 188 | rlTexCoord2f(idx_vert.uv.x, idx_vert.uv.y); 189 | rlVertex2f(idx_vert.pos.x, idx_vert.pos.y); 190 | } 191 | 192 | static void ImGuiRenderTriangles(unsigned int count, int indexStart, const ImVector& indexBuffer, const ImVector& vertBuffer, void* texturePtr) 193 | { 194 | if (count < 3) 195 | return; 196 | 197 | Texture* texture = (Texture*)texturePtr; 198 | 199 | unsigned int textureId = (texture == nullptr) ? 0 : texture->id; 200 | 201 | rlBegin(RL_TRIANGLES); 202 | rlSetTexture(textureId); 203 | 204 | for (unsigned int i = 0; i <= (count - 3); i += 3) 205 | { 206 | if (rlCheckRenderBatchLimit(3)) 207 | { 208 | rlBegin(RL_TRIANGLES); 209 | rlSetTexture(textureId); 210 | } 211 | 212 | ImDrawIdx indexA = indexBuffer[indexStart + i]; 213 | ImDrawIdx indexB = indexBuffer[indexStart + i + 1]; 214 | ImDrawIdx indexC = indexBuffer[indexStart + i + 2]; 215 | 216 | ImDrawVert vertexA = vertBuffer[indexA]; 217 | ImDrawVert vertexB = vertBuffer[indexB]; 218 | ImDrawVert vertexC = vertBuffer[indexC]; 219 | 220 | ImGuiTriangleVert(vertexA); 221 | ImGuiTriangleVert(vertexB); 222 | ImGuiTriangleVert(vertexC); 223 | } 224 | rlEnd(); 225 | } 226 | 227 | static void EnableScissor(float x, float y, float width, float height) 228 | { 229 | rlEnableScissorTest(); 230 | ImGuiIO& io = ImGui::GetIO(); 231 | rlScissor((int)(x * io.DisplayFramebufferScale.x), 232 | int((io.DisplaySize.y - (int)(y + height)) * io.DisplayFramebufferScale.y), 233 | (int)(width * io.DisplayFramebufferScale.x), 234 | (int)(height * io.DisplayFramebufferScale.y)); 235 | } 236 | 237 | static void SetupMouseCursors() 238 | { 239 | MouseCursorMap[ImGuiMouseCursor_Arrow] = MOUSE_CURSOR_ARROW; 240 | MouseCursorMap[ImGuiMouseCursor_TextInput] = MOUSE_CURSOR_IBEAM; 241 | MouseCursorMap[ImGuiMouseCursor_Hand] = MOUSE_CURSOR_POINTING_HAND; 242 | MouseCursorMap[ImGuiMouseCursor_ResizeAll] = MOUSE_CURSOR_RESIZE_ALL; 243 | MouseCursorMap[ImGuiMouseCursor_ResizeEW] = MOUSE_CURSOR_RESIZE_EW; 244 | MouseCursorMap[ImGuiMouseCursor_ResizeNESW] = MOUSE_CURSOR_RESIZE_NESW; 245 | MouseCursorMap[ImGuiMouseCursor_ResizeNS] = MOUSE_CURSOR_RESIZE_NS; 246 | MouseCursorMap[ImGuiMouseCursor_ResizeNWSE] = MOUSE_CURSOR_RESIZE_NWSE; 247 | MouseCursorMap[ImGuiMouseCursor_NotAllowed] = MOUSE_CURSOR_NOT_ALLOWED; 248 | } 249 | 250 | void SetupFontAwesome() 251 | { 252 | #ifndef NO_FONT_AWESOME 253 | static const ImWchar icons_ranges[] = { ICON_MIN_FA, ICON_MAX_FA, 0 }; 254 | ImFontConfig icons_config; 255 | icons_config.MergeMode = true; 256 | icons_config.PixelSnapH = true; 257 | icons_config.FontDataOwnedByAtlas = false; 258 | 259 | icons_config.GlyphRanges = icons_ranges; 260 | 261 | ImGuiIO& io = ImGui::GetIO(); 262 | 263 | io.Fonts->AddFontFromMemoryCompressedTTF((void*)fa_solid_900_compressed_data, fa_solid_900_compressed_size, FONT_AWESOME_ICON_SIZE, &icons_config, icons_ranges); 264 | #endif 265 | 266 | } 267 | 268 | void SetupBackend() 269 | { 270 | ImGuiIO& io = ImGui::GetIO(); 271 | io.BackendPlatformName = "imgui_impl_raylib"; 272 | 273 | io.BackendFlags |= ImGuiBackendFlags_HasMouseCursors; 274 | 275 | io.MousePos = ImVec2(0, 0); 276 | 277 | io.SetClipboardTextFn = SetClipTextCallback; 278 | io.GetClipboardTextFn = GetClipTextCallback; 279 | 280 | io.ClipboardUserData = nullptr; 281 | } 282 | 283 | void rlImGuiEndInitImGui() 284 | { 285 | ImGui::SetCurrentContext(GlobalContext); 286 | 287 | SetupFontAwesome(); 288 | 289 | SetupMouseCursors(); 290 | 291 | SetupBackend(); 292 | 293 | ReloadFonts(); 294 | } 295 | 296 | static void SetupKeymap() 297 | { 298 | if (!RaylibKeyMap.empty()) 299 | return; 300 | 301 | // build up a map of raylib keys to ImGuiKeys 302 | RaylibKeyMap[KEY_APOSTROPHE] = ImGuiKey_Apostrophe; 303 | RaylibKeyMap[KEY_COMMA] = ImGuiKey_Comma; 304 | RaylibKeyMap[KEY_MINUS] = ImGuiKey_Minus; 305 | RaylibKeyMap[KEY_PERIOD] = ImGuiKey_Period; 306 | RaylibKeyMap[KEY_SLASH] = ImGuiKey_Slash; 307 | RaylibKeyMap[KEY_ZERO] = ImGuiKey_0; 308 | RaylibKeyMap[KEY_ONE] = ImGuiKey_1; 309 | RaylibKeyMap[KEY_TWO] = ImGuiKey_2; 310 | RaylibKeyMap[KEY_THREE] = ImGuiKey_3; 311 | RaylibKeyMap[KEY_FOUR] = ImGuiKey_4; 312 | RaylibKeyMap[KEY_FIVE] = ImGuiKey_5; 313 | RaylibKeyMap[KEY_SIX] = ImGuiKey_6; 314 | RaylibKeyMap[KEY_SEVEN] = ImGuiKey_7; 315 | RaylibKeyMap[KEY_EIGHT] = ImGuiKey_8; 316 | RaylibKeyMap[KEY_NINE] = ImGuiKey_9; 317 | RaylibKeyMap[KEY_SEMICOLON] = ImGuiKey_Semicolon; 318 | RaylibKeyMap[KEY_EQUAL] = ImGuiKey_Equal; 319 | RaylibKeyMap[KEY_A] = ImGuiKey_A; 320 | RaylibKeyMap[KEY_B] = ImGuiKey_B; 321 | RaylibKeyMap[KEY_C] = ImGuiKey_C; 322 | RaylibKeyMap[KEY_D] = ImGuiKey_D; 323 | RaylibKeyMap[KEY_E] = ImGuiKey_E; 324 | RaylibKeyMap[KEY_F] = ImGuiKey_F; 325 | RaylibKeyMap[KEY_G] = ImGuiKey_G; 326 | RaylibKeyMap[KEY_H] = ImGuiKey_H; 327 | RaylibKeyMap[KEY_I] = ImGuiKey_I; 328 | RaylibKeyMap[KEY_J] = ImGuiKey_J; 329 | RaylibKeyMap[KEY_K] = ImGuiKey_K; 330 | RaylibKeyMap[KEY_L] = ImGuiKey_L; 331 | RaylibKeyMap[KEY_M] = ImGuiKey_M; 332 | RaylibKeyMap[KEY_N] = ImGuiKey_N; 333 | RaylibKeyMap[KEY_O] = ImGuiKey_O; 334 | RaylibKeyMap[KEY_P] = ImGuiKey_P; 335 | RaylibKeyMap[KEY_Q] = ImGuiKey_Q; 336 | RaylibKeyMap[KEY_R] = ImGuiKey_R; 337 | RaylibKeyMap[KEY_S] = ImGuiKey_S; 338 | RaylibKeyMap[KEY_T] = ImGuiKey_T; 339 | RaylibKeyMap[KEY_U] = ImGuiKey_U; 340 | RaylibKeyMap[KEY_V] = ImGuiKey_V; 341 | RaylibKeyMap[KEY_W] = ImGuiKey_W; 342 | RaylibKeyMap[KEY_X] = ImGuiKey_X; 343 | RaylibKeyMap[KEY_Y] = ImGuiKey_Y; 344 | RaylibKeyMap[KEY_Z] = ImGuiKey_Z; 345 | RaylibKeyMap[KEY_SPACE] = ImGuiKey_Space; 346 | RaylibKeyMap[KEY_ESCAPE] = ImGuiKey_Escape; 347 | RaylibKeyMap[KEY_ENTER] = ImGuiKey_Enter; 348 | RaylibKeyMap[KEY_TAB] = ImGuiKey_Tab; 349 | RaylibKeyMap[KEY_BACKSPACE] = ImGuiKey_Backspace; 350 | RaylibKeyMap[KEY_INSERT] = ImGuiKey_Insert; 351 | RaylibKeyMap[KEY_DELETE] = ImGuiKey_Delete; 352 | RaylibKeyMap[KEY_RIGHT] = ImGuiKey_RightArrow; 353 | RaylibKeyMap[KEY_LEFT] = ImGuiKey_LeftArrow; 354 | RaylibKeyMap[KEY_DOWN] = ImGuiKey_DownArrow; 355 | RaylibKeyMap[KEY_UP] = ImGuiKey_UpArrow; 356 | RaylibKeyMap[KEY_PAGE_UP] = ImGuiKey_PageUp; 357 | RaylibKeyMap[KEY_PAGE_DOWN] = ImGuiKey_PageDown; 358 | RaylibKeyMap[KEY_HOME] = ImGuiKey_Home; 359 | RaylibKeyMap[KEY_END] = ImGuiKey_End; 360 | RaylibKeyMap[KEY_CAPS_LOCK] = ImGuiKey_CapsLock; 361 | RaylibKeyMap[KEY_SCROLL_LOCK] = ImGuiKey_ScrollLock; 362 | RaylibKeyMap[KEY_NUM_LOCK] = ImGuiKey_NumLock; 363 | RaylibKeyMap[KEY_PRINT_SCREEN] = ImGuiKey_PrintScreen; 364 | RaylibKeyMap[KEY_PAUSE] = ImGuiKey_Pause; 365 | RaylibKeyMap[KEY_F1] = ImGuiKey_F1; 366 | RaylibKeyMap[KEY_F2] = ImGuiKey_F2; 367 | RaylibKeyMap[KEY_F3] = ImGuiKey_F3; 368 | RaylibKeyMap[KEY_F4] = ImGuiKey_F4; 369 | RaylibKeyMap[KEY_F5] = ImGuiKey_F5; 370 | RaylibKeyMap[KEY_F6] = ImGuiKey_F6; 371 | RaylibKeyMap[KEY_F7] = ImGuiKey_F7; 372 | RaylibKeyMap[KEY_F8] = ImGuiKey_F8; 373 | RaylibKeyMap[KEY_F9] = ImGuiKey_F9; 374 | RaylibKeyMap[KEY_F10] = ImGuiKey_F10; 375 | RaylibKeyMap[KEY_F11] = ImGuiKey_F11; 376 | RaylibKeyMap[KEY_F12] = ImGuiKey_F12; 377 | RaylibKeyMap[KEY_LEFT_SHIFT] = ImGuiKey_LeftShift; 378 | RaylibKeyMap[KEY_LEFT_CONTROL] = ImGuiKey_LeftCtrl; 379 | RaylibKeyMap[KEY_LEFT_ALT] = ImGuiKey_LeftAlt; 380 | RaylibKeyMap[KEY_LEFT_SUPER] = ImGuiKey_LeftSuper; 381 | RaylibKeyMap[KEY_RIGHT_SHIFT] = ImGuiKey_RightShift; 382 | RaylibKeyMap[KEY_RIGHT_CONTROL] = ImGuiKey_RightCtrl; 383 | RaylibKeyMap[KEY_RIGHT_ALT] = ImGuiKey_RightAlt; 384 | RaylibKeyMap[KEY_RIGHT_SUPER] = ImGuiKey_RightSuper; 385 | RaylibKeyMap[KEY_KB_MENU] = ImGuiKey_Menu; 386 | RaylibKeyMap[KEY_LEFT_BRACKET] = ImGuiKey_LeftBracket; 387 | RaylibKeyMap[KEY_BACKSLASH] = ImGuiKey_Backslash; 388 | RaylibKeyMap[KEY_RIGHT_BRACKET] = ImGuiKey_RightBracket; 389 | RaylibKeyMap[KEY_GRAVE] = ImGuiKey_GraveAccent; 390 | RaylibKeyMap[KEY_KP_0] = ImGuiKey_Keypad0; 391 | RaylibKeyMap[KEY_KP_1] = ImGuiKey_Keypad1; 392 | RaylibKeyMap[KEY_KP_2] = ImGuiKey_Keypad2; 393 | RaylibKeyMap[KEY_KP_3] = ImGuiKey_Keypad3; 394 | RaylibKeyMap[KEY_KP_4] = ImGuiKey_Keypad4; 395 | RaylibKeyMap[KEY_KP_5] = ImGuiKey_Keypad5; 396 | RaylibKeyMap[KEY_KP_6] = ImGuiKey_Keypad6; 397 | RaylibKeyMap[KEY_KP_7] = ImGuiKey_Keypad7; 398 | RaylibKeyMap[KEY_KP_8] = ImGuiKey_Keypad8; 399 | RaylibKeyMap[KEY_KP_9] = ImGuiKey_Keypad9; 400 | RaylibKeyMap[KEY_KP_DECIMAL] = ImGuiKey_KeypadDecimal; 401 | RaylibKeyMap[KEY_KP_DIVIDE] = ImGuiKey_KeypadDivide; 402 | RaylibKeyMap[KEY_KP_MULTIPLY] = ImGuiKey_KeypadMultiply; 403 | RaylibKeyMap[KEY_KP_SUBTRACT] = ImGuiKey_KeypadSubtract; 404 | RaylibKeyMap[KEY_KP_ADD] = ImGuiKey_KeypadAdd; 405 | RaylibKeyMap[KEY_KP_ENTER] = ImGuiKey_KeypadEnter; 406 | RaylibKeyMap[KEY_KP_EQUAL] = ImGuiKey_KeypadEqual; 407 | } 408 | 409 | static void SetupGlobals() 410 | { 411 | LastFrameFocused = IsWindowFocused(); 412 | LastControlPressed = false; 413 | LastShiftPressed = false; 414 | LastAltPressed = false; 415 | LastSuperPressed = false; 416 | 417 | } 418 | 419 | void rlImGuiBeginInitImGui() 420 | { 421 | SetupGlobals(); 422 | GlobalContext = ImGui::CreateContext(nullptr); 423 | SetupKeymap(); 424 | 425 | ImGuiIO& io = ImGui::GetIO(); 426 | io.Fonts->AddFontDefault(); 427 | } 428 | 429 | void rlImGuiSetup(bool dark) 430 | { 431 | rlImGuiBeginInitImGui(); 432 | 433 | if (dark) 434 | ImGui::StyleColorsDark(); 435 | else 436 | ImGui::StyleColorsLight(); 437 | 438 | rlImGuiEndInitImGui(); 439 | } 440 | 441 | void rlImGuiReloadFonts() 442 | { 443 | ImGui::SetCurrentContext(GlobalContext); 444 | 445 | ReloadFonts(); 446 | } 447 | 448 | void rlImGuiBegin() 449 | { 450 | ImGui::SetCurrentContext(GlobalContext); 451 | rlImGuiBeginDelta(GetFrameTime()); 452 | } 453 | 454 | void rlImGuiBeginDelta(float deltaTime) 455 | { 456 | ImGui::SetCurrentContext(GlobalContext); 457 | ImGuiNewFrame(deltaTime); 458 | ImGui_ImplRaylib_ProcessEvents(); 459 | ImGui::NewFrame(); 460 | } 461 | 462 | void rlImGuiEnd() 463 | { 464 | ImGui::SetCurrentContext(GlobalContext); 465 | ImGui::Render(); 466 | ImGui_ImplRaylib_RenderDrawData(ImGui::GetDrawData()); 467 | } 468 | 469 | void rlImGuiShutdown() 470 | { 471 | ImGui::SetCurrentContext(GlobalContext); 472 | ImGui_ImplRaylib_Shutdown(); 473 | 474 | ImGui::DestroyContext(); 475 | } 476 | 477 | void rlImGuiImage(const Texture* image) 478 | { 479 | if (!image) 480 | return; 481 | if (GlobalContext) 482 | ImGui::SetCurrentContext(GlobalContext); 483 | ImGui::Image((ImTextureID)image, ImVec2(float(image->width), float(image->height))); 484 | } 485 | 486 | bool rlImGuiImageButton(const char* name, const Texture* image) 487 | { 488 | if (!image) 489 | return false; 490 | if (GlobalContext) 491 | ImGui::SetCurrentContext(GlobalContext); 492 | return ImGui::ImageButton(name, (ImTextureID)image, ImVec2(float(image->width), float(image->height))); 493 | } 494 | 495 | bool rlImGuiImageButtonSize(const char* name, const Texture* image, ImVec2 size) 496 | { 497 | if (!image) 498 | return false; 499 | if (GlobalContext) 500 | ImGui::SetCurrentContext(GlobalContext); 501 | return ImGui::ImageButton(name, (ImTextureID)image, size); 502 | } 503 | 504 | void rlImGuiImageSize(const Texture* image, int width, int height) 505 | { 506 | if (!image) 507 | return; 508 | if (GlobalContext) 509 | ImGui::SetCurrentContext(GlobalContext); 510 | ImGui::Image((ImTextureID)image, ImVec2(float(width), float(height))); 511 | } 512 | 513 | void rlImGuiImageSizeV(const Texture* image, Vector2 size) 514 | { 515 | if (!image) 516 | return; 517 | if (GlobalContext) 518 | ImGui::SetCurrentContext(GlobalContext); 519 | ImGui::Image((ImTextureID)image, ImVec2(size.x, size.y)); 520 | } 521 | 522 | void rlImGuiImageRect(const Texture* image, int destWidth, int destHeight, Rectangle sourceRect) 523 | { 524 | if (!image) 525 | return; 526 | if (GlobalContext) 527 | ImGui::SetCurrentContext(GlobalContext); 528 | ImVec2 uv0; 529 | ImVec2 uv1; 530 | 531 | if (sourceRect.width < 0) 532 | { 533 | uv0.x = -((float)sourceRect.x / image->width); 534 | uv1.x = (uv0.x - (float)(fabs(sourceRect.width) / image->width)); 535 | } 536 | else 537 | { 538 | uv0.x = (float)sourceRect.x / image->width; 539 | uv1.x = uv0.x + (float)(sourceRect.width / image->width); 540 | } 541 | 542 | if (sourceRect.height < 0) 543 | { 544 | uv0.y = -((float)sourceRect.y / image->height); 545 | uv1.y = (uv0.y - (float)(fabs(sourceRect.height) / image->height)); 546 | } 547 | else 548 | { 549 | uv0.y = (float)sourceRect.y / image->height; 550 | uv1.y = uv0.y + (float)(sourceRect.height / image->height); 551 | } 552 | 553 | ImGui::Image((ImTextureID)image, ImVec2(float(destWidth), float(destHeight)), uv0, uv1); 554 | } 555 | 556 | void rlImGuiImageRenderTexture(const RenderTexture* image) 557 | { 558 | if (!image) 559 | return; 560 | if (GlobalContext) 561 | ImGui::SetCurrentContext(GlobalContext); 562 | rlImGuiImageRect(&image->texture, image->texture.width, image->texture.height, Rectangle{ 0,0, float(image->texture.width), -float(image->texture.height) }); 563 | } 564 | 565 | void rlImGuiImageRenderTextureFit(const RenderTexture* image, bool center) 566 | { 567 | if (!image) 568 | return; 569 | if (GlobalContext) 570 | ImGui::SetCurrentContext(GlobalContext); 571 | 572 | ImVec2 area = ImGui::GetContentRegionAvail(); 573 | 574 | float scale = area.x / image->texture.width; 575 | 576 | float y = image->texture.height * scale; 577 | if (y > area.y) 578 | { 579 | scale = area.y / image->texture.height; 580 | } 581 | 582 | int sizeX = int(image->texture.width * scale); 583 | int sizeY = int(image->texture.height * scale); 584 | 585 | if (center) 586 | { 587 | ImGui::SetCursorPosX(0); 588 | ImGui::SetCursorPosX(area.x/2 - sizeX/2); 589 | ImGui::SetCursorPosY(ImGui::GetCursorPosY() + (area.y / 2 - sizeY / 2)); 590 | } 591 | 592 | rlImGuiImageRect(&image->texture, sizeX, sizeY, Rectangle{ 0,0, float(image->texture.width), -float(image->texture.height) }); 593 | } 594 | 595 | // raw ImGui backend API 596 | bool ImGui_ImplRaylib_Init() 597 | { 598 | SetupGlobals(); 599 | 600 | SetupKeymap(); 601 | 602 | SetupMouseCursors(); 603 | 604 | SetupBackend(); 605 | 606 | return true; 607 | } 608 | 609 | void Imgui_ImplRaylib_BuildFontAtlas() 610 | { 611 | ReloadFonts(); 612 | } 613 | 614 | void ImGui_ImplRaylib_Shutdown() 615 | { 616 | ImGuiIO& io =ImGui::GetIO(); 617 | Texture2D* fontTexture = (Texture2D*)io.Fonts->TexID; 618 | 619 | if (fontTexture) 620 | { 621 | UnloadTexture(*fontTexture); 622 | MemFree(fontTexture); 623 | } 624 | 625 | io.Fonts->TexID = 0; 626 | } 627 | 628 | void ImGui_ImplRaylib_NewFrame() 629 | { 630 | ImGuiNewFrame(GetFrameTime()); 631 | } 632 | 633 | void ImGui_ImplRaylib_RenderDrawData(ImDrawData* draw_data) 634 | { 635 | rlDrawRenderBatchActive(); 636 | rlDisableBackfaceCulling(); 637 | 638 | for (int l = 0; l < draw_data->CmdListsCount; ++l) 639 | { 640 | const ImDrawList* commandList = draw_data->CmdLists[l]; 641 | 642 | for (const auto& cmd : commandList->CmdBuffer) 643 | { 644 | EnableScissor(cmd.ClipRect.x - draw_data->DisplayPos.x, cmd.ClipRect.y - draw_data->DisplayPos.y, cmd.ClipRect.z - (cmd.ClipRect.x - draw_data->DisplayPos.x), cmd.ClipRect.w - (cmd.ClipRect.y - draw_data->DisplayPos.y)); 645 | if (cmd.UserCallback != nullptr) 646 | { 647 | cmd.UserCallback(commandList, &cmd); 648 | 649 | continue; 650 | } 651 | 652 | ImGuiRenderTriangles(cmd.ElemCount, cmd.IdxOffset, commandList->IdxBuffer, commandList->VtxBuffer, cmd.TextureId); 653 | rlDrawRenderBatchActive(); 654 | } 655 | } 656 | 657 | rlSetTexture(0); 658 | rlDisableScissorTest(); 659 | rlEnableBackfaceCulling(); 660 | } 661 | 662 | bool ImGui_ImplRaylib_ProcessEvents() 663 | { 664 | ImGuiIO& io = ImGui::GetIO(); 665 | 666 | bool focused = IsWindowFocused(); 667 | if (focused != LastFrameFocused) 668 | io.AddFocusEvent(focused); 669 | LastFrameFocused = focused; 670 | 671 | // handle the modifyer key events so that shortcuts work 672 | bool ctrlDown = rlImGuiIsControlDown(); 673 | if (ctrlDown != LastControlPressed) 674 | io.AddKeyEvent(ImGuiMod_Ctrl, ctrlDown); 675 | LastControlPressed = ctrlDown; 676 | 677 | bool shiftDown = rlImGuiIsShiftDown(); 678 | if (shiftDown != LastShiftPressed) 679 | io.AddKeyEvent(ImGuiMod_Shift, shiftDown); 680 | LastShiftPressed = shiftDown; 681 | 682 | bool altDown = rlImGuiIsAltDown(); 683 | if (altDown != LastAltPressed) 684 | io.AddKeyEvent(ImGuiMod_Alt, altDown); 685 | LastAltPressed = altDown; 686 | 687 | bool superDown = rlImGuiIsSuperDown(); 688 | if (superDown != LastSuperPressed) 689 | io.AddKeyEvent(ImGuiMod_Super, superDown); 690 | LastSuperPressed = superDown; 691 | 692 | // get the pressed keys, they are in event order 693 | int keyId = GetKeyPressed(); 694 | while (keyId != 0) 695 | { 696 | auto keyItr = RaylibKeyMap.find(KeyboardKey(keyId)); 697 | if (keyItr != RaylibKeyMap.end()) 698 | io.AddKeyEvent(keyItr->second, true); 699 | keyId = GetKeyPressed(); 700 | } 701 | 702 | // look for any keys that were down last frame and see if they were down and are released 703 | for (const auto keyItr : RaylibKeyMap) 704 | { 705 | if (IsKeyReleased(keyItr.first)) 706 | io.AddKeyEvent(keyItr.second, false); 707 | } 708 | 709 | // add the text input in order 710 | unsigned int pressed = GetCharPressed(); 711 | while (pressed != 0) 712 | { 713 | io.AddInputCharacter(pressed); 714 | pressed = GetCharPressed(); 715 | } 716 | 717 | return true; 718 | } 719 | -------------------------------------------------------------------------------- /src/robot.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace urdf { 18 | using std::string; 19 | 20 | static inline Matrix MatMul(const Matrix& left, const Matrix& right) 21 | { 22 | return MatrixMultiply(right, left); 23 | } 24 | 25 | static void storeFloat(const char *s, float& f) { 26 | std::stringstream iss(s); 27 | iss >> f; 28 | CHECK_F(iss && (iss >> std::ws).eof(), "storeFloat: invalid string"); 29 | } 30 | 31 | static void storeVec3(const char *s, Vector3& vec) { 32 | std::stringstream iss(s); 33 | iss >> vec.x >> vec.y >> vec.z; 34 | CHECK_F(iss && (iss >> std::ws).eof(), "store_vec3: invalid string"); 35 | } 36 | 37 | static void storeVec4(const char *s, Vector4& vec) { 38 | std::stringstream iss(s); 39 | iss >> vec.x >> vec.y >> vec.z >> vec.w; 40 | CHECK_F(iss && (iss >> std::ws).eof(), "store_vec4: invalid string"); 41 | } 42 | 43 | Origin::Origin() : xyz{}, rpy{} { } 44 | 45 | Origin::Origin(const char *xyz_s, const char *rpy_s) 46 | { 47 | xyz.x = xyz.y = xyz.z = 0.0F; 48 | rpy.x = rpy.y = rpy.z = 0.0F; 49 | 50 | if (!xyz_s || std::strlen(xyz_s) == 0) { 51 | LOG_F(WARNING, "Origin xyz is empty"); 52 | } else { 53 | storeVec3(xyz_s, xyz); 54 | } 55 | 56 | if (!rpy_s || std::strlen(rpy_s) == 0) { 57 | LOG_F(WARNING, "Origin rpy is empty"); 58 | } else { 59 | storeVec3(rpy_s, rpy); 60 | } 61 | } 62 | 63 | Origin::Origin(const Vector3& xyz, const Vector3& rpy) 64 | : xyz(xyz), rpy(rpy) 65 | { 66 | 67 | } 68 | 69 | Matrix Origin::toMatrix() const 70 | { 71 | Matrix rot_mat {MatrixRotateXYZ(rpy)}; 72 | Matrix pos_mat {MatrixTranslate(xyz.x, xyz.y, xyz.z)}; 73 | return MatrixMultiply(rot_mat, pos_mat); 74 | } 75 | 76 | Box::Box() 77 | { 78 | size.x = size.y = size.z = 1.0F; 79 | } 80 | 81 | Box::Box(const char *_size) 82 | { 83 | storeVec3(_size, size); 84 | } 85 | 86 | Box::Box(const Vector3& size) 87 | : size(size) 88 | { 89 | 90 | } 91 | 92 | Model Box::generateGeometry() 93 | { 94 | ::Mesh mesh = GenMeshCube(size.x, size.y, size.z); 95 | return LoadModelFromMesh(mesh); 96 | } 97 | 98 | Cylinder::Cylinder() 99 | { 100 | radius = length = 1.0F; 101 | } 102 | 103 | Cylinder::Cylinder(const char *_radius, const char *_length) 104 | { 105 | radius = length = 0.0F; 106 | if (_radius) storeFloat(_radius, radius); 107 | if (_length) storeFloat(_length, length); 108 | } 109 | 110 | Model Cylinder::generateGeometry() 111 | { 112 | ::Mesh mesh = GenMeshCenteredCylinder(radius, length, 32); 113 | return LoadModelFromMesh(mesh); 114 | } 115 | 116 | Sphere::Sphere() 117 | { 118 | radius = 1.0F; 119 | } 120 | 121 | Sphere::Sphere(const char *_radius) 122 | { 123 | storeFloat(_radius, radius); 124 | } 125 | 126 | Sphere::Sphere(float _radius) 127 | : radius(_radius) 128 | { 129 | 130 | } 131 | 132 | Model Sphere::generateGeometry() 133 | { 134 | ::Mesh mesh = GenMeshSphere(radius, 32, 32); 135 | return LoadModelFromMesh(mesh); 136 | } 137 | 138 | Mesh::Mesh(const char *filename) 139 | : filename(filename) 140 | { 141 | } 142 | 143 | Model Mesh::generateGeometry() 144 | { 145 | return LoadModelFromCollada(filename); 146 | } 147 | 148 | Inertia::Inertia(float ixx, float iyy, float izz, float ixy, float ixz, float iyz) 149 | : ixx(ixx), iyy(iyy), izz(izz), ixy(ixy), ixz(ixz), iyz(iyz) 150 | { 151 | 152 | } 153 | 154 | Inertial::Inertial(const char *xyz, const char *rpy, 155 | float mass, 156 | float ixx, float iyy, float izz, 157 | float ixy, float ixz, float iyz) 158 | : origin(xyz, rpy), mass(mass), inertia(ixx, iyy, izz, ixy, ixz, iyz) 159 | { 160 | 161 | } 162 | 163 | Axis::Axis() : xyz{} { } 164 | 165 | Axis::Axis(const Vector3& _xyz) : xyz(_xyz) { } 166 | 167 | Axis::Axis(const char *s_xyz) 168 | { 169 | storeVec3(s_xyz, xyz); 170 | } 171 | 172 | Dynamics::Dynamics() : damping(0.0F), friction(0.0F) { } 173 | 174 | Dynamics::Dynamics(float _damping, float _friction) : damping(_damping), friction(_friction) { } 175 | 176 | Limit::Limit(float _lower, float _upper, float _effort, float _velocity) 177 | : lower(_lower), upper(_upper), effort(_effort), velocity(_velocity) 178 | { 179 | 180 | } 181 | 182 | Link::Link(string name) : name(std::move(name)) {} 183 | 184 | Joint::Joint(const char *_name, const char *_parent, const char *_child, const char *_type) 185 | : name(_name), parent(_parent), child(_child) 186 | { 187 | std::string stype(_type); 188 | if (stype == "revolute") { 189 | type = kRevolute; 190 | } else if (stype == "continuous") { 191 | type = kContinuous; 192 | } else if (stype == "prismatic") { 193 | type = kPrismatic; 194 | } else if (stype == "fixed") { 195 | type = kFixed; 196 | } else if (stype == "floating") { 197 | type = kFloating; 198 | } else if (stype == "planar") { 199 | type = kPlanar; 200 | } else { 201 | LOG_F(WARNING, "Unknown joint type: %s", _type); 202 | type = kFixed; 203 | } 204 | } 205 | 206 | LinkNode::LinkNode() 207 | : link("New link"), w_T_l(MatrixIdentity()) 208 | { 209 | 210 | } 211 | 212 | LinkNode::LinkNode(Link link, JointNodePtr parent_joint) 213 | : link(std::move(link)), parent(std::move(parent_joint)), w_T_l(MatrixIdentity()) 214 | { 215 | 216 | } 217 | 218 | void LinkNode::addCollision() 219 | { 220 | link.collision.emplace_back(); 221 | link.collision.back().geometry.type = std::make_shared(); 222 | collision_models.push_back(link.collision.back().geometry.type->generateGeometry()); 223 | } 224 | 225 | void LinkNode::deleteCollision(int i) 226 | { 227 | link.collision.erase(link.collision.begin() + i); 228 | UnloadModel(collision_models[i]); 229 | collision_models.erase(collision_models.begin() + i); 230 | } 231 | 232 | JointNode::JointNode(Joint joint, LinkNodePtr parent, LinkNodePtr child) 233 | : joint(std::move(joint)), parent(std::move(parent)), child(std::move(child)) 234 | { 235 | 236 | } 237 | 238 | RobotPtr buildRobot(const char *urdf_file) 239 | { 240 | pugi::xml_document doc; 241 | pugi::xml_parse_result result = doc.load_file(urdf_file); 242 | CHECK_F(result, "Failed to open %s", urdf_file); 243 | 244 | bool is_root_robot = doc.root().name() != std::string("robot"); 245 | CHECK_F(is_root_robot, "The urdf root name (%s) is not robot", doc.root().name()); 246 | LOG_F(INFO, "Building robot..."); 247 | 248 | pugi::xml_node root_link = findRoot(doc); 249 | CHECK_F(not root_link.empty(), "No root link found"); 250 | 251 | auto tree_root = std::make_shared(); 252 | tree_root->link = xmlNodeToLink(root_link); 253 | tree_root->w_T_l = MatrixIdentity(); 254 | 255 | // Create all materials 256 | // TODO(ramon): support loading materials that are defined within the link 257 | std::map materials; 258 | for (pugi::xml_node& mat : doc.child("robot").children("material")) { 259 | if (const auto m = xmlNodeToMaterial(mat)) { 260 | materials.insert({m->name, *m}); 261 | } 262 | } 263 | // TODO(ramon): every link uses the same shader, the material properties change the shader color. 264 | 265 | // Parent link to all child joints 266 | std::map> joint_p_hash; 267 | for (const pugi::xml_node& joint : doc.child("robot").children("joint")) { 268 | Joint j = xmlNodeToJoint(joint); 269 | if (joint_p_hash.find(j.parent) == joint_p_hash.end()) { 270 | joint_p_hash.insert({ j.parent, std::vector({j}) }); 271 | } else { 272 | joint_p_hash[j.parent].push_back(j); 273 | } 274 | } 275 | 276 | // Link name to link 277 | std::map link_hash; 278 | for (const pugi::xml_node& link : doc.child("robot").children("link")) { 279 | Link l = xmlNodeToLink(link); 280 | CHECK_F(link_hash.find(l.name) == link_hash.end()); 281 | link_hash.insert({l.name, l}); 282 | } 283 | 284 | std::deque deq{tree_root}; 285 | while (not deq.empty()) { 286 | LinkNodePtr current_root = deq.front(); 287 | deq.pop_front(); 288 | 289 | const auto it = joint_p_hash.find(current_root->link.name); 290 | if (it == joint_p_hash.end()) { 291 | continue; 292 | } 293 | 294 | for (const auto& joint : it->second) { 295 | auto joint_node = std::make_shared(JointNode {joint, current_root, nullptr}); 296 | CHECK_F(link_hash.find(joint.child) != link_hash.end()); 297 | 298 | auto child_node = std::make_shared(LinkNode {link_hash[joint.child], joint_node}); 299 | joint_node->child = child_node; 300 | current_root->children.push_back(joint_node); 301 | deq.push_back(child_node); 302 | } 303 | } 304 | 305 | RobotPtr robot = std::make_shared(tree_root, materials); 306 | 307 | LOG_F(INFO, "URDF Tree built successfully!"); 308 | 309 | return robot; 310 | } 311 | 312 | pugi::xml_node findRoot(const pugi::xml_document& doc) 313 | { 314 | pugi::xml_node root_link; 315 | std::set child_link_names; 316 | 317 | for (const pugi::xml_node joint : doc.child("robot").children("joint")) { 318 | child_link_names.insert(joint.child("child").attribute("link").value()); 319 | } 320 | for (const pugi::xml_node link : doc.child("robot").children("link")) { 321 | if (child_link_names.find(link.attribute("name").value()) == child_link_names.end()) { 322 | root_link = link; 323 | break; 324 | } 325 | } 326 | 327 | return root_link; 328 | } 329 | 330 | Link xmlNodeToLink(const pugi::xml_node& xml_node) 331 | { 332 | CHECK_F(static_cast(xml_node.attribute("name")), "Link has no name"); 333 | Link link(xml_node.attribute("name").as_string()); 334 | 335 | link.inertial = xmlNodeToInertial(xml_node.child("inertial")); 336 | 337 | const pugi::xml_node& vis_node = xml_node.child("visual"); 338 | if (vis_node) { 339 | link.visual = Visual{}; 340 | 341 | // --- Name 342 | std::string visual_name = vis_node.attribute("name").as_string(); 343 | if (not visual_name.empty()) link.visual->name = visual_name; 344 | 345 | // --- Origin 346 | const auto& ori_node = vis_node.child("origin"); 347 | link.visual->origin = xmlNodeToOrigin(ori_node); 348 | 349 | // --- Geometry 350 | const auto& geom_node = vis_node.child("geometry"); 351 | link.visual->geometry = xmlNodeToGeometry(geom_node); 352 | 353 | // --- Material 354 | if (const auto& mat_node = vis_node.child("material")) { 355 | link.visual->material_name = mat_node.attribute("name").as_string(); 356 | CHECK_F(not link.visual->material_name->empty(), "Materials need to have a name!"); 357 | } 358 | } 359 | 360 | for (const auto& col_node : xml_node.children("collision")) { 361 | Collision col { }; 362 | 363 | // --- Name 364 | std::string collision_name = col_node.attribute("name").as_string(); 365 | if (not collision_name.empty()) col.name = collision_name; 366 | 367 | // --- Origin 368 | const auto& ori_node = col_node.child("origin"); 369 | col.origin = xmlNodeToOrigin(ori_node); 370 | 371 | // --- Geometry 372 | const auto& geom_node = col_node.child("geometry"); 373 | col.geometry = xmlNodeToGeometry(geom_node); 374 | 375 | link.collision.push_back(col); 376 | } 377 | 378 | return link; 379 | } 380 | 381 | Joint xmlNodeToJoint(const pugi::xml_node& xml_node) 382 | { 383 | Joint joint( 384 | xml_node.attribute("name").as_string(), 385 | xml_node.child("parent").attribute("link").as_string(), 386 | xml_node.child("child").attribute("link").as_string(), 387 | xml_node.attribute("type").as_string() 388 | ); 389 | if (xml_node.child("origin")) { 390 | joint.origin = Origin( 391 | xml_node.child("origin").attribute("xyz").as_string(), 392 | xml_node.child("origin").attribute("rpy").as_string() 393 | ); 394 | } 395 | if (xml_node.child("axis")) { 396 | joint.axis = Axis( 397 | xml_node.child("axis").attribute("xyz").as_string() 398 | ); 399 | } 400 | if (xml_node.child("dynamics")) { 401 | joint.dynamics = Dynamics( 402 | xml_node.child("dynamics").attribute("damping").as_float(), 403 | xml_node.child("dynamics").attribute("friction").as_float() 404 | ); 405 | } 406 | if (xml_node.child("limit")) { 407 | joint.limit = Limit( 408 | xml_node.child("limit").attribute("lower").as_float(), 409 | xml_node.child("limit").attribute("upper").as_float(), 410 | xml_node.child("limit").attribute("effort").as_float(), 411 | xml_node.child("limit").attribute("velocity").as_float() 412 | ); 413 | } 414 | return joint; 415 | } 416 | 417 | std::optional xmlNodeToOrigin(const pugi::xml_node& xml_node) 418 | { 419 | if (xml_node) { 420 | return Origin( 421 | xml_node.attribute("xyz").as_string(), 422 | xml_node.attribute("rpy").as_string() 423 | ); 424 | } else { 425 | return std::nullopt; 426 | } 427 | } 428 | 429 | std::optional xmlNodeToInertial(const pugi::xml_node& xml_node) 430 | { 431 | if (xml_node) { 432 | return Inertial( 433 | xml_node.child("inertial").child("origin").attribute("xyz").as_string(), 434 | xml_node.child("inertial").child("origin").attribute("rpy").as_string(), 435 | xml_node.child("inertial").child("mass").attribute("value").as_float(), 436 | xml_node.child("inertial").child("inertia").attribute("ixx").as_float(), 437 | xml_node.child("inertial").child("inertia").attribute("iyy").as_float(), 438 | xml_node.child("inertial").child("inertia").attribute("izz").as_float(), 439 | xml_node.child("inertial").child("inertia").attribute("ixy").as_float(), 440 | xml_node.child("inertial").child("inertia").attribute("ixz").as_float(), 441 | xml_node.child("inertial").child("inertia").attribute("iyz").as_float() 442 | ); 443 | } else { 444 | return std::nullopt; 445 | } 446 | } 447 | 448 | Geometry xmlNodeToGeometry(const pugi::xml_node& geom_node) 449 | { 450 | CHECK_F(static_cast(geom_node), "Missing geometry tag!"); 451 | 452 | Geometry geom; 453 | if (geom_node.child("box")) { 454 | geom.type = std::make_shared(geom_node.child("box").attribute("size").as_string()); 455 | } else if (geom_node.child("cylinder")) { 456 | geom.type = std::make_shared( 457 | geom_node.child("cylinder").attribute("radius").as_string(), 458 | geom_node.child("cylinder").attribute("length").as_string() 459 | ); 460 | } else if (geom_node.child("sphere")) { 461 | geom.type = std::make_shared(geom_node.child("sphere").attribute("radius").as_string()); 462 | } else if (geom_node.child("mesh")) { 463 | LOG_F(INFO, "Mesh visuals are not implemented yet!"); // TODO(ramon) 464 | geom.type = std::make_shared(); 465 | } 466 | 467 | return geom; 468 | } 469 | 470 | std::optional xmlNodeToMaterial(const pugi::xml_node& mat_node) 471 | { 472 | if (mat_node) { 473 | Material mat; 474 | 475 | mat.name = mat_node.attribute("name").as_string(); 476 | CHECK_F(not mat.name.empty(), "Materials need to have a name!"); 477 | 478 | if (mat_node.child("color")) { 479 | CHECK_F(static_cast(mat_node.child("color").attribute("rgba")), 480 | "The material color tag needs to have an rgba value"); 481 | mat.rgba = Vector4{}; 482 | storeVec4(mat_node.child("color").attribute("rgba").as_string(), mat.rgba.value()); 483 | } 484 | 485 | if (mat_node.child("texture")) { 486 | CHECK_F(static_cast(mat_node.child("texture").attribute("filename")), 487 | "The texture tag needs to have a filename attribute!"); 488 | mat.texture_file = mat_node.child("texture").attribute("filename").as_string(); 489 | } 490 | 491 | return mat; 492 | } else { 493 | return std::nullopt; 494 | } 495 | } 496 | 497 | void exportRobot(const Robot& robot, std::string out_filename) 498 | { 499 | pugi::xml_document out_doc; 500 | const LinkNodePtr root_node = robot.getRoot(); 501 | 502 | pugi::xml_node robot_root = out_doc.append_child("robot"); 503 | 504 | // save links 505 | std::deque deq {root_node}; 506 | while (not deq.empty()) { 507 | const auto& current_link = deq.front(); 508 | deq.pop_front(); 509 | 510 | pugi::xml_node link_node = robot_root.append_child("link"); 511 | linkToXmlNode(link_node, current_link->link); 512 | 513 | for (const auto& joint : current_link->children) { 514 | deq.push_back(joint->child); 515 | } 516 | } 517 | 518 | // save joints 519 | deq = std::deque {root_node}; 520 | while (not deq.empty()) { 521 | const auto& current_link = deq.front(); 522 | deq.pop_front(); 523 | 524 | for (const auto& joint : current_link->children) { 525 | pugi::xml_node joint_node = robot_root.append_child("joint"); 526 | jointToXmlNode(joint_node, joint->joint); 527 | 528 | deq.push_back(joint->child); 529 | } 530 | } 531 | 532 | // save materials 533 | for (const auto& mat : robot.getMaterials()) { 534 | pugi::xml_node mat_node = robot_root.append_child("material"); 535 | materialToXmlNode(mat_node, mat.second); 536 | } 537 | 538 | if (!out_doc.save_file(out_filename.c_str())) { 539 | LOG_F(ERROR, "File %s could not be opened or written!", out_filename.c_str()); 540 | } 541 | } 542 | 543 | void linkToXmlNode(pugi::xml_node& xml_node, const Link& link) 544 | { 545 | xml_node.set_name("link"); 546 | xml_node.append_attribute("name") = link.name.c_str(); 547 | 548 | if (link.inertial) { 549 | pugi::xml_node inertial_node = xml_node.append_child("inertial"); 550 | inertialToXmlNode(inertial_node, *link.inertial); 551 | } 552 | 553 | if (link.visual) { 554 | pugi::xml_node visual_node = xml_node.append_child("visual"); 555 | if (link.visual->name) { 556 | visual_node.append_attribute("name") = link.visual->name->c_str(); 557 | } 558 | if (link.visual->origin) { 559 | pugi::xml_node origin_node = visual_node.append_child("origin"); 560 | originToXmlNode(origin_node, *link.visual->origin); 561 | } 562 | pugi::xml_node geometry_node = visual_node.append_child("geometry"); 563 | geometryToXmlNode(geometry_node, link.visual->geometry); 564 | if (link.visual->material_name) { 565 | visual_node.append_child("material").append_attribute("name") 566 | = link.visual->material_name->c_str(); 567 | } 568 | } 569 | 570 | for (const Collision& col : link.collision) { 571 | pugi::xml_node collision_node = xml_node.append_child("collision"); 572 | 573 | if (col.name) { 574 | collision_node.append_attribute("name") = col.name->c_str(); 575 | } 576 | 577 | if (col.origin) { 578 | pugi::xml_node origin_node = collision_node.append_child("origin"); 579 | originToXmlNode(origin_node, *col.origin); 580 | } 581 | 582 | pugi::xml_node geometry_node = collision_node.append_child("geometry"); 583 | geometryToXmlNode(geometry_node, col.geometry); 584 | } 585 | 586 | if (not xml_node) LOG_F(ERROR, "Link node is empty!"); 587 | } 588 | 589 | void jointToXmlNode(pugi::xml_node& joint_node, const Joint& joint) 590 | { 591 | joint_node.set_name("joint"); 592 | switch (joint.type) { 593 | case Joint::kRevolute: 594 | joint_node.append_attribute("type") = "revolute"; 595 | break; 596 | case Joint::kContinuous: 597 | joint_node.append_attribute("type") = "continuous"; 598 | break; 599 | case Joint::kPrismatic: 600 | joint_node.append_attribute("type") = "prismatic"; 601 | break; 602 | case Joint::kFixed: 603 | joint_node.append_attribute("type") = "fixed"; 604 | break; 605 | case Joint::kFloating: 606 | joint_node.append_attribute("type") = "floating"; 607 | break; 608 | case Joint::kPlanar: 609 | joint_node.append_attribute("type") = "planar"; 610 | break; 611 | default: 612 | LOG_F(ERROR, "Unknown joint type."); 613 | } 614 | 615 | pugi::xml_node parent_node = joint_node.append_child("parent"); 616 | parent_node.append_attribute("link") = joint.parent.c_str(); 617 | pugi::xml_node child_node = joint_node.append_child("child"); 618 | child_node.append_attribute("link") = joint.child.c_str(); 619 | 620 | if (joint.origin) { 621 | pugi::xml_node origin_node = joint_node.append_child("origin"); 622 | originToXmlNode(origin_node, *joint.origin); 623 | } 624 | 625 | if (joint.axis) { 626 | pugi::xml_node axis_node = joint_node.append_child("axis"); 627 | axis_node.append_attribute("xyz") = (std::to_string(joint.axis->xyz.x) + " " + 628 | std::to_string(joint.axis->xyz.y) + " " + 629 | std::to_string(joint.axis->xyz.z)).c_str(); 630 | } 631 | 632 | if (joint.dynamics) { 633 | pugi::xml_node dynamics_node = joint_node.append_child("dynamics"); 634 | dynamics_node.append_attribute("damping") = joint.dynamics->damping; 635 | dynamics_node.append_attribute("friction") = joint.dynamics->friction; 636 | } 637 | 638 | if (joint.limit) { 639 | pugi::xml_node limit_node = joint_node.append_child("limit"); 640 | limit_node.append_attribute("lower") = joint.limit->lower; 641 | limit_node.append_attribute("upper") = joint.limit->upper; 642 | limit_node.append_attribute("effort") = joint.limit->effort; 643 | limit_node.append_attribute("velocity") = joint.limit->velocity; 644 | } 645 | } 646 | 647 | void inertialToXmlNode(pugi::xml_node& xml_node, const Inertial& inertial) 648 | { 649 | xml_node.set_name("inertial"); 650 | 651 | pugi::xml_node mass_node = xml_node.append_child("mass"); 652 | mass_node.append_attribute("value") = inertial.mass; 653 | 654 | pugi::xml_node inertia_node = xml_node.append_child("inertia"); 655 | inertia_node.append_attribute("ixx") = inertial.inertia.ixx; 656 | inertia_node.append_attribute("iyy") = inertial.inertia.iyy; 657 | inertia_node.append_attribute("izz") = inertial.inertia.izz; 658 | inertia_node.append_attribute("ixy") = inertial.inertia.ixy; 659 | inertia_node.append_attribute("ixz") = inertial.inertia.ixz; 660 | inertia_node.append_attribute("iyz") = inertial.inertia.iyz; 661 | 662 | pugi::xml_node origin_node = xml_node.append_child("origin"); 663 | originToXmlNode(origin_node, inertial.origin); 664 | } 665 | 666 | void originToXmlNode(pugi::xml_node& xml_node, const Origin& origin) 667 | { 668 | xml_node.set_name("origin"); 669 | 670 | xml_node.append_attribute("xyz") = (std::to_string(origin.xyz.x) + " " + 671 | std::to_string(origin.xyz.y) + " " + 672 | std::to_string(origin.xyz.z)).c_str(); 673 | xml_node.append_attribute("rpy") = (std::to_string(origin.rpy.x) + " " + 674 | std::to_string(origin.rpy.y) + " " + 675 | std::to_string(origin.rpy.z)).c_str(); 676 | } 677 | 678 | void geometryToXmlNode(pugi::xml_node& xml_node, const Geometry& geometry) 679 | { 680 | xml_node.set_name("geometry"); 681 | 682 | if (std::dynamic_pointer_cast(geometry.type)) { 683 | const Box& box = *std::dynamic_pointer_cast(geometry.type); 684 | pugi::xml_node box_node = xml_node.append_child("box"); 685 | box_node.append_attribute("size") = (std::to_string(box.size.x) + " " + 686 | std::to_string(box.size.y) + " " + 687 | std::to_string(box.size.z)).c_str(); 688 | } else if (std::dynamic_pointer_cast(geometry.type)) { 689 | const Cylinder& cylinder = *std::dynamic_pointer_cast(geometry.type); 690 | pugi::xml_node cylinder_node = xml_node.append_child("cylinder"); 691 | cylinder_node.append_attribute("radius") = std::to_string(cylinder.radius).c_str(); 692 | cylinder_node.append_attribute("length") = std::to_string(cylinder.length).c_str(); 693 | } else if (std::dynamic_pointer_cast(geometry.type)) { 694 | const Sphere& sphere = *std::dynamic_pointer_cast(geometry.type); 695 | pugi::xml_node sphere_node = xml_node.append_child("sphere"); 696 | sphere_node.append_attribute("radius") = std::to_string(sphere.radius).c_str(); 697 | } else { 698 | LOG_F(WARNING, "Geometry type not implemented yet"); 699 | } 700 | } 701 | 702 | void materialToXmlNode(pugi::xml_node& xml_node, const Material& material) 703 | { 704 | xml_node.set_name("material"); 705 | xml_node.append_attribute("name") = material.name.c_str(); 706 | if (material.rgba) { 707 | pugi::xml_node color_node = xml_node.append_child("color"); 708 | color_node.append_attribute("rgba") = (std::to_string(material.rgba->x) + " " + 709 | std::to_string(material.rgba->y) + " " + 710 | std::to_string(material.rgba->z) + " " + 711 | std::to_string(material.rgba->w)).c_str(); 712 | } 713 | if (material.texture_file) { 714 | pugi::xml_node texture_node = xml_node.append_child("texture"); 715 | texture_node.append_attribute("filename") = material.texture_file->c_str(); 716 | } 717 | } 718 | 719 | Robot::Robot(LinkNodePtr root, 720 | const std::map& materials) 721 | : root_(std::move(root)), materials_(materials) 722 | { 723 | 724 | } 725 | 726 | Robot::~Robot() 727 | { 728 | auto func = [&](const LinkNodePtr& link){ 729 | UnloadModel(link->visual_model); 730 | for (const Model& mod : link->collision_models) { 731 | UnloadModel(mod); 732 | } 733 | }; 734 | 735 | forEveryLink(func); 736 | } 737 | 738 | void Robot::forwardKinematics() 739 | { 740 | forwardKinematics(root_); 741 | } 742 | 743 | void Robot::forwardKinematics(LinkNodePtr& link) 744 | { 745 | std::function recursion = [&](LinkNodePtr& node){ 746 | const Matrix& w_t_p = node->w_T_l; 747 | 748 | for (auto& joint_node : node->children) { 749 | // Forward kinematics 750 | const Matrix p_t_j = originToMatrix(joint_node->joint.origin); 751 | const Matrix j_t_c = MatrixIdentity(); // TODO(ramon): joint -> child transform 752 | const Matrix w_t_c = MatMul(w_t_p, MatMul(p_t_j, j_t_c)); 753 | joint_node->child->w_T_l = w_t_c; 754 | 755 | // Update visual transform 756 | const Matrix c_t_v = originToMatrix(joint_node->child->link.visual->origin); 757 | const Matrix w_t_v = MatMul(w_t_c, c_t_v); 758 | joint_node->child->visual_model.transform = w_t_v; 759 | 760 | // Update collision transform 761 | for (size_t i = 0; i < joint_node->child->collision_models.size(); ++i) { 762 | const Matrix c_t_col = originToMatrix(joint_node->child->link.collision[i].origin); 763 | const Matrix w_t_col = MatMul(w_t_c, c_t_col); 764 | joint_node->child->collision_models[i].transform = w_t_col; 765 | } 766 | 767 | recursion(joint_node->child); 768 | } 769 | }; 770 | 771 | recursion(link); 772 | } 773 | 774 | Matrix Robot::originToMatrix(std::optional& origin) 775 | { 776 | Matrix t = MatrixIdentity(); 777 | 778 | if (origin) { 779 | t = origin->toMatrix(); 780 | } 781 | 782 | return t; 783 | } 784 | 785 | void Robot::buildGeometry() 786 | { 787 | auto func = [&](const LinkNodePtr& link){ 788 | // Visual model 789 | if (link->link.visual) { 790 | link->visual_model = link->link.visual->geometry.type->generateGeometry(); 791 | link->visual_model.materials[0].shader = visual_shader_; 792 | updateMaterial(link); 793 | } 794 | 795 | // Collision model 796 | for (const Collision& col : link->link.collision) { 797 | // link->collision_mesh.push_back(col.geometry.type->generateGeometry()); 798 | link->collision_models.push_back(col.geometry.type->generateGeometry()); 799 | } 800 | }; 801 | 802 | forEveryLink(func); 803 | } 804 | 805 | void Robot::updateMaterial(const LinkNodePtr& link) 806 | { 807 | auto set_material_diffuse_color = [](const Model& model, const Vector4& color){ 808 | model.materials[0].maps[MATERIAL_MAP_DIFFUSE].color.r = static_cast(color.x * 255.0F); 809 | model.materials[0].maps[MATERIAL_MAP_DIFFUSE].color.g = static_cast(color.y * 255.0F); 810 | model.materials[0].maps[MATERIAL_MAP_DIFFUSE].color.b = static_cast(color.z * 255.0F); 811 | model.materials[0].maps[MATERIAL_MAP_DIFFUSE].color.a = static_cast(color.w * 255.0F); 812 | }; 813 | 814 | if (link->link.visual->material_name) { 815 | const Material& mat = materials_[*link->link.visual->material_name]; 816 | if (mat.rgba) { 817 | set_material_diffuse_color(link->visual_model, *mat.rgba); 818 | } else { 819 | set_material_diffuse_color(link->visual_model, Vector4{127, 127, 127, 255}); 820 | LOG_F(INFO, "Link: %s. RGBA not found, using default color. Texture is not implemented yet.", link->link.name.c_str()); 821 | } 822 | } else { 823 | set_material_diffuse_color(link->visual_model, Vector4{127, 127, 127, 255}); 824 | LOG_F(INFO, "Link: %s. No material name specified. Using default color", link->link.name.c_str()); 825 | } 826 | } 827 | 828 | void Robot::draw(const LinkNodePtr& highlighted, const LinkNodePtr& selected) const 829 | { 830 | auto draw_link = [&](const LinkNodePtr& link){ 831 | if (link->link.visual and link->visual_model.meshCount > 0) { 832 | if (highlighted.get() == link.get()) { 833 | DrawModel(link->visual_model, Vector3Zero(), 1.0, RED); 834 | } else { 835 | DrawModel(link->visual_model, Vector3Zero(), 1.0, WHITE); 836 | } 837 | } 838 | for (const Model& model : link->collision_models) { 839 | if (selected.get() == link.get()) { 840 | DrawModelWires(model, Vector3Zero(), 1.0, GREEN); 841 | } else { 842 | DrawModelWires(model, Vector3Zero(), 1.0, GRAY); 843 | } 844 | } 845 | }; 846 | 847 | forEveryLink(draw_link); 848 | } 849 | 850 | void Robot::setShader(const Shader& sh) 851 | { 852 | visual_shader_ = sh; 853 | 854 | auto func = [&](const LinkNodePtr& link){ 855 | if (link->visual_model.materialCount > 0) { 856 | if (::Material *mat = link->visual_model.materials) { 857 | mat[0].shader = visual_shader_; 858 | } 859 | } else { 860 | LOG_F(WARNING, "Link %s visual model does not have materials", link->link.name.c_str()); 861 | LOG_F(WARNING, "Link %s has %d meshes", link->link.name.c_str(), link->visual_model.meshCount); 862 | } 863 | }; 864 | 865 | forEveryLink(func); 866 | } 867 | 868 | void Robot::printTree() const 869 | { 870 | auto func = [](const LinkNodePtr& link){ LOG_F(INFO, "%s", link->link.name.c_str()); }; 871 | forEveryLink(func); 872 | } 873 | 874 | LinkNodePtr Robot::getRoot() const 875 | { 876 | return root_; 877 | } 878 | 879 | const std::map& Robot::getMaterials() const 880 | { 881 | return materials_; 882 | } 883 | 884 | void Robot::forEveryLink(const std::function& func) const 885 | { 886 | std::deque deq {root_}; 887 | 888 | while (not deq.empty()) { 889 | const auto& current_link = deq.front(); 890 | deq.pop_front(); 891 | 892 | func(current_link); 893 | 894 | for (const auto& joint : current_link->children) { 895 | deq.push_back(joint->child); 896 | } 897 | } 898 | } 899 | 900 | void Robot::forEveryJoint(const std::function& func) const 901 | { 902 | std::deque deq {root_}; 903 | 904 | while (not deq.empty()) { 905 | const auto& current_link = deq.front(); 906 | deq.pop_front(); 907 | 908 | for (const auto& joint : current_link->children) { 909 | func(joint); 910 | deq.push_back(joint->child); 911 | } 912 | } 913 | } 914 | 915 | } // namespace urdf 916 | --------------------------------------------------------------------------------