diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2RayQuery.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2RayQuery.hh index 96a1e9f44..84c42fd0f 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2RayQuery.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2RayQuery.hh @@ -52,6 +52,14 @@ namespace ignition // Documentation inherited public: virtual RayQueryResult ClosestPoint(); + /// \brief Get closest point by selection buffer. + /// This is executed on the GPU. + private: RayQueryResult ClosestPointBySelectionBuffer(); + + /// \brief Get closest point by ray triangle intersection test. + /// This is executed on the CPU. + private: RayQueryResult ClosestPointByIntersection(); + /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2SelectionBuffer.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2SelectionBuffer.hh index 35fd51332..3656e3e08 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2SelectionBuffer.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2SelectionBuffer.hh @@ -62,6 +62,16 @@ namespace ignition /// \return Returns the Ogre item at the coordinate. public: Ogre::Item *OnSelectionClick(const int _x, const int _y); + /// \brief Perform selection operation and get ogre item and + /// point of intersection. + /// \param[in] _x X coordinate in pixels. + /// \param[in] _y Y coordinate in pixels. + /// \param[out] Ogre item at the coordinate. + /// \param[out] 3D point of intersection with the ogre item's mesh. + /// \return True if an ogre item is found, false otherwise + public: bool ExecuteQuery(const int _x, const int _y, Ogre::Item *&_item, + math::Vector3d &_point); + /// \brief Debug show overlay /// \param[in] _show True to show the selection buffer in an overlay. // public: void ShowOverlay(const bool _show); diff --git a/ogre2/src/Ogre2RayQuery.cc b/ogre2/src/Ogre2RayQuery.cc index 2f6e713fb..41995f127 100644 --- a/ogre2/src/Ogre2RayQuery.cc +++ b/ogre2/src/Ogre2RayQuery.cc @@ -36,8 +36,11 @@ class ignition::rendering::Ogre2RayQueryPrivate //// \brief Pointer to camera public: Ogre2CameraPtr camera{nullptr}; - /// Image pos to cast the ray from + /// \brief Image pos to cast the ray from public: math::Vector2i imgPos = math::Vector2i::Zero; + + /// \brief thread that ray query is created in + public: std::thread::id threadId; }; using namespace ignition; @@ -47,6 +50,7 @@ using namespace rendering; Ogre2RayQuery::Ogre2RayQuery() : dataPtr(new Ogre2RayQueryPrivate) { + this->dataPtr->threadId = std::this_thread::get_id(); } ////////////////////////////////////////////////// @@ -69,97 +73,171 @@ void Ogre2RayQuery::SetFromCamera(const CameraPtr &_camera, this->dataPtr->camera = camera; - this->dataPtr->imgPos.X() = - screenPos.X() * this->dataPtr->camera->ImageWidth(); - this->dataPtr->imgPos.Y() = - screenPos.Y() * this->dataPtr->camera->ImageHeight(); + this->dataPtr->imgPos.X() = static_cast( + screenPos.X() * this->dataPtr->camera->ImageWidth()); + this->dataPtr->imgPos.Y() = static_cast( + screenPos.Y() * this->dataPtr->camera->ImageHeight()); } ////////////////////////////////////////////////// RayQueryResult Ogre2RayQuery::ClosestPoint() { RayQueryResult result; - result.distance = -1; - // hack to create selection buffer if it does not exist yet - if (!this->dataPtr->camera->SelectionBuffer()) - this->dataPtr->camera->VisualAt(this->dataPtr->imgPos); +#ifdef __APPLE__ + return this->ClosestPointByIntersection(); +#else + if (!this->dataPtr->camera || + !this->dataPtr->camera->Parent() || + std::this_thread::get_id() != this->dataPtr->threadId) + { + // use legacy method for backward compatibility if no camera is set or + // camera is not attached in the scene tree or + // this function is called from non-rendering thread + return this->ClosestPointByIntersection(); + } + else + { + // the VisualAt function is a hack to force creation of the selection + // buffer object + // todo(anyone) Make Camera::SetSelectionBuffer function public? + if (!this->dataPtr->camera->SelectionBuffer()) + this->dataPtr->camera->VisualAt(math::Vector2i(0, 0)); - Ogre::Item *ogreItem = - this->dataPtr->camera->SelectionBuffer()->OnSelectionClick( - this->dataPtr->imgPos.X(), this->dataPtr->imgPos.Y()); + return this->ClosestPointBySelectionBuffer(); + } +#endif +} - if (!ogreItem) - return result; +////////////////////////////////////////////////// +RayQueryResult Ogre2RayQuery::ClosestPointBySelectionBuffer() +{ + RayQueryResult result; + Ogre::Item *ogreItem = nullptr; + math::Vector3d point; + bool success = this->dataPtr->camera->SelectionBuffer()->ExecuteQuery( + this->dataPtr->imgPos.X(), this->dataPtr->imgPos.Y(), ogreItem, point); + result.distance = -1; - Ogre::Any userAny = ogreItem->getUserObjectBindings().getUserAny(); + if (success && ogreItem) + { + if (!ogreItem->getUserObjectBindings().getUserAny().isEmpty() && + ogreItem->getUserObjectBindings().getUserAny().getType() == + typeid(unsigned int)) + { + auto userAny = ogreItem->getUserObjectBindings().getUserAny(); + double pointLength = point.Length(); + if (!std::isinf(pointLength)) + { + result.distance = pointLength; + result.point = point; + result.objectId = Ogre::any_cast(userAny); + } + } + } + return result; +} - if (userAny.isEmpty() || userAny.getType() != typeid(unsigned int)) +////////////////////////////////////////////////// +RayQueryResult Ogre2RayQuery::ClosestPointByIntersection() +{ + RayQueryResult result; + Ogre2ScenePtr ogreScene = + std::dynamic_pointer_cast(this->Scene()); + if (!ogreScene) return result; - double distance = -1.0; - Ogre::Ray mouseRay(Ogre2Conversions::Convert(this->origin), Ogre2Conversions::Convert(this->direction)); - // mesh factory creates name with ::CENTER or ::ORIGINAL depending on - // the params passed in the MeshDescriptor when loading the mesh - // so strip off the suffix - std::string meshName = ogreItem->getMesh()->getName(); - size_t idx = meshName.find("::"); - if (idx != std::string::npos) - meshName = meshName.substr(0, idx); - - const common::Mesh *mesh = - common::MeshManager::Instance()->MeshByName(meshName); + if (!this->dataPtr->rayQuery) + { + this->dataPtr->rayQuery = + ogreScene->OgreSceneManager()->createRayQuery(mouseRay); + } + this->dataPtr->rayQuery->setSortByDistance(true); + this->dataPtr->rayQuery->setRay(mouseRay); - if (!mesh) - return result; + // Perform the scene query + Ogre::RaySceneQueryResult &ogreResult = this->dataPtr->rayQuery->execute(); - Ogre::Matrix4 transform = ogreItem->_getParentNodeFullTransform(); + double distance = -1.0; - // test for hitting individual triangles on the mesh - for (unsigned int j = 0; j < mesh->SubMeshCount(); ++j) + // Iterate over all the results. + for (auto iter = ogreResult.begin(); iter != ogreResult.end(); ++iter) { - auto s = mesh->SubMeshByIndex(j); - auto submesh = s.lock(); - if (!submesh || submesh->VertexCount() < 3u) + if (iter->distance <= 0.0) + continue; + + if (!iter->movable || !iter->movable->getVisible()) continue; - unsigned int indexCount = submesh->IndexCount(); - for (unsigned int k = 0; k < indexCount; k += 3) + + auto userAny = iter->movable->getUserObjectBindings().getUserAny(); + if (!userAny.isEmpty() && userAny.getType() == typeid(unsigned int) && + iter->movable->getMovableType() == "Item") { - if (indexCount <= k+2) + Ogre::Item *ogreItem = static_cast(iter->movable); + + // mesh factory creates name with ::CENTER or ::ORIGINAL depending on + // the params passed in the MeshDescriptor when loading the mesh + // so strip off the suffix + std::string meshName = ogreItem->getMesh()->getName(); + size_t idx = meshName.find("::"); + if (idx != std::string::npos) + meshName = meshName.substr(0, idx); + + const common::Mesh *mesh = + common::MeshManager::Instance()->MeshByName(meshName); + + if (!mesh) continue; - ignition::math::Vector3d vertexA = - submesh->Vertex(submesh->Index(k)); - ignition::math::Vector3d vertexB = - submesh->Vertex(submesh->Index(k+1)); - ignition::math::Vector3d vertexC = - submesh->Vertex(submesh->Index(k+2)); - - Ogre::Vector3 worldVertexA = - transform * Ogre2Conversions::Convert(vertexA); - Ogre::Vector3 worldVertexB = - transform * Ogre2Conversions::Convert(vertexB); - Ogre::Vector3 worldVertexC = - transform * Ogre2Conversions::Convert(vertexC); - - // check for a hit against this triangle - std::pair hit = Ogre::Math::intersects(mouseRay, - worldVertexA, worldVertexB, worldVertexC, - true, false); - - // if it was a hit check if its the closest - if (hit.first && - (distance < 0.0f || hit.second < distance)) + Ogre::Matrix4 transform = ogreItem->_getParentNodeFullTransform(); + + // test for hitting individual triangles on the mesh + for (unsigned int j = 0; j < mesh->SubMeshCount(); ++j) { - // this is the closest so far, save it off - distance = hit.second; - result.distance = distance; - result.point = - Ogre2Conversions::Convert(mouseRay.getPoint(distance)); - result.objectId = Ogre::any_cast(userAny); + auto s = mesh->SubMeshByIndex(j); + auto submesh = s.lock(); + if (!submesh || submesh->VertexCount() < 3u) + continue; + unsigned int indexCount = submesh->IndexCount(); + for (unsigned int k = 0; k < indexCount; k += 3) + { + if (indexCount <= k+2) + continue; + + ignition::math::Vector3d vertexA = + submesh->Vertex(submesh->Index(k)); + ignition::math::Vector3d vertexB = + submesh->Vertex(submesh->Index(k+1)); + ignition::math::Vector3d vertexC = + submesh->Vertex(submesh->Index(k+2)); + + Ogre::Vector3 worldVertexA = + transform * Ogre2Conversions::Convert(vertexA); + Ogre::Vector3 worldVertexB = + transform * Ogre2Conversions::Convert(vertexB); + Ogre::Vector3 worldVertexC = + transform * Ogre2Conversions::Convert(vertexC); + + // check for a hit against this triangle + std::pair hit = Ogre::Math::intersects(mouseRay, + worldVertexA, worldVertexB, worldVertexC, + true, false); + + // if it was a hit check if its the closest + if (hit.first && + (distance < 0.0f || hit.second < distance)) + { + // this is the closest so far, save it off + distance = hit.second; + result.distance = distance; + result.point = + Ogre2Conversions::Convert(mouseRay.getPoint(distance)); + result.objectId = Ogre::any_cast(userAny); + } + } } } } diff --git a/ogre2/src/Ogre2SelectionBuffer.cc b/ogre2/src/Ogre2SelectionBuffer.cc index 9ec622011..26ac9a5f0 100644 --- a/ogre2/src/Ogre2SelectionBuffer.cc +++ b/ogre2/src/Ogre2SelectionBuffer.cc @@ -20,6 +20,7 @@ #include "ignition/common/Console.hh" #include "ignition/rendering/RenderTypes.hh" +#include "ignition/rendering/ogre2/Ogre2Conversions.hh" #include "ignition/rendering/ogre2/Ogre2Includes.hh" #include "ignition/rendering/ogre2/Ogre2MaterialSwitcher.hh" #include "ignition/rendering/ogre2/Ogre2RenderEngine.hh" @@ -63,10 +64,13 @@ class ignition::rendering::Ogre2SelectionBufferPrivate public: Ogre::CompositorWorkspace *ogreCompositorWorkspace = nullptr; /// \brief Render texture data buffer - public: uint8_t *buffer = nullptr; + public: float *buffer = nullptr; /// \brief Ogre pixel box that contains description of the data buffer public: Ogre::PixelBox *pixelBox = nullptr; + + /// \brief The selection buffer material + public: Ogre::MaterialPtr selectionMaterial; }; ///////////////////////////////////////////////// @@ -85,8 +89,9 @@ Ogre2SelectionBuffer::Ogre2SelectionBuffer(const std::string &_cameraName, return; } + std::string selectionCameraName = _cameraName + "_selection_buffer"; this->dataPtr->selectionCamera = - this->dataPtr->sceneMgr->createCamera(_cameraName + "_selection_buffer"); + this->dataPtr->sceneMgr->createCamera(selectionCameraName); this->dataPtr->materialSwitcher.reset( new Ogre2MaterialSwitcher(this->dataPtr->scene)); @@ -123,6 +128,12 @@ void Ogre2SelectionBuffer::Update() ///////////////////////////////////////////////// void Ogre2SelectionBuffer::DeleteRTTBuffer() { + if (this->dataPtr->selectionMaterial) + { + Ogre::MaterialManager::getSingleton().remove( + this->dataPtr->selectionMaterial->getName()); + } + auto &manager = Ogre::TextureManager::getSingleton(); manager.unload(this->dataPtr->texture->getName()); manager.remove(this->dataPtr->texture->getName()); @@ -142,17 +153,55 @@ void Ogre2SelectionBuffer::CreateRTTBuffer() // create a 1x1 pixel buffer unsigned int width = 1; unsigned int height = 1; - Ogre::PixelFormat format = Ogre::PF_R8G8B8; + Ogre::PixelFormat format = Ogre::PF_FLOAT32_RGBA; this->dataPtr->texture = Ogre::TextureManager::getSingleton().createManual( "SelectionPassTex", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_R8G8B8, + Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_FLOAT32_RGBA, Ogre::TU_RENDERTARGET); this->dataPtr->renderTexture = this->dataPtr->texture->getBuffer()->getRenderTarget(); - this->dataPtr->renderTexture->addListener( - this->dataPtr->materialSwitcher.get()); + + // Load selection material + // The SelectionBuffer material is defined in script + // (selection_buffer.material). + std::string matSelectionName = "SelectionBuffer"; + Ogre::MaterialPtr matSelection = + Ogre::MaterialManager::getSingleton().getByName(matSelectionName); + this->dataPtr->selectionMaterial = matSelection->clone( + this->dataPtr->camera->getName() + "_" + matSelectionName); + this->dataPtr->selectionMaterial->load(); + Ogre::Pass *p = this->dataPtr->selectionMaterial->getTechnique(0)->getPass(0); + Ogre::GpuProgramParametersSharedPtr psParams = + p->getFragmentProgramParameters(); + + // Set the uniform variables (selection_buffer_fs.glsl). + // The projectParams is used to linearize depth buffer data + double nearPlane = this->dataPtr->camera->getNearClipDistance(); + double farPlane = this->dataPtr->camera->getFarClipDistance(); + + this->dataPtr->selectionCamera->setNearClipDistance(nearPlane); + this->dataPtr->selectionCamera->setFarClipDistance(farPlane); + + // \todo(anyone) change the code below when merging forward to fortress that + // uses ogre 2.2 otherwise the depth values will be incorrect due to + // reverse-z depth buffer + // Ogre::Vector2 projectionAB = + // this->dataPtr->camera->getProjectionParamsAB(); + // double projectionA = projectionAB.x + // double projectionB = projectionAB.y + double projectionA = farPlane / + (farPlane - nearPlane); + double projectionB = (-farPlane * nearPlane) / + (farPlane - nearPlane); + projectionB /= farPlane; + psParams->setNamedConstant("projectionParams", + Ogre::Vector2(projectionA, projectionB)); + psParams->setNamedConstant("far", + static_cast(farPlane)); + psParams->setNamedConstant("inf", + static_cast(math::INF_F)); // create compositor workspace for rendering auto engine = Ogre2RenderEngine::Instance(); @@ -161,55 +210,157 @@ void Ogre2SelectionBuffer::CreateRTTBuffer() const Ogre::String workspaceName = "SelectionBufferWorkspace" + this->dataPtr->camera->getName(); - ogreCompMgr->createBasicWorkspaceDef(workspaceName, - Ogre::ColourValue(0.0f, 0.0f, 0.0f, 1.0f)); + + Ogre::CompositorNodeDef *nodeDef = + ogreCompMgr->addNodeDefinition("AutoGen " + Ogre::IdString(workspaceName + + "/Node").getReleaseText()); + + Ogre::TextureDefinitionBase::TextureDefinition *colorTexDef = + nodeDef->addTextureDefinition("colorTexture"); + colorTexDef->textureType = Ogre::TEX_TYPE_2D; + colorTexDef->width = 0; + colorTexDef->height = 0; + colorTexDef->depth = 1; + colorTexDef->numMipmaps = 0; + colorTexDef->widthFactor = 1; + colorTexDef->heightFactor = 1; + colorTexDef->formatList = {Ogre::PF_FLOAT32_RGBA}; + colorTexDef->fsaa = 0; + colorTexDef->uav = false; + colorTexDef->automipmaps = false; + colorTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + colorTexDef->depthBufferFormat = Ogre::PF_D32_FLOAT; + colorTexDef->preferDepthTexture = true; + colorTexDef->fsaaExplicitResolve = false; + + Ogre::TextureDefinitionBase::TextureDefinition *depthTexDef = + nodeDef->addTextureDefinition("depthTexture"); + depthTexDef->textureType = Ogre::TEX_TYPE_2D; + depthTexDef->width = 0; + depthTexDef->height = 0; + depthTexDef->depth = 1; + depthTexDef->numMipmaps = 0; + depthTexDef->widthFactor = 1; + depthTexDef->heightFactor = 1; + depthTexDef->formatList = {Ogre::PF_D32_FLOAT}; + depthTexDef->fsaa = 0; + depthTexDef->uav = false; + depthTexDef->automipmaps = false; + depthTexDef->hwGammaWrite = Ogre::TextureDefinitionBase::BoolFalse; + depthTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + depthTexDef->depthBufferFormat = Ogre::PF_UNKNOWN; + depthTexDef->fsaaExplicitResolve = false; + + // Input texture + nodeDef->addTextureSourceName("rt", 0, + Ogre::TextureDefinitionBase::TEXTURE_INPUT); + + nodeDef->setNumTargetPass(2); + Ogre::CompositorTargetDef *colorTargetDef = + nodeDef->addTargetPass("colorTexture"); + colorTargetDef->setNumPasses(2); + { + // clear pass + Ogre::CompositorPassClearDef *passClear = + static_cast( + colorTargetDef->addPass(Ogre::PASS_CLEAR)); + passClear->mColourValue = Ogre::ColourValue(0.0f, 0.0f, 0.0f, 1.0f); + // scene pass + Ogre::CompositorPassSceneDef *passScene = + static_cast( + colorTargetDef->addPass(Ogre::PASS_SCENE)); + passScene->mVisibilityMask = IGN_VISIBILITY_SELECTABLE; + } + + Ogre::CompositorTargetDef *targetDef = nodeDef->addTargetPass("rt"); + targetDef->setNumPasses(2); + { + { + Ogre::CompositorPassClearDef *passClear = + static_cast( + targetDef->addPass(Ogre::PASS_CLEAR)); + passClear->mColourValue = Ogre::ColourValue(0.0f, 0.0f, 0.0f, 1.0f); + } + // quad pass + Ogre::CompositorPassQuadDef *passQuad = + static_cast( + targetDef->addPass(Ogre::PASS_QUAD)); + passQuad->mMaterialName = this->dataPtr->selectionMaterial->getName(); + passQuad->addQuadTextureSource(0, "colorTexture", 0); + passQuad->addQuadTextureSource(1, "depthTexture", 0); + passQuad->mFrustumCorners = + Ogre::CompositorPassQuadDef::VIEW_SPACE_CORNERS; + } + + Ogre::CompositorWorkspaceDef *workDef = + ogreCompMgr->addWorkspaceDefinition(workspaceName); + workDef->connectExternal(0, nodeDef->getName(), 0); + this->dataPtr->ogreCompositorWorkspace = ogreCompMgr->addWorkspace(this->dataPtr->scene->OgreSceneManager(), this->dataPtr->renderTexture, this->dataPtr->selectionCamera, workspaceName, false); - // set visibility mask to see only items that are selectable - auto nodeSeq = this->dataPtr->ogreCompositorWorkspace->getNodeSequence(); - auto pass = nodeSeq[0]->_getPasses()[1]->getDefinition(); - auto scenePass = dynamic_cast(pass); - const_cast(scenePass)->mVisibilityMask = - IGN_VISIBILITY_SELECTABLE; + // add the listener + Ogre::CompositorNode *node = + this->dataPtr->ogreCompositorWorkspace->getNodeSequence()[0]; + auto channelsTex = node->getLocalTextures(); + + for (auto &c : channelsTex) + { + if (c.textures[0]->getSrcFormat() == Ogre::PF_FLOAT32_RGBA) + { + c.target->addListener( + this->dataPtr->materialSwitcher.get()); + break; + } + } - // buffer to store render texture data. Ensure it's at least 4 bytes + // buffer to store render texture data. Ensure it's at least 4 channels size_t bufferSize = std::max( Ogre::PixelUtil::getMemorySize(width, height, 1, format), 4u); - this->dataPtr->buffer = new uint8_t[bufferSize]; - memset(this->dataPtr->buffer, 0, 4u); + this->dataPtr->buffer = new float[width * height * 4u]; + memset(this->dataPtr->buffer, 0, bufferSize); this->dataPtr->pixelBox = new Ogre::PixelBox(width, height, 1, format, this->dataPtr->buffer); } ///////////////////////////////////////////////// Ogre::Item *Ogre2SelectionBuffer::OnSelectionClick(const int _x, const int _y) +{ + Ogre::Item *item = nullptr; + math::Vector3d point; + this->ExecuteQuery(_x, _y, item, point); + return item; +} + +///////////////////////////////////////////////// +bool Ogre2SelectionBuffer::ExecuteQuery(const int _x, const int _y, + Ogre::Item *&_item, math::Vector3d &_point) { if (!this->dataPtr->renderTexture) - return nullptr; + return false; if (!this->dataPtr->camera) - return nullptr; + return false; Ogre::Viewport *vp = this->dataPtr->camera->getLastViewport(); if (!vp) - return nullptr; + return false; Ogre::RenderTarget *rt = vp->getTarget(); if (!rt) - return nullptr; + return false; const unsigned int targetWidth = rt->getWidth(); const unsigned int targetHeight = rt->getHeight(); if (_x < 0 || _y < 0 || _x >= static_cast(targetWidth) || _y >= static_cast(targetHeight)) - return nullptr; + return false; // 1x1 selection buffer, adapted from rviz // http://docs.ros.org/indigo/api/rviz/html/c++/selection__manager_8cpp.html @@ -229,8 +380,28 @@ Ogre::Item *Ogre2SelectionBuffer::OnSelectionClick(const int _x, const int _y) scaleMatrix[1][1] = 1.0 / (y2-y1); transMatrix[0][3] -= x1+x2; transMatrix[1][3] += y1+y2; - this->dataPtr->selectionCamera->setCustomProjectionMatrix(true, - scaleMatrix * transMatrix * this->dataPtr->camera->getProjectionMatrix()); + Ogre::Matrix4 customProjectionMatrix = + scaleMatrix * transMatrix * this->dataPtr->camera->getProjectionMatrix(); + + // There is a bug in ogre 2.1 that produces incorrect frustum + // extents when a custom projection matrix is set. + // So we manually compute the frusm extents ourselves + // \todo(anyone) The bug should be fixed in ogre 2.2 so we should be + // able to uncomment the setCustomProjectionMatrix call below + // and remove the extents set by setFrustumExtents + // this->dataPtr->selectionCamera->setCustomProjectionMatrix(true, + // customProjectionMatrix); + Ogre::Matrix4 invProj = customProjectionMatrix.inverse(); + Ogre::Vector4 topLeft(-1.0f, 1.0f, -1.0f, 1.0f); + Ogre::Vector4 bottomRight(1.0f, -1.0f, -1.0f, 1.0f); + topLeft = invProj * topLeft; + bottomRight = invProj * bottomRight; + float left = topLeft.x / topLeft.w; + float top = topLeft.y / topLeft.w; + float right = bottomRight.x / bottomRight.w; + float bottom = bottomRight.y / bottomRight.w; + this->dataPtr->selectionCamera->setFrustumExtents(left, right, top, bottom); + this->dataPtr->selectionCamera->setPosition( this->dataPtr->camera->getDerivedPosition()); this->dataPtr->selectionCamera->setOrientation( @@ -242,31 +413,51 @@ Ogre::Item *Ogre2SelectionBuffer::OnSelectionClick(const int _x, const int _y) // update render texture this->Update(); - size_t posInStream = 0; - ignition::math::Color::BGRA color(0); if (!this->dataPtr->buffer) { ignerr << "Selection buffer is null." << std::endl; - return nullptr; + return false; } - memcpy(static_cast(&color), this->dataPtr->buffer + posInStream, 4); + + float color = this->dataPtr->buffer[3]; + uint32_t *rgba = reinterpret_cast(&color); + unsigned int r = *rgba >> 24 & 0xFF; + unsigned int g = *rgba >> 16 & 0xFF; + unsigned int b = *rgba >> 8 & 0xFF; + + math::Vector3d point(this->dataPtr->buffer[0], this->dataPtr->buffer[1], + this->dataPtr->buffer[2]); + auto rot = Ogre2Conversions::Convert( + this->dataPtr->camera->getParentSceneNode()->_getDerivedOrientation()); + auto pos = Ogre2Conversions::Convert( + this->dataPtr->camera->getParentSceneNode()->_getDerivedPosition()); + math::Pose3d p(pos, rot); + point = rot * point + pos; + ignition::math::Color cv; - cv.SetFromARGB(color); cv.A(1.0); + cv.R(r / 255.0); + cv.G(g / 255.0); + cv.B(b / 255.0); + const std::string &entName = this->dataPtr->materialSwitcher->EntityName(cv); if (entName.empty()) { - return 0; + return false; } else { auto collection = this->dataPtr->sceneMgr->findMovableObjects( Ogre::ItemFactory::FACTORY_TYPE_NAME, entName); if (collection.empty()) - return nullptr; + return false; else - return dynamic_cast(collection[0]); + { + _item = dynamic_cast(collection[0]); + _point = point; + return true; + } } } diff --git a/ogre2/src/media/materials/programs/selection_buffer_fs.glsl b/ogre2/src/media/materials/programs/selection_buffer_fs.glsl new file mode 100644 index 000000000..d9acd041e --- /dev/null +++ b/ogre2/src/media/materials/programs/selection_buffer_fs.glsl @@ -0,0 +1,66 @@ +/* + * 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. + * + */ + +#version 330 + +in block +{ + vec2 uv0; + vec3 cameraDir; +} inPs; + +uniform sampler2D colorTexture; +uniform sampler2D depthTexture; + +out vec4 fragColor; + +uniform vec2 projectionParams; +uniform float far; +uniform float inf; + +float packFloat(vec4 color) +{ + int rgba = (int(color.x * 255.0) << 24) + + (int(color.y * 255.0) << 16) + + (int(color.z * 255.0) << 8) + + int(color.w * 255.0); + return intBitsToFloat(rgba); +} + +void main() +{ + // get linear depth + float fDepth = texture(depthTexture, inPs.uv0).x; + float d = projectionParams.y / (fDepth - projectionParams.x); + + // reconstruct 3d viewspace pos from depth + vec3 viewSpacePos = inPs.cameraDir * d; + + // convert to z up + vec3 point = vec3(-viewSpacePos.z, -viewSpacePos.x, viewSpacePos.y); + + // set to inf if point is at far clip plane + if (point.x > far - 1e-4) + point = vec3(inf); + + // color + vec4 color = texture(colorTexture, inPs.uv0); + + float rgba = packFloat(color); + + fragColor = vec4(point.xyz, rgba); +} diff --git a/ogre2/src/media/materials/scripts/selection_buffer.material b/ogre2/src/media/materials/scripts/selection_buffer.material new file mode 100644 index 000000000..3766fc83d --- /dev/null +++ b/ogre2/src/media/materials/scripts/selection_buffer.material @@ -0,0 +1,40 @@ +fragment_program selection_buffer_fs glsl +{ + source selection_buffer_fs.glsl + default_params + { + param_named colorTexture int 0 + param_named depthTexture int 1 + } +} + +material SelectionBuffer +{ + // Material has one technique + technique + { + // This technique has one pass + pass + { +// fog_override true + + // Make this pass use the vertex shader defined above + vertex_program_ref DepthCameraVS + { + } + + // Make this pass use the pixel shader defined above + fragment_program_ref selection_buffer_fs { } + texture_unit colorTexture + { + filtering none + tex_address_mode clamp + } + texture_unit depthTexture + { + filtering none + tex_address_mode clamp + } + } + } +}