diff --git a/ogre2/src/Ogre2GpuRays.cc b/ogre2/src/Ogre2GpuRays.cc index 2d6e02a98..0d19589fa 100644 --- a/ogre2/src/Ogre2GpuRays.cc +++ b/ogre2/src/Ogre2GpuRays.cc @@ -27,6 +27,7 @@ #include "ignition/rendering/RenderTypes.hh" #include "ignition/rendering/ogre2/Ogre2Conversions.hh" #include "ignition/rendering/ogre2/Ogre2Includes.hh" +#include "ignition/rendering/ogre2/Ogre2ParticleEmitter.hh" #include "ignition/rendering/ogre2/Ogre2RenderTarget.hh" #include "ignition/rendering/ogre2/Ogre2RenderTypes.hh" #include "ignition/rendering/ogre2/Ogre2Scene.hh" @@ -162,6 +163,13 @@ class ignition::rendering::Ogre2GpuRaysPrivate /// \brief Pointer to material switcher public: std::unique_ptr laserRetroMaterialSwitcher[6]; + + /// \brief standard deviation of particle noise + public: double particleStddev = 0.01; + + /// \brief Particle scatter ratio. This is used to determine the ratio of + /// particles that will detected by the depth camera + public: double particleScatterRatio = 0.1; }; using namespace ignition; @@ -607,6 +615,10 @@ void Ogre2GpuRays::Setup1stPass() static_cast(this->dataMaxVal)); psParams->setNamedConstant("min", static_cast(this->dataMinVal)); + psParams->setNamedConstant("particleStddev", + static_cast(this->dataPtr->particleStddev)); + psParams->setNamedConstant("particleScatterRatio", + static_cast(this->dataPtr->particleScatterRatio)); // Create 1st pass compositor auto engine = Ogre2RenderEngine::Instance(); @@ -622,6 +634,8 @@ void Ogre2GpuRays::Setup1stPass() // in 0 rt_input // texture depthTexture target_width target_height PF_D32_FLOAT // texture colorTexture target_width target_height PF_R8G8B8 + // texture particleTexture target_width target_height PF_L8 + // texture particleDepthTexture target_width target_height PF_D32_FLOAT // target colorTexture // { // pass clear @@ -630,6 +644,18 @@ void Ogre2GpuRays::Setup1stPass() // } // pass render_scene // { + // visibility_mask 0x11011111 + // } + // } + // target particleTexture + // { + // pass clear + // { + // colour_value 0.0 0.0 0.0 1.0 + // } + // pass render_scene + // { + // visibility_mask 0.00100000 // } // } // target rt_input @@ -697,7 +723,42 @@ void Ogre2GpuRays::Setup1stPass() colorTexDef->preferDepthTexture = true; colorTexDef->fsaaExplicitResolve = false; - nodeDef->setNumTargetPass(2); + Ogre::TextureDefinitionBase::TextureDefinition *particleDepthTexDef = + nodeDef->addTextureDefinition("particleDepthTexture"); + particleDepthTexDef->textureType = Ogre::TEX_TYPE_2D; + particleDepthTexDef->width = 0; + particleDepthTexDef->height = 0; + particleDepthTexDef->depth = 1; + particleDepthTexDef->numMipmaps = 0; + particleDepthTexDef->widthFactor = 0.5; + particleDepthTexDef->heightFactor = 0.5; + particleDepthTexDef->formatList = {Ogre::PF_D32_FLOAT}; + particleDepthTexDef->fsaa = 0; + particleDepthTexDef->uav = false; + particleDepthTexDef->automipmaps = false; + particleDepthTexDef->hwGammaWrite = Ogre::TextureDefinitionBase::BoolFalse; + particleDepthTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + + Ogre::TextureDefinitionBase::TextureDefinition *particleTexDef = + nodeDef->addTextureDefinition("particleTexture"); + particleTexDef->textureType = Ogre::TEX_TYPE_2D; + particleTexDef->width = 0; + particleTexDef->height = 0; + particleTexDef->depth = 1; + particleTexDef->numMipmaps = 0; + particleTexDef->widthFactor = 0.5; + particleTexDef->heightFactor = 0.5; + particleTexDef->formatList = {Ogre::PF_R8G8B8}; + particleTexDef->fsaa = 0; + particleTexDef->uav = false; + particleTexDef->automipmaps = false; + particleTexDef->hwGammaWrite = Ogre::TextureDefinitionBase::BoolFalse; + particleTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + particleTexDef->depthBufferFormat = Ogre::PF_D32_FLOAT; + particleTexDef->preferDepthTexture = true; + particleTexDef->fsaaExplicitResolve = false; + + nodeDef->setNumTargetPass(3); Ogre::CompositorTargetDef *colorTargetDef = nodeDef->addTargetPass("colorTexture"); @@ -713,7 +774,26 @@ void Ogre2GpuRays::Setup1stPass() static_cast( colorTargetDef->addPass(Ogre::PASS_SCENE)); // set camera custom visibility mask when rendering laser retro - passScene->mVisibilityMask = 0x01000000; + passScene->mVisibilityMask = 0x01000000 & + ~Ogre2ParticleEmitter::kParticleVisibilityFlags; + } + + Ogre::CompositorTargetDef *particleTargetDef = + nodeDef->addTargetPass("particleTexture"); + particleTargetDef->setNumPasses(2); + { + // clear pass + Ogre::CompositorPassClearDef *passClear = + static_cast( + particleTargetDef->addPass(Ogre::PASS_CLEAR)); + passClear->mColourValue = Ogre::ColourValue::Black; + // scene pass + Ogre::CompositorPassSceneDef *passScene = + static_cast( + particleTargetDef->addPass(Ogre::PASS_SCENE)); + // set camera custom visibility mask when rendering laser retro + passScene->mVisibilityMask = + Ogre2ParticleEmitter::kParticleVisibilityFlags; } // rt_input target - converts depth to range @@ -733,6 +813,8 @@ void Ogre2GpuRays::Setup1stPass() passQuad->mMaterialName = this->dataPtr->matFirstPass->getName(); passQuad->addQuadTextureSource(0, "depthTexture", 0); passQuad->addQuadTextureSource(1, "colorTexture", 0); + passQuad->addQuadTextureSource(2, "particleDepthTexture", 0); + passQuad->addQuadTextureSource(3, "particleTexture", 0); passQuad->mFrustumCorners = Ogre::CompositorPassQuadDef::VIEW_SPACE_CORNERS; } diff --git a/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl b/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl index d01d153ac..410e8511f 100644 --- a/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl +++ b/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl @@ -25,6 +25,8 @@ in block uniform sampler2D depthTexture; uniform sampler2D colorTexture; +uniform sampler2D particleDepthTexture; +uniform sampler2D particleTexture; out vec4 fragColor; @@ -34,19 +36,45 @@ uniform float far; uniform float min; uniform float max; -float getDepth(vec2 uv) +uniform float particleStddev; +uniform float particleScatterRatio; +uniform float time; + +// see gaussian_noise_fs.glsl for documentation on the rand and gaussrand +// functions + +#define PI 3.14159265358979323846264 + +float rand(vec2 co) { - float fDepth = texture(depthTexture, uv).x; - float linearDepth = projectionParams.y / (fDepth - projectionParams.x); - return linearDepth; + float r = fract(sin(dot(co.xy, vec2(12.9898,78.233))) * 43758.5453); + // Make sure that we don't return 0.0 + if(r == 0.0) + return 0.000000000001; + else + return r; } +vec4 gaussrand(vec2 co, vec3 offsets, float stddev, float mean) +{ + float U, V, R, Z; + U = rand(co + vec2(offsets.x, offsets.x)); + V = rand(co + vec2(offsets.y, offsets.y)); + R = rand(co + vec2(offsets.z, offsets.z)); + if(R < 0.5) + Z = sqrt(-2.0 * log(U)) * sin(2.0 * PI * V); + else + Z = sqrt(-2.0 * log(U)) * cos(2.0 * PI * V); + Z = Z * stddev + mean; + return vec4(Z, Z, Z, 0.0); +} void main() { // get linear depth - float d = getDepth(inPs.uv0); - + float fDepth = texture(depthTexture, inPs.uv0).x; + float d = projectionParams.y / (fDepth - projectionParams.x); + // get retro float retro = texture(colorTexture, inPs.uv0).x * 2000.0; @@ -56,6 +84,27 @@ void main() // get length of 3d point, i.e.range float l = length(viewSpacePos); + // particle mask - color and depth + vec4 particle = texture(particleTexture, inPs.uv0); + float particleDepth = texture(particleDepthTexture, inPs.uv0).x; + + if (particle.x > 0.0 && particleDepth > 0.0 && particleDepth < fDepth) + { + // apply scatter effect so that only some of the smoke pixels are visible + float r = rand(inPs.uv0 + vec2(time, time)); + if (r < particleScatterRatio) + { + float pd = projectionParams.y / (particleDepth - projectionParams.x); + vec3 point = inPs.cameraDir * pd; + + // apply gaussian noise to particle depth data + point = point + gaussrand(inPs.uv0, vec3(time, time, time), + particleStddev, 0.0).xyz; + + l = length(point); + } + } + if (l > far) l = max; else if (l < near) diff --git a/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl b/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl index dab691092..ec4bb0b07 100644 --- a/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl +++ b/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl @@ -87,9 +87,9 @@ void main() else if (faceIdx == 5) d = getRange(uv, tex5); - // todo(anyone) set retro values - float retro = 0.0; + float range = d.x; + float retro = d.y; - fragColor = vec4(d.x, d.y, 0, 1.0); + fragColor = vec4(range, retro, 0, 1.0); return; } diff --git a/ogre2/src/media/materials/scripts/gpu_rays.material b/ogre2/src/media/materials/scripts/gpu_rays.material index 5730b8ddd..fb027f569 100644 --- a/ogre2/src/media/materials/scripts/gpu_rays.material +++ b/ogre2/src/media/materials/scripts/gpu_rays.material @@ -30,8 +30,11 @@ fragment_program GpuRaysScan1stFS glsl default_params { + param_named_auto time time param_named depthTexture int 0 param_named colorTexture int 1 + param_named particleDepthTexture int 2 + param_named particleTexture int 3 } } @@ -53,6 +56,16 @@ material GpuRaysScan1st filtering none tex_address_mode clamp } + texture_unit particleDepthTexture + { + filtering none + tex_address_mode clamp + } + texture_unit particleTexture + { + filtering none + tex_address_mode clamp + } } } } diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index b58c57817..81bf2cedc 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -24,6 +24,7 @@ #include "test_config.h" // NOLINT(build/include) #include "ignition/rendering/GpuRays.hh" +#include "ignition/rendering/ParticleEmitter.hh" #include "ignition/rendering/RenderEngine.hh" #include "ignition/rendering/RenderingIface.hh" #include "ignition/rendering/Scene.hh" @@ -60,6 +61,9 @@ class GpuRaysTest: public testing::Test, // Test vertical measurements public: void LaserVertical(const std::string &_renderEngine); + + // Test detection of particles + public: void RaysParticles(const std::string &_renderEngine); }; ///////////////////////////////////////////////// @@ -459,6 +463,173 @@ void GpuRaysTest::LaserVertical(const std::string &_renderEngine) rendering::unloadEngine(engine->Name()); } +///////////////////////////////////////////////// +/// \brief Test detection of particles +void GpuRaysTest::RaysParticles(const std::string &_renderEngine) +{ +#ifdef __APPLE__ + std::cerr << "Skipping test for apple, see issue #35." << std::endl; + return; +#endif + + if (_renderEngine != "ogre2") + { + igndbg << "GpuRays with particle effect is not supported yet in rendering " + << "engine: " << _renderEngine << std::endl; + return; + } + + // Test GPU ray with 3 boxes in the world. + // Add noise in btewen GPU ray and box in the center + + const double hMinAngle = -IGN_PI / 2.0; + const double hMaxAngle = IGN_PI / 2.0; + const double minRange = 0.1; + const double maxRange = 10.0; + const int hRayCount = 320; + const int vRayCount = 1; + + // create and populate scene + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ScenePtr scene = engine->CreateScene("scene"); + ASSERT_TRUE(scene != nullptr); + + VisualPtr root = scene->RootVisual(); + + // Create ray caster + ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.1), + ignition::math::Quaterniond::Identity); + + GpuRaysPtr gpuRays = scene->CreateGpuRays("gpu_rays_1"); + gpuRays->SetWorldPosition(testPose.Pos()); + gpuRays->SetWorldRotation(testPose.Rot()); + gpuRays->SetNearClipPlane(minRange); + gpuRays->SetFarClipPlane(maxRange); + gpuRays->SetAngleMin(hMinAngle); + gpuRays->SetAngleMax(hMaxAngle); + gpuRays->SetRayCount(hRayCount); + + gpuRays->SetVerticalRayCount(vRayCount); + root->AddChild(gpuRays); + + // Create testing boxes + // box in the center + ignition::math::Pose3d box01Pose(ignition::math::Vector3d(3, 0, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox1 = scene->CreateVisual("UnitBox1"); + visualBox1->AddGeometry(scene->CreateBox()); + visualBox1->SetWorldPosition(box01Pose.Pos()); + visualBox1->SetWorldRotation(box01Pose.Rot()); + root->AddChild(visualBox1); + + // box on the right of the first gpu rays caster + ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox2 = scene->CreateVisual("UnitBox2"); + visualBox2->AddGeometry(scene->CreateBox()); + visualBox2->SetWorldPosition(box02Pose.Pos()); + visualBox2->SetWorldRotation(box02Pose.Rot()); + root->AddChild(visualBox2); + + // box on the left of the rays caster 1 but out of range + ignition::math::Pose3d box03Pose( + ignition::math::Vector3d(0, maxRange + 1, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox3 = scene->CreateVisual("UnitBox3"); + visualBox3->AddGeometry(scene->CreateBox()); + visualBox3->SetWorldPosition(box03Pose.Pos()); + visualBox3->SetWorldRotation(box03Pose.Rot()); + root->AddChild(visualBox3); + + // create particle emitter between sensor and box in the center + ignition::math::Vector3d particlePosition(1.0, 0, 0); + ignition::math::Vector3d particleSize(0.2, 0.2, 0.2); + ignition::rendering::ParticleEmitterPtr emitter = + scene->CreateParticleEmitter(); + emitter->SetLocalPosition(particlePosition); + emitter->SetParticleSize(particleSize); + emitter->SetRate(100); + emitter->SetLifetime(2); + emitter->SetVelocityRange(0.1, 0.1); + emitter->SetScaleRate(0.2); + emitter->SetColorRange(ignition::math::Color::Red, + ignition::math::Color::Black); + emitter->SetEmitting(true); + root->AddChild(emitter); + + // Verify rays caster 1 range readings + // listen to new gpu rays frames + unsigned int channels = gpuRays->Channels(); + float *scan = new float[hRayCount * vRayCount * channels]; + common::ConnectionPtr c = + gpuRays->ConnectNewGpuRaysFrame( + std::bind(&::OnNewGpuRaysFrame, scan, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + + int mid = static_cast(hRayCount / 2) * channels; + int last = (hRayCount - 1) * channels; + double unitBoxSize = 1.0; + double expectedRangeAtMidPointBox1 = + abs(box01Pose.Pos().X()) - unitBoxSize / 2; + double expectedRangeAtMidPointBox2 = + abs(box02Pose.Pos().Y()) - unitBoxSize / 2; + + // set a larger tol for particle range + double laserNoiseTol = particleSize.X(); + double expectedParticleRange = particlePosition.X(); + + // update 100 frames. There should be a descent chance that we will see both + // a particle hit and miss in the readings returned by the sensor + unsigned int particleHitCount = 0u; + unsigned int particleMissCount = 0u; + for (unsigned int i = 0u; i < 100u; ++i) + { + gpuRays->Update(); + + // sensor should see ether a particle or box01 + double particleRange = static_cast(scan[mid]); + bool particleHit = ignition::math::equal( + expectedParticleRange, particleRange, laserNoiseTol); + bool particleMiss = ignition::math::equal( + expectedRangeAtMidPointBox1, particleRange, LASER_TOL); + EXPECT_TRUE(particleHit || particleMiss) + << "actual vs expected particle range: " + << particleRange << " vs " << expectedParticleRange; + + particleHitCount += particleHit ? 1u : 0u; + particleMissCount += particleMiss ? 1u : 0u; + + // sensor should see box02 without noise or scatter effect + EXPECT_NEAR(expectedRangeAtMidPointBox2, scan[0], LASER_TOL); + + // sensor should not see box03 as it is out of range + EXPECT_DOUBLE_EQ(ignition::math::INF_D, scan[last]); + } + + // particles are sparse so there should be more misses than hits + EXPECT_GT(particleMissCount, particleHitCount); + // there should be at least one hit + EXPECT_GT(particleHitCount, 0u); + + c.reset(); + + delete [] scan; + + scan = nullptr; + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} ///////////////////////////////////////////////// TEST_P(GpuRaysTest, Configure) { @@ -477,6 +648,12 @@ TEST_P(GpuRaysTest, LaserVertical) LaserVertical(GetParam()); } +///////////////////////////////////////////////// +TEST_P(GpuRaysTest, RaysParticles) +{ + RaysParticles(GetParam()); +} + INSTANTIATE_TEST_CASE_P(GpuRays, GpuRaysTest, RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam());