From b0c9523316843f6c29c3c466b9552ee09d17edb6 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Wed, 16 Jun 2021 00:28:30 +0530 Subject: [PATCH] Inertia visual (#326) * Add inertia visual classes Signed-off-by: Atharva Pusalkar * Add methods to create inertia visual Signed-off-by: Atharva Pusalkar * Fix box creation and material Signed-off-by: Atharva Pusalkar * Fix comments Signed-off-by: Atharva Pusalkar * Fix override Signed-off-by: Atharva Pusalkar * Add Inertia visual test Signed-off-by: Atharva Pusalkar * Style fixes Signed-off-by: Atharva Pusalkar * Add example Signed-off-by: Atharva Pusalkar * Add Windows support Signed-off-by: Atharva Pusalkar * Remove traces from updateCameras Signed-off-by: Atharva Pusalkar * Rename to visualization_demo Signed-off-by: Atharva Pusalkar * Fix comments Signed-off-by: Atharva Pusalkar * Add SetInertial method Signed-off-by: Atharva Pusalkar * Move code to BaseInertiaVisual::SetInertial Signed-off-by: Atharva Pusalkar * Add Ogre2 destroy method Signed-off-by: Atharva Pusalkar * Fix CMakeLists in example Signed-off-by: Atharva Pusalkar * Fix CMakeLists Signed-off-by: Atharva Pusalkar * Fix crash and test Signed-off-by: Atharva Pusalkar * Fix spaces Signed-off-by: Atharva Pusalkar * Set material depth write to false Signed-off-by: Atharva Pusalkar --- examples/visualization_demo/CMakeLists.txt | 48 ++++ examples/visualization_demo/GlutWindow.cc | 227 ++++++++++++++++++ examples/visualization_demo/GlutWindow.hh | 29 +++ examples/visualization_demo/Main.cc | 207 ++++++++++++++++ include/ignition/rendering/InertiaVisual.hh | 59 +++++ include/ignition/rendering/RenderTypes.hh | 5 + include/ignition/rendering/Scene.hh | 29 +++ .../rendering/base/BaseInertiaVisual.hh | 132 ++++++++++ include/ignition/rendering/base/BaseScene.hh | 21 ++ .../rendering/ogre/OgreInertiaVisual.hh | 89 +++++++ .../rendering/ogre/OgreRenderTypes.hh | 2 + .../ignition/rendering/ogre/OgreScene.hh | 3 + ogre/src/OgreInertiaVisual.cc | 172 +++++++++++++ ogre/src/OgreScene.cc | 10 + .../rendering/ogre2/Ogre2InertiaVisual.hh | 87 +++++++ .../rendering/ogre2/Ogre2RenderTypes.hh | 2 + .../ignition/rendering/ogre2/Ogre2Scene.hh | 4 + ogre2/src/Ogre2DynamicRenderable.cc | 25 +- ogre2/src/Ogre2InertiaVisual.cc | 190 +++++++++++++++ ogre2/src/Ogre2Node.cc | 3 + ogre2/src/Ogre2Scene.cc | 10 + src/InertiaVisual_TEST.cc | 92 +++++++ src/base/BaseScene.cc | 41 ++++ 23 files changed, 1482 insertions(+), 5 deletions(-) create mode 100644 examples/visualization_demo/CMakeLists.txt create mode 100644 examples/visualization_demo/GlutWindow.cc create mode 100644 examples/visualization_demo/GlutWindow.hh create mode 100644 examples/visualization_demo/Main.cc create mode 100644 include/ignition/rendering/InertiaVisual.hh create mode 100644 include/ignition/rendering/base/BaseInertiaVisual.hh create mode 100644 ogre/include/ignition/rendering/ogre/OgreInertiaVisual.hh create mode 100644 ogre/src/OgreInertiaVisual.cc create mode 100644 ogre2/include/ignition/rendering/ogre2/Ogre2InertiaVisual.hh create mode 100644 ogre2/src/Ogre2InertiaVisual.cc create mode 100644 src/InertiaVisual_TEST.cc diff --git a/examples/visualization_demo/CMakeLists.txt b/examples/visualization_demo/CMakeLists.txt new file mode 100644 index 000000000..21474cd5d --- /dev/null +++ b/examples/visualization_demo/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(ignition-rendering-visualization-demo) + +find_package(ignition-rendering6) + +if (APPLE OR UNIX) + find_package(GLUT REQUIRED) + include_directories(SYSTEM ${GLUT_INCLUDE_DIRS}) + link_directories(${GLUT_LIBRARY_DIRS}) + + find_package(OpenGL REQUIRED) + include_directories(SYSTEM ${OpenGL_INCLUDE_DIRS}) + link_directories(${OpenGL_LIBRARY_DIRS}) + + set(TARGET_THIRD_PARTY_DEPENDS + ${TARGET_THIRD_PARTY_DEPENDS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARIES} + ) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations") +endif() + +if (NOT APPLE) + find_package(GLEW REQUIRED) + if (WIN32) + set(TARGET_THIRD_PARTY_DEPENDS + ${TARGET_THIRD_PARTY_DEPENDS} + GLEW::glew + ) + else () + set(TARGET_THIRD_PARTY_DEPENDS + ${TARGET_THIRD_PARTY_DEPENDS} + GLEW + ) + endif() +endif() + +if (WIN32) + find_package(FreeGLUT REQUIRED) + set(TARGET_THIRD_PARTY_DEPENDS ${TARGET_THIRD_PARTY_DEPENDS} FreeGLUT::freeglut) +endif() + +add_executable(visualization_demo Main.cc GlutWindow.cc) + +target_link_libraries(visualization_demo + ${IGNITION-RENDERING_LIBRARIES} + ${TARGET_THIRD_PARTY_DEPENDS} +) diff --git a/examples/visualization_demo/GlutWindow.cc b/examples/visualization_demo/GlutWindow.cc new file mode 100644 index 000000000..5aae62c70 --- /dev/null +++ b/examples/visualization_demo/GlutWindow.cc @@ -0,0 +1,227 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#if __APPLE__ + #include + #include + #include +#elif _WIN32 + #define NOMINMAX + // Must include this before GL/gl.h + #include + #include + // OpenGL utilities header file + #include + #include "Wingdi.h" +#else + #include + #include + #include +#endif + +#if !defined(__APPLE__) && !defined(_WIN32) + #include +#endif + +#include + +#include +#include +#include +#include + +#include "GlutWindow.hh" + +#define KEY_ESC 27 +#define KEY_TAB 9 + +////////////////////////////////////////////////// +unsigned int imgw = 0; +unsigned int imgh = 0; + +std::vector g_cameras; +ir::CameraPtr g_camera; +ir::CameraPtr g_currCamera; +unsigned int g_cameraIndex = 0; +ir::ImagePtr g_image; + +bool g_initContext = false; + +#if __APPLE__ + CGLContextObj g_context; + CGLContextObj g_glutContext; +#elif _WIN32 + HGLRC g_context = 0; + HDC g_display = 0; + HGLRC g_glutContext = 0; + HDC g_glutDisplay = 0; +#else + GLXContext g_context; + Display *g_display; + GLXDrawable g_drawable; + GLXContext g_glutContext; + Display *g_glutDisplay; + GLXDrawable g_glutDrawable; +#endif + +double g_offset = 0.0; + +////////////////////////////////////////////////// +void updateCameras() +{ + double angle = g_offset / 2 * M_PI; + double x = sin(angle) * 3.0 + 3.0; + double y = cos(angle) * 3.0; + for (ir::CameraPtr camera : g_cameras) + { + camera->SetLocalPosition(x, y, 0.0); + } + + g_offset += 0.0005; +} + +////////////////////////////////////////////////// +void displayCB() +{ +#if __APPLE__ + CGLSetCurrentContext(g_context); +#elif _WIN32 + if(!wglMakeCurrent(g_display, g_context)) + { + std::cerr << "Not able to wglMakeCurrent" << '\n'; + exit(-1); + } +#else + if (g_display) + { + glXMakeCurrent(g_display, g_drawable, g_context); + } +#endif + + g_cameras[g_cameraIndex]->Capture(*g_image); + +#if __APPLE__ + CGLSetCurrentContext(g_glutContext); +#elif _WIN32 + wglMakeCurrent(g_glutDisplay, g_glutContext); +#else + glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext); +#endif + + unsigned char *data = g_image->Data(); + + glClearColor(0.5, 0.5, 0.5, 1); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPixelZoom(1, -1); + glRasterPos2f(-1, 1); + glDrawPixels(imgw, imgh, GL_RGB, GL_UNSIGNED_BYTE, data); + + glutSwapBuffers(); + updateCameras(); +} + +////////////////////////////////////////////////// +void idleCB() +{ + glutPostRedisplay(); +} + +////////////////////////////////////////////////// +void keyboardCB(unsigned char _key, int, int) +{ + if (_key == KEY_ESC || _key == 'q' || _key == 'Q') + { + exit(0); + } + else if (_key == KEY_TAB) + { + g_cameraIndex = (g_cameraIndex + 1) % g_cameras.size(); + } +} + +////////////////////////////////////////////////// +void initCamera(ir::CameraPtr _camera) +{ + g_camera = _camera; + imgw = g_camera->ImageWidth(); + imgh = g_camera->ImageHeight(); + ir::Image image = g_camera->CreateImage(); + g_image = std::make_shared(image); + g_camera->Capture(*g_image); +} + +////////////////////////////////////////////////// +void initContext() +{ + glutInitDisplayMode(GLUT_DOUBLE); + glutInitWindowPosition(0, 0); + glutInitWindowSize(imgw, imgh); + glutCreateWindow("Visualization Demo"); + glutDisplayFunc(displayCB); + glutIdleFunc(idleCB); + glutKeyboardFunc(keyboardCB); +} + +////////////////////////////////////////////////// +void printUsage() +{ + std::cout << "===============================" << std::endl; + std::cout << " TAB - Switch render engines " << std::endl; + std::cout << " ESC - Exit " << std::endl; + std::cout << "===============================" << std::endl; +} + +////////////////////////////////////////////////// +void run(std::vector _cameras) +{ + if (_cameras.empty()) + { + ignerr << "No cameras found. Scene will not be rendered" << std::endl; + return; + } + +#if __APPLE__ + g_context = CGLGetCurrentContext(); +#elif _WIN32 + g_context = wglGetCurrentContext(); + g_display = wglGetCurrentDC(); +#else + g_context = glXGetCurrentContext(); + g_display = glXGetCurrentDisplay(); + g_drawable = glXGetCurrentDrawable(); +#endif + + g_cameras = _cameras; + initCamera(_cameras[0]); + initContext(); + printUsage(); + +#if __APPLE__ + g_glutContext = CGLGetCurrentContext(); +#elif _WIN32 + g_glutContext = wglGetCurrentContext(); + g_glutDisplay = wglGetCurrentDC(); +#else + g_glutDisplay = glXGetCurrentDisplay(); + g_glutDrawable = glXGetCurrentDrawable(); + g_glutContext = glXGetCurrentContext(); +#endif + + glutMainLoop(); +} diff --git a/examples/visualization_demo/GlutWindow.hh b/examples/visualization_demo/GlutWindow.hh new file mode 100644 index 000000000..2c636d1a9 --- /dev/null +++ b/examples/visualization_demo/GlutWindow.hh @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_EXAMPLES_VISUALIZATION_DEMO_GLUTWINDOW_HH_ +#define IGNITION_RENDERING_EXAMPLES_VISUALIZATION_DEMO_GLUTWINDOW_HH_ + +#include +#include "ignition/rendering/RenderTypes.hh" + +namespace ir = ignition::rendering; + +/// \brief Run the demo and render the scene from the cameras +/// \param[in] _cameras Cameras in the scene +void run(std::vector _cameras); + +#endif diff --git a/examples/visualization_demo/Main.cc b/examples/visualization_demo/Main.cc new file mode 100644 index 000000000..68540c77f --- /dev/null +++ b/examples/visualization_demo/Main.cc @@ -0,0 +1,207 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#if defined(__APPLE__) + #include + #include +#elif _WIN32 + #define NOMINMAX + // Must include this before GL/gl.h + #include + #include + // OpenGL utilities header file + #include + // OpenGL utilities header file + #include +#else + #include + #include + #include +#endif + +#include +#include + +#include +#include + +#include "GlutWindow.hh" + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +void buildScene(ScenePtr _scene) +{ + // initialize _scene + _scene->SetAmbientLight(0.3, 0.3, 0.3); + VisualPtr root = _scene->RootVisual(); + + // create directional light + DirectionalLightPtr light0 = _scene->CreateDirectionalLight(); + light0->SetDirection(-0.5, 0.5, -1); + light0->SetDiffuseColor(0.5, 0.5, 0.5); + light0->SetSpecularColor(0.5, 0.5, 0.5); + root->AddChild(light0); + + // create point light + PointLightPtr light1 = _scene->CreatePointLight(); + light1->SetDiffuseColor(0.5, 0.5, 0.5); + light1->SetSpecularColor(0.5, 0.5, 0.5); + light1->SetLocalPosition(5, -5, 10); + root->AddChild(light1); + + // create point light + PointLightPtr light2 = _scene->CreatePointLight(); + light2->SetDiffuseColor(0.5, 0.5, 0.5); + light2->SetSpecularColor(0.5, 0.5, 0.5); + light2->SetLocalPosition(3, 5, 5); + root->AddChild(light2); + + // create green material + MaterialPtr green = _scene->CreateMaterial(); + green->SetAmbient(0.0, 0.5, 0.0); + green->SetDiffuse(0.0, 0.7, 0.0); + green->SetSpecular(0.5, 0.5, 0.5); + green->SetShininess(50); + green->SetReflectivity(0); + + // create center visual + VisualPtr center = _scene->CreateVisual(); + center->AddGeometry(_scene->CreateSphere()); + center->SetLocalPosition(3, 0, 0); + center->SetLocalScale(0.1, 0.1, 0.1); + center->SetMaterial(green); + root->AddChild(center); + + // create red material + MaterialPtr red = _scene->CreateMaterial(); + red->SetAmbient(0.5, 0.0, 0.0); + red->SetDiffuse(1.0, 0.0, 0.0); + red->SetSpecular(0.5, 0.5, 0.5); + red->SetShininess(50); + red->SetReflectivity(0); + red->SetRenderOrder(3); + + // create sphere visual + VisualPtr sphere = _scene->CreateVisual(); + sphere->AddGeometry(_scene->CreateSphere()); + sphere->SetOrigin(0.0, -0.5, 0.0); + sphere->SetLocalPosition(3, -1, 0); + sphere->SetLocalRotation(0, 0, 0); + sphere->SetLocalScale(1, 1, 1); + sphere->SetMaterial(red); + sphere->SetWireframe(true); + root->AddChild(sphere); + + // create white material + MaterialPtr white = _scene->CreateMaterial(); + white->SetAmbient(0.5, 0.5, 0.5); + white->SetDiffuse(0.8, 0.8, 0.8); + white->SetReceiveShadows(true); + white->SetReflectivity(0); + white->SetRenderOrder(0); + + // create plane visual + VisualPtr plane = _scene->CreateVisual(); + plane->AddGeometry(_scene->CreatePlane()); + plane->SetLocalScale(5, 8, 1); + plane->SetLocalPosition(3, 0, -0.5); + plane->SetMaterial(white); + root->AddChild(plane); + + // create inertia visual + InertiaVisualPtr inertiaVisual = _scene->CreateInertiaVisual(); + ignition::math::MassMatrix3d massMatrix(1.0, {0.1, 0.1, 0.1}, {0.0, 0.0, 0.0}); + ignition::math::Pose3d p(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + ignition::math::Inertiald inertial{massMatrix, p}; + inertiaVisual->SetInertial(inertial); + inertiaVisual->SetLocalPosition(3.2, 1.5, 0); + root->AddChild(inertiaVisual); + + // create camera + CameraPtr camera = _scene->CreateCamera("camera"); + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetImageWidth(800); + camera->SetImageHeight(600); + camera->SetAntiAliasing(2); + camera->SetAspectRatio(1.333); + camera->SetHFOV(IGN_PI / 2); + root->AddChild(camera); + + // track target + camera->SetTrackTarget(center); +} + +////////////////////////////////////////////////// +CameraPtr createCamera(const std::string &_engineName) +{ + // create and populate scene + RenderEngine *engine = rendering::engine(_engineName); + if (!engine) + { + std::cout << "Engine '" << _engineName + << "' is not supported" << std::endl; + return CameraPtr(); + } + ScenePtr scene = engine->CreateScene("scene"); + buildScene(scene); + + // return camera sensor + SensorPtr sensor = scene->SensorByName("camera"); + return std::dynamic_pointer_cast(sensor); +} + +////////////////////////////////////////////////// +int main(int _argc, char** _argv) +{ + glutInit(&_argc, _argv); + + // Expose engine name to command line because we can't instantiate both + // ogre and ogre2 at the same time + std::string engine("ogre"); + if (_argc > 1) + { + engine = _argv[1]; + } + + common::Console::SetVerbosity(4); + std::vector engineNames; + std::vector cameras; + + engineNames.push_back(engine); + engineNames.push_back("optix"); + + for (auto engineName : engineNames) + { + try + { + CameraPtr camera = createCamera(engineName); + if (camera) + { + cameras.push_back(camera); + } + } + catch (...) + { + std::cerr << "Error starting up: " << engineName << std::endl; + } + } + run(cameras); + return 0; +} diff --git a/include/ignition/rendering/InertiaVisual.hh b/include/ignition/rendering/InertiaVisual.hh new file mode 100644 index 000000000..2b233aef0 --- /dev/null +++ b/include/ignition/rendering/InertiaVisual.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_INERTIAVISUAL_HH_ +#define IGNITION_RENDERING_INERTIAVISUAL_HH_ + +#include +#include "ignition/rendering/config.hh" +#include "ignition/rendering/Object.hh" +#include "ignition/rendering/RenderTypes.hh" +#include "ignition/rendering/Visual.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + + /// \class InertiaVisual InertiaVisual.hh + /// ignition/rendering/InertiaVisual.hh + /// \brief Represents a inertia visual + class IGNITION_RENDERING_VISIBLE InertiaVisual : + public virtual Visual + { + /// \brief Destructor + public: virtual ~InertiaVisual() {} + + /// \brief Set the inertial component of the visual + /// \param[in] _inertial Inertial component of the visual + public: virtual void SetInertial( + const ignition::math::Inertiald &_inertial) = 0; + + /// \brief Load the Inertia visual from its pose and scale + /// \param[in] _pose Pose of the Inertia visual + /// \param[in] _scale Scale factor of the box visual + public: virtual void Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) = 0; + + /// \brief Get the box visual + /// \return Pointer to the box visual + public: virtual VisualPtr BoxVisual() const = 0; + }; + } + } +} +#endif diff --git a/include/ignition/rendering/RenderTypes.hh b/include/ignition/rendering/RenderTypes.hh index 23ff5e202..b332cc6b6 100644 --- a/include/ignition/rendering/RenderTypes.hh +++ b/include/ignition/rendering/RenderTypes.hh @@ -59,6 +59,7 @@ namespace ignition class Grid; class Heightmap; class Image; + class InertiaVisual; class Light; class LightVisual; class JointVisual; @@ -149,6 +150,10 @@ namespace ignition /// \brief Shared pointer to Image typedef shared_ptr ImagePtr; + /// \def InertiaVisualPtr + /// \def Shared pointer to InertiaVisual + typedef shared_ptr InertiaVisualPtr; + /// \def LightPtr /// \brief Shared pointer to Light typedef shared_ptr LightPtr; diff --git a/include/ignition/rendering/Scene.hh b/include/ignition/rendering/Scene.hh index b61404580..e1e915096 100644 --- a/include/ignition/rendering/Scene.hh +++ b/include/ignition/rendering/Scene.hh @@ -823,6 +823,35 @@ namespace ignition public: virtual GizmoVisualPtr CreateGizmoVisual( unsigned int _id, const std::string &_name) = 0; + /// \brief Create new inertia visual. A unique ID and name will + /// automatically be assigned to the inertia visual. + /// \return The created inertia visual + public: virtual InertiaVisualPtr CreateInertiaVisual() = 0; + + /// \brief Create new inertia visual with the given ID. A unique name + /// will automatically be assigned to the visual. If the given ID is + /// already in use, NULL will be returned. + /// \param[in] _id ID of the new inertia visual + /// \return The created light visual + public: virtual InertiaVisualPtr CreateInertiaVisual( + unsigned int _id) = 0; + + /// \brief Create new inertia visual with the given name. A unique ID + /// will automatically be assigned to the visual. If the given name is + /// already in use, NULL will be returned. + /// \param[in] _name Name of the new inertia visual + /// \return The created light visual + public: virtual InertiaVisualPtr CreateInertiaVisual( + const std::string &_name) = 0; + + /// \brief Create new inertia visual with the given name. If either the + /// given ID or name is already in use, NULL will be returned. + /// \param[in] _id ID of the new inertia visual + /// \param[in] _name Name of the new inertia visual + /// \return The created inertia visual + public: virtual InertiaVisualPtr CreateInertiaVisual( + unsigned int _id, const std::string &_name) = 0; + /// \brief Create new light visual. A unique ID and name will /// automatically be assigned to the light visual. /// \return The created light visual diff --git a/include/ignition/rendering/base/BaseInertiaVisual.hh b/include/ignition/rendering/base/BaseInertiaVisual.hh new file mode 100644 index 000000000..9be984395 --- /dev/null +++ b/include/ignition/rendering/base/BaseInertiaVisual.hh @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_BASE_BASEINERTIAVISUAL_HH_ +#define IGNITION_RENDERING_BASE_BASEINERTIAVISUAL_HH_ + +#include "ignition/common/Console.hh" + +#include "ignition/rendering/base/BaseObject.hh" +#include "ignition/rendering/base/BaseRenderTypes.hh" +#include "ignition/rendering/InertiaVisual.hh" +#include "ignition/rendering/Scene.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Base implementation of an inertia visual + template + class BaseInertiaVisual : + public virtual InertiaVisual, + public virtual T + { + /// \brief Constructor + protected: BaseInertiaVisual(); + + /// \brief Destructor + public: virtual ~BaseInertiaVisual(); + + // Documentation inherited. + protected: virtual void Init() override; + + // Documentation inherited. + protected: virtual void PreRender() override; + + // Documentation inherited. + public: virtual void SetInertial( + const ignition::math::Inertiald &_inertial) override; + + // Documentation inherited. + public: virtual void Load(const ignition::math::Pose3d &, + const ignition::math::Vector3d &) override; + + // Documentation inherited + public: virtual VisualPtr BoxVisual() const override; + }; + + ////////////////////////////////////////////////// + template + BaseInertiaVisual::BaseInertiaVisual() + { + } + + ////////////////////////////////////////////////// + template + BaseInertiaVisual::~BaseInertiaVisual() + { + } + + ///////////////////////////////////////////////// + template + void BaseInertiaVisual::PreRender() + { + T::PreRender(); + } + + ////////////////////////////////////////////////// + template + void BaseInertiaVisual::Init() + { + T::Init(); + } + + ////////////////////////////////////////////////// + template + void BaseInertiaVisual::SetInertial( + const ignition::math::Inertiald &_inertial) + { + auto xyz = _inertial.Pose().Pos(); + auto q = _inertial.Pose().Rot(); + + // Use ignition::math::MassMatrix3 to compute + // equivalent box size and rotation + auto m = _inertial.MassMatrix(); + ignition::math::Vector3d boxScale; + ignition::math::Quaterniond boxRot; + if (!m.EquivalentBox(boxScale, boxRot)) + { + // Invalid inertia, load with default scale + ignlog << "The link is static or has unrealistic " + << "inertia, so the equivalent inertia box will not be shown.\n"; + } + else + { + // Apply additional rotation by boxRot + this->Load(ignition::math::Pose3d(xyz, q * boxRot), boxScale); + } + } + + ////////////////////////////////////////////////// + template + void BaseInertiaVisual::Load(const ignition::math::Pose3d &, + const ignition::math::Vector3d &) + { + // no op + } + + ////////////////////////////////////////////////// + template + VisualPtr BaseInertiaVisual::BoxVisual() const + { + return nullptr; + } + } + } +} +#endif diff --git a/include/ignition/rendering/base/BaseScene.hh b/include/ignition/rendering/base/BaseScene.hh index 31670265b..d47276408 100644 --- a/include/ignition/rendering/base/BaseScene.hh +++ b/include/ignition/rendering/base/BaseScene.hh @@ -265,6 +265,12 @@ namespace ignition public: virtual PointLightPtr CreatePointLight(unsigned int _id, const std::string &_name) override; + /// \brief Implementation for creating Inertia visual. + /// \param[in] _id Unique id + /// \param[in] _name Name of inertia visual + protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( + unsigned int _id, const std::string &_name) = 0; + /// \brief Implementation for creating Light visual. /// \param[in] _id Unique id /// \param[in] _name Name of light visual @@ -360,6 +366,21 @@ namespace ignition public: virtual AxisVisualPtr CreateAxisVisual(unsigned int _id, const std::string &_name) override; + // Documentation inherited + public: virtual InertiaVisualPtr CreateInertiaVisual() override; + + // Documentation inherited + public: virtual InertiaVisualPtr CreateInertiaVisual(unsigned int _id) + override; + + // Documentation inherited + public: virtual InertiaVisualPtr CreateInertiaVisual( + const std::string &_name) override; + + // Documentation inherited + public: virtual InertiaVisualPtr CreateInertiaVisual(unsigned int _id, + const std::string &_name) override; + // Documentation inherited public: virtual LightVisualPtr CreateLightVisual() override; diff --git a/ogre/include/ignition/rendering/ogre/OgreInertiaVisual.hh b/ogre/include/ignition/rendering/ogre/OgreInertiaVisual.hh new file mode 100644 index 000000000..75b0c78eb --- /dev/null +++ b/ogre/include/ignition/rendering/ogre/OgreInertiaVisual.hh @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_RENDERING_OGRE_OGREINERTIAVISUAL_HH_ +#define IGNITION_RENDERING_OGRE_OGREINERTIAVISUAL_HH_ + +#include + +#include "ignition/rendering/base/BaseInertiaVisual.hh" +#include "ignition/rendering/ogre/OgreIncludes.hh" +#include "ignition/rendering/ogre/OgreMaterial.hh" +#include "ignition/rendering/ogre/OgreVisual.hh" + +namespace Ogre +{ + class MovableObject; +} + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + + // Forward declaration + class OgreInertiaVisualPrivate; + + class IGNITION_RENDERING_OGRE_VISIBLE OgreInertiaVisual : + public BaseInertiaVisual + { + /// \brief Constructor + protected: OgreInertiaVisual(); + + /// \brief Destructor + public: virtual ~OgreInertiaVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + public: Ogre::MovableObject *OgreObject() const; + + /// \brief Load the Inertia visual from its pose and scale + /// \param[in] _pose Pose of the Inertia visual + /// \param[in] _scale Scale factor of the box visual + public: void Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) override; + + /// \brief Get the box visual + /// \return Pointer to the box visual + public: VisualPtr BoxVisual() const override; + + // Documentation inherited. + public: virtual MaterialPtr Material() const; + + // Documentation inherited. + public: virtual void SetMaterial( + MaterialPtr _material, bool _unique) override; + + /// \brief Set material to line geometry. + /// \param[in] _material Ogre material. + protected: virtual void SetMaterialImpl(OgreMaterialPtr _material); + + private: friend class OgreScene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh index 3743b77de..40736ab43 100644 --- a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh +++ b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh @@ -37,6 +37,7 @@ namespace ignition class OgreGpuRays; class OgreGrid; class OgreHeightmap; + class OgreInertiaVisual; class OgreJointVisual; class OgreLight; class OgreLightVisual; @@ -86,6 +87,7 @@ namespace ignition typedef shared_ptr OgreGpuRaysPtr; typedef shared_ptr OgreGridPtr; typedef shared_ptr OgreHeightmapPtr; + typedef shared_ptr OgreInertiaVisualPtr; typedef shared_ptr OgreJointVisualPtr; typedef shared_ptr OgreLightPtr; typedef shared_ptr OgreLightVisualPtr; diff --git a/ogre/include/ignition/rendering/ogre/OgreScene.hh b/ogre/include/ignition/rendering/ogre/OgreScene.hh index fca6a5c2d..9d1282682 100644 --- a/ogre/include/ignition/rendering/ogre/OgreScene.hh +++ b/ogre/include/ignition/rendering/ogre/OgreScene.hh @@ -79,6 +79,9 @@ namespace ignition unsigned int _id, const std::string &_name) override; // Documentation inherited + protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( + unsigned int _id, const std::string &_name) override; + protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre/src/OgreInertiaVisual.cc b/ogre/src/OgreInertiaVisual.cc new file mode 100644 index 000000000..d95571bc7 --- /dev/null +++ b/ogre/src/OgreInertiaVisual.cc @@ -0,0 +1,172 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "ignition/rendering/ogre/OgreInertiaVisual.hh" +#include "ignition/rendering/ogre/OgreDynamicLines.hh" + +#include "ignition/rendering/ogre/OgreScene.hh" + +class ignition::rendering::OgreInertiaVisualPrivate +{ + /// \brief Grid materal + public: OgreMaterialPtr material = nullptr; + + /// \brief Ogre renderable used to render the cross lines. + public: std::shared_ptr crossLines = nullptr; + + /// \brief Box visual + public: VisualPtr boxVis = nullptr; +}; + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +OgreInertiaVisual::OgreInertiaVisual() + : dataPtr(new OgreInertiaVisualPrivate) +{ +} + +////////////////////////////////////////////////// +OgreInertiaVisual::~OgreInertiaVisual() +{ +} + +////////////////////////////////////////////////// +void OgreInertiaVisual::PreRender() +{ +} + +////////////////////////////////////////////////// +void OgreInertiaVisual::Init() +{ + BaseInertiaVisual::Init(); +} + +////////////////////////////////////////////////// +Ogre::MovableObject *OgreInertiaVisual::OgreObject() const +{ + std::shared_ptr mv = + std::dynamic_pointer_cast(this->dataPtr->crossLines); + return mv.get(); +} + +////////////////////////////////////////////////// +void OgreInertiaVisual::Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) +{ + if (!this->dataPtr->crossLines) + { + this->dataPtr->crossLines = std::shared_ptr( + new OgreDynamicLines(MT_LINE_LIST)); + this->ogreNode->attachObject(this->OgreObject()); + MaterialPtr inertiaVisualMaterial = + this->Scene()->Material("Default/TransGreen"); + this->SetMaterial(inertiaVisualMaterial, false); + } + + if (!this->dataPtr->boxVis) + { + this->dataPtr->boxVis = this->Scene()->CreateVisual(); + this->dataPtr->boxVis->AddGeometry(this->Scene()->CreateBox()); + this->dataPtr->boxVis->SetMaterial("Default/TransPurple"); + this->AddChild(this->dataPtr->boxVis); + } + + // Inertia position indicator + ignition::math::Vector3d p1(0, 0, -2*_scale.Z()); + ignition::math::Vector3d p2(0, 0, 2*_scale.Z()); + ignition::math::Vector3d p3(0, -2*_scale.Y(), 0); + ignition::math::Vector3d p4(0, 2*_scale.Y(), 0); + ignition::math::Vector3d p5(-2*_scale.X(), 0, 0); + ignition::math::Vector3d p6(2*_scale.X(), 0, 0); + p1 = _pose.Rot().RotateVector(p1); + p2 = _pose.Rot().RotateVector(p2); + p3 = _pose.Rot().RotateVector(p3); + p4 = _pose.Rot().RotateVector(p4); + p5 = _pose.Rot().RotateVector(p5); + p6 = _pose.Rot().RotateVector(p6); + p1 += _pose.Pos(); + p2 += _pose.Pos(); + p3 += _pose.Pos(); + p4 += _pose.Pos(); + p5 += _pose.Pos(); + p6 += _pose.Pos(); + + this->dataPtr->crossLines->AddPoint(p1); + this->dataPtr->crossLines->AddPoint(p2); + this->dataPtr->crossLines->AddPoint(p3); + this->dataPtr->crossLines->AddPoint(p4); + this->dataPtr->crossLines->AddPoint(p5); + this->dataPtr->crossLines->AddPoint(p6); + + this->dataPtr->crossLines->Update(); + this->ogreNode->setVisible(true); + + this->dataPtr->boxVis->SetLocalScale(_scale); + this->dataPtr->boxVis->SetLocalPosition(_pose.Pos()); + this->dataPtr->boxVis->SetLocalRotation(_pose.Rot()); +} + +////////////////////////////////////////////////// +VisualPtr OgreInertiaVisual::BoxVisual() const +{ + return this->dataPtr->boxVis; +} + +////////////////////////////////////////////////// +void OgreInertiaVisual::SetMaterial(MaterialPtr _material, bool _unique) +{ + _material = (_unique) ? _material->Clone() : _material; + + OgreMaterialPtr derived = + std::dynamic_pointer_cast(_material); + + if (!derived) + { + ignerr << "Cannot assign material created by another render-engine" + << std::endl; + + return; + } + + this->SetMaterialImpl(derived); +} + +////////////////////////////////////////////////// +void OgreInertiaVisual::SetMaterialImpl(OgreMaterialPtr _material) +{ + std::string materialName = _material->Name(); + Ogre::MaterialPtr ogreMaterial = _material->Material(); + +// OGRE 1.10.7 +#if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + this->dataPtr->crossLines->setMaterial(materialName); +#else + this->dataPtr->crossLines->setMaterial(ogreMaterial); +#endif + this->dataPtr->material = _material; + + this->dataPtr->material->SetReceiveShadows(false); + this->dataPtr->material->SetLightingEnabled(false); +} + +////////////////////////////////////////////////// +MaterialPtr OgreInertiaVisual::Material() const +{ + return this->dataPtr->material; +} diff --git a/ogre/src/OgreScene.cc b/ogre/src/OgreScene.cc index ea13d4ae8..e3351befb 100644 --- a/ogre/src/OgreScene.cc +++ b/ogre/src/OgreScene.cc @@ -29,6 +29,7 @@ #include "ignition/rendering/ogre/OgreGrid.hh" #include "ignition/rendering/ogre/OgreHeightmap.hh" #include "ignition/rendering/ogre/OgreIncludes.hh" +#include "ignition/rendering/ogre/OgreInertiaVisual.hh" #include "ignition/rendering/ogre/OgreLidarVisual.hh" #include "ignition/rendering/ogre/OgreLightVisual.hh" #include "ignition/rendering/ogre/OgreMarker.hh" @@ -375,6 +376,15 @@ PointLightPtr OgreScene::CreatePointLightImpl(unsigned int _id, return (result) ? light : nullptr; } +////////////////////////////////////////////////// +InertiaVisualPtr OgreScene::CreateInertiaVisualImpl(unsigned int _id, + const std::string &_name) +{ + OgreInertiaVisualPtr visual(new OgreInertiaVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr OgreScene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2InertiaVisual.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2InertiaVisual.hh new file mode 100644 index 000000000..d18511c1c --- /dev/null +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2InertiaVisual.hh @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_OGRE2_OGRE2INERTIAVISUAL_HH_ +#define IGNITION_RENDERING_OGRE2_OGRE2INERTIAVISUAL_HH_ + +#include + +#include "ignition/rendering/base/BaseInertiaVisual.hh" +#include "ignition/rendering/ogre2/Ogre2Visual.hh" + +namespace Ogre +{ + class MovableObject; +} + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + + // Forward declaration + class Ogre2InertiaVisualPrivate; + + /// \brief Ogre2.x implementation of the inertia visual class + class IGNITION_RENDERING_OGRE2_VISIBLE Ogre2InertiaVisual : + public BaseInertiaVisual + { + /// \brief Constructor + protected: Ogre2InertiaVisual(); + + /// \brief Destructor + public: virtual ~Ogre2InertiaVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + protected: virtual void Destroy() override; + + /// \brief Load the Inertia visual from its pose and scale + /// \param[in] _pose Pose of the Inertia visual + /// \param[in] _scale Scale factor of the box visual + public: void Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) override; + + /// \brief Get the box visual + /// \return Pointer to the box visual + public: VisualPtr BoxVisual() const override; + + // Documentation inherited. + public: virtual MaterialPtr Material() const; + + // Documentation inherited. + public: virtual void SetMaterial( + MaterialPtr _material, bool _unique) override; + + /// \brief Set material to line geometry. + /// \param[in] _material Ogre material. + protected: virtual void SetMaterialImpl(Ogre2MaterialPtr _material); + + private: friend class Ogre2Scene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh index 115c9d69b..65885dd1e 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh @@ -38,6 +38,7 @@ namespace ignition class Ogre2GizmoVisual; class Ogre2GpuRays; class Ogre2Grid; + class Ogre2InertiaVisual; class Ogre2Light; class Ogre2LightVisual; class Ogre2LidarVisual; @@ -83,6 +84,7 @@ namespace ignition typedef shared_ptr Ogre2GizmoVisualPtr; typedef shared_ptr Ogre2GpuRaysPtr; typedef shared_ptr Ogre2GridPtr; + typedef shared_ptr Ogre2InertiaVisualPtr; typedef shared_ptr Ogre2LightPtr; typedef shared_ptr Ogre2LightVisualPtr; typedef shared_ptr Ogre2LidarVisualPtr; diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh index e4d05a2f9..2570e857e 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh @@ -119,6 +119,10 @@ namespace ignition // Documentation inherited protected: virtual bool InitImpl() override; + // Documentation inherited + protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( + unsigned int _id, const std::string &_name) override; + // Documentation inherited protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre2/src/Ogre2DynamicRenderable.cc b/ogre2/src/Ogre2DynamicRenderable.cc index c300c5d6f..90ffbedd6 100644 --- a/ogre2/src/Ogre2DynamicRenderable.cc +++ b/ogre2/src/Ogre2DynamicRenderable.cc @@ -130,6 +130,16 @@ void Ogre2DynamicRenderable::Destroy() this->dataPtr->sceneManager->destroyItem(this->dataPtr->ogreItem); this->dataPtr->ogreItem = nullptr; + // remove mesh from mesh manager + if (this->dataPtr->subMesh && + Ogre::MeshManager::getSingleton().resourceExists( + this->dataPtr->subMesh->mParent->getName())) + { + Ogre::MeshManager::getSingleton().remove( + this->dataPtr->subMesh->mParent->getName()); + this->dataPtr->subMesh = nullptr; + } + if (this->dataPtr->material && this->dataPtr->ownsMaterial) { this->dataPtr->scene->DestroyMaterial(this->dataPtr->material); @@ -150,11 +160,16 @@ void Ogre2DynamicRenderable::DestroyBuffer() if (!vaoManager) return; - if (this->dataPtr->vertexBuffer) - vaoManager->destroyVertexBuffer(this->dataPtr->vertexBuffer); - - if (this->dataPtr->vao) - vaoManager->destroyVertexArrayObject(this->dataPtr->vao); + if (this->dataPtr->subMesh) + { + if (!this->dataPtr->subMesh->mVao[Ogre::VpNormal].empty()) + { + this->dataPtr->subMesh->destroyVaos( + this->dataPtr->subMesh->mVao[Ogre::VpNormal], vaoManager); + } + if (!this->dataPtr->subMesh->mVao[Ogre::VpShadow].empty()) + this->dataPtr->subMesh->mVao[Ogre::VpShadow].clear(); + } this->dataPtr->vertexBuffer = nullptr; this->dataPtr->vao = nullptr; diff --git a/ogre2/src/Ogre2InertiaVisual.cc b/ogre2/src/Ogre2InertiaVisual.cc new file mode 100644 index 000000000..3a623a279 --- /dev/null +++ b/ogre2/src/Ogre2InertiaVisual.cc @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include "ignition/rendering/ogre2/Ogre2InertiaVisual.hh" +#include "ignition/rendering/ogre2/Ogre2Material.hh" +#include "ignition/rendering/ogre2/Ogre2DynamicRenderable.hh" + +#ifdef _MSC_VER + #pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER + #pragma warning(pop) +#endif + +using namespace ignition; +using namespace rendering; + +class ignition::rendering::Ogre2InertiaVisualPrivate +{ + /// \brief inertia visual materal + public: Ogre2MaterialPtr material = nullptr; + + /// \brief Ogre renderable used to render the cross lines. + public: std::shared_ptr crossLines = nullptr; + + /// \brief Box visual + public: VisualPtr boxVis = nullptr; +}; + +////////////////////////////////////////////////// +Ogre2InertiaVisual::Ogre2InertiaVisual() + : dataPtr(new Ogre2InertiaVisualPrivate) +{ +} + +////////////////////////////////////////////////// +Ogre2InertiaVisual::~Ogre2InertiaVisual() +{ +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::PreRender() +{ +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::Init() +{ + BaseInertiaVisual::Init(); +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::Destroy() +{ + if (this->dataPtr->boxVis != nullptr) + { + this->dataPtr->boxVis->Destroy(); + this->dataPtr->boxVis.reset(); + } + + if (this->dataPtr->crossLines) + { + this->dataPtr->crossLines->Destroy(); + this->dataPtr->crossLines.reset(); + } + + if (this->dataPtr->material && this->Scene()) + { + this->Scene()->DestroyMaterial(this->dataPtr->material); + this->dataPtr->material.reset(); + } +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) +{ + if (!this->dataPtr->crossLines) + { + this->dataPtr->crossLines.reset( + new Ogre2DynamicRenderable(this->Scene())); + this->ogreNode->attachObject(this->dataPtr->crossLines->OgreObject()); + } + + if (!this->dataPtr->boxVis) + { + this->dataPtr->boxVis = this->Scene()->CreateVisual(); + this->dataPtr->boxVis->AddGeometry(this->Scene()->CreateBox()); + this->dataPtr->boxVis->SetMaterial("Default/TransPurple"); + this->AddChild(this->dataPtr->boxVis); + } + + // Clear any previous data from the grid and update + this->dataPtr->crossLines->Clear(); + this->dataPtr->crossLines->Update(); + + this->dataPtr->crossLines->SetOperationType(MarkerType::MT_LINE_LIST); + if (this->dataPtr->material == nullptr) + { + MaterialPtr defaultMat = + this->Scene()->Material("Default/TransGreen")->Clone(); + this->SetMaterial(defaultMat, false); + } + + // Inertia position indicator + ignition::math::Vector3d p1(0, 0, -2*_scale.Z()); + ignition::math::Vector3d p2(0, 0, 2*_scale.Z()); + ignition::math::Vector3d p3(0, -2*_scale.Y(), 0); + ignition::math::Vector3d p4(0, 2*_scale.Y(), 0); + ignition::math::Vector3d p5(-2*_scale.X(), 0, 0); + ignition::math::Vector3d p6(2*_scale.X(), 0, 0); + p1 = _pose.Rot().RotateVector(p1); + p2 = _pose.Rot().RotateVector(p2); + p3 = _pose.Rot().RotateVector(p3); + p4 = _pose.Rot().RotateVector(p4); + p5 = _pose.Rot().RotateVector(p5); + p6 = _pose.Rot().RotateVector(p6); + p1 += _pose.Pos(); + p2 += _pose.Pos(); + p3 += _pose.Pos(); + p4 += _pose.Pos(); + p5 += _pose.Pos(); + p6 += _pose.Pos(); + + this->dataPtr->crossLines->AddPoint(p1); + this->dataPtr->crossLines->AddPoint(p2); + this->dataPtr->crossLines->AddPoint(p3); + this->dataPtr->crossLines->AddPoint(p4); + this->dataPtr->crossLines->AddPoint(p5); + this->dataPtr->crossLines->AddPoint(p6); + + this->dataPtr->crossLines->Update(); + + this->dataPtr->boxVis->SetLocalScale(_scale); + this->dataPtr->boxVis->SetLocalPosition(_pose.Pos()); + this->dataPtr->boxVis->SetLocalRotation(_pose.Rot()); +} + +////////////////////////////////////////////////// +VisualPtr Ogre2InertiaVisual::BoxVisual() const +{ + return this->dataPtr->boxVis; +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::SetMaterial(MaterialPtr _material, bool _unique) +{ + _material = (_unique) ? _material->Clone() : _material; + + Ogre2MaterialPtr derived = + std::dynamic_pointer_cast(_material); + + if (!derived) + { + ignerr << "Cannot assign material created by another render-engine" + << std::endl; + + return; + } + + this->dataPtr->crossLines->SetMaterial(_material, false); + this->SetMaterialImpl(derived); +} + +////////////////////////////////////////////////// +void Ogre2InertiaVisual::SetMaterialImpl(Ogre2MaterialPtr _material) +{ + Ogre::MaterialPtr ogreMaterial = _material->Material(); + this->dataPtr->material = _material; +} + +////////////////////////////////////////////////// +MaterialPtr Ogre2InertiaVisual::Material() const +{ + return this->dataPtr->material; +} diff --git a/ogre2/src/Ogre2Node.cc b/ogre2/src/Ogre2Node.cc index 63a43a936..ec8a5d158 100644 --- a/ogre2/src/Ogre2Node.cc +++ b/ogre2/src/Ogre2Node.cc @@ -64,6 +64,9 @@ Ogre::SceneNode *Ogre2Node::Node() const ////////////////////////////////////////////////// void Ogre2Node::Destroy() { + if (!ogreNode) + return; + BaseNode::Destroy(); Ogre::SceneManager *ogreSceneManager = this->scene->OgreSceneManager(); ogreSceneManager->destroySceneNode(this->ogreNode); diff --git a/ogre2/src/Ogre2Scene.cc b/ogre2/src/Ogre2Scene.cc index 59f53be0e..0a9141bf8 100644 --- a/ogre2/src/Ogre2Scene.cc +++ b/ogre2/src/Ogre2Scene.cc @@ -27,6 +27,7 @@ #include "ignition/rendering/ogre2/Ogre2GizmoVisual.hh" #include "ignition/rendering/ogre2/Ogre2GpuRays.hh" #include "ignition/rendering/ogre2/Ogre2Grid.hh" +#include "ignition/rendering/ogre2/Ogre2InertiaVisual.hh" #include "ignition/rendering/ogre2/Ogre2Light.hh" #include "ignition/rendering/ogre2/Ogre2LightVisual.hh" #include "ignition/rendering/ogre2/Ogre2LidarVisual.hh" @@ -739,6 +740,15 @@ AxisVisualPtr Ogre2Scene::CreateAxisVisualImpl(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +InertiaVisualPtr Ogre2Scene::CreateInertiaVisualImpl(unsigned int _id, + const std::string &_name) +{ + Ogre2InertiaVisualPtr visual(new Ogre2InertiaVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr Ogre2Scene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/src/InertiaVisual_TEST.cc b/src/InertiaVisual_TEST.cc new file mode 100644 index 000000000..0d958cadf --- /dev/null +++ b/src/InertiaVisual_TEST.cc @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/InertiaVisual.hh" +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" + +using namespace ignition; +using namespace rendering; + +class InertiaVisualTest : public testing::Test, + public testing::WithParamInterface +{ + /// \brief Test basic API + public: void InertiaVisual(const std::string &_renderEngine); +}; + +///////////////////////////////////////////////// +void InertiaVisualTest::InertiaVisual(const std::string &_renderEngine) +{ + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ScenePtr scene = engine->CreateScene("scene"); + + // create visual + InertiaVisualPtr inertiaVisual = scene->CreateInertiaVisual(); + ASSERT_NE(nullptr, inertiaVisual); + + // check initial values + EXPECT_EQ(nullptr, inertiaVisual->BoxVisual()); + + ignition::math::MassMatrix3d massMatrix( + 2.0, {2.0, 1.5, 1.0}, {0.0, 0.0, 0.0}); + ignition::math::Pose3d p(0.0, 1.0, 2.5, 1.0, 0.4, 0.4); + ignition::math::Inertiald inertial; + + inertiaVisual->SetInertial(inertial); + EXPECT_EQ(nullptr, inertiaVisual->BoxVisual()); + + inertial.SetMassMatrix(massMatrix); + inertial.SetPose(p); + inertiaVisual->SetInertial(inertial); + EXPECT_NE(nullptr, inertiaVisual->BoxVisual()); + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +TEST_P(InertiaVisualTest, InertiaVisual) +{ + InertiaVisual(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(Visual, InertiaVisualTest, + RENDER_ENGINE_VALUES, + ignition::rendering::PrintToStringParam()); + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index 76094afb8..b2fd44596 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -27,6 +27,7 @@ #include "ignition/rendering/ArrowVisual.hh" #include "ignition/rendering/AxisVisual.hh" +#include "ignition/rendering/InertiaVisual.hh" #include "ignition/rendering/LidarVisual.hh" #include "ignition/rendering/LightVisual.hh" #include "ignition/rendering/Camera.hh" @@ -918,6 +919,36 @@ AxisVisualPtr BaseScene::CreateAxisVisual(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +InertiaVisualPtr BaseScene::CreateInertiaVisual() +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateInertiaVisual(objId); +} + +////////////////////////////////////////////////// +InertiaVisualPtr BaseScene::CreateInertiaVisual(unsigned int _id) +{ + std::string objName = this->CreateObjectName(_id, "InertiaVisual"); + return this->CreateInertiaVisual(_id, objName); +} + +////////////////////////////////////////////////// +InertiaVisualPtr BaseScene::CreateInertiaVisual(const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateInertiaVisual(objId, _name); +} + +////////////////////////////////////////////////// +InertiaVisualPtr BaseScene::CreateInertiaVisual(unsigned int _id, + const std::string &_name) +{ + InertiaVisualPtr visual = this->CreateInertiaVisualImpl(_id, _name); + bool result = this->RegisterVisual(visual); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr BaseScene::CreateLightVisual() { @@ -1350,6 +1381,16 @@ void BaseScene::CreateMaterials() material->SetReceiveShadows(false); material->SetLightingEnabled(false); + material = this->CreateMaterial("Default/TransPurple"); + material->SetAmbient(1.0, 0.0, 1.0); + material->SetDiffuse(1.0, 0.0, 1.0); + material->SetEmissive(1.0, 0.0, 1.0); + material->SetTransparency(0.5); + material->SetCastShadows(false); + material->SetReceiveShadows(false); + material->SetLightingEnabled(false); + material->SetDepthWriteEnabled(false); + material = this->CreateMaterial("Default/White"); material->SetAmbient(1.0, 1.0, 1.0); material->SetDiffuse(1.0, 1.0, 1.0);