openvdb_viewer.cpp

openvdb_test.cpp

/* -*-c++-*- */
/* osgEarth - Geospatial SDK for OpenSceneGraph
* Copyright 2020 Pelican Mapping
* http://osgearth.org
*
* osgEarth is free software; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
* IN THE SOFTWARE.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>
*/
#include "OsgImGuiHandler.hpp"
#include <osgEarth/LineDrawable>

#include <osg/Notify>
#include <osg/TriangleIndexFunctor>
#include <osgDB/ReadFile>
#include <osgDB/WriteFile>
#include <osgGA/GUIEventHandler>
#include <osgGA/StateSetManipulator>
#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>
#include <osgGA/TrackballManipulator>
#include <osgGA/StateSetManipulator>

#include <osgEarth/MapNode>
#include <osgEarth/ImageLayer>
#include <osgEarth/TMS>
#include <osgEarth/VirtualProgram>
#include <osgEarth/GLUtils>
#include <osgEarth/Lighting>

#include <osgEarth/ExampleResources>
#include <osgEarth/EarthManipulator>
#include <osgEarth/LogarithmicDepthBuffer>
#include <osgEarth/ShaderUtils>
#include <osgEarth/Registry>
#include <osgEarth/ShaderGenerator>
#include <osgEarth/MeshConsolidator>
#include <osgUtil/SmoothingVisitor>
#include <osgEarth/PhongLightingEffect>
#include <osgEarth/NodeUtils>


#include <chrono>
#include <thread>

#define NOMINMAX
#include <openvdb/openvdb.h>
#include <openvdb/tools/LevelSetSphere.h>
#include <openvdb/points/PointConversion.h>
#include <openvdb/points/PointCount.h>
#include <openvdb/math/Operators.h>
#include <openvdb/tools/Interpolation.h>

#include <openvdb/tools/VolumeToMesh.h>
#include <openvdb/tools/MeshToVolume.h>
#include <openvdb/tools/GridTransformer.h>
#include <openvdb/tools/Composite.h>
#include <openvdb/tools/ParticlesToLevelSet.h>
#include <openvdb/tools/VolumeToSpheres.h>


using namespace std;
using namespace osgEarth;
using namespace osgEarth::Util;
using namespace openvdb;
using namespace openvdb::math;

double randomBetween(double min, double max)
{
    return min + (max - min) * ((double)rand() / (double)RAND_MAX);
}

const char* pointFrag = R""(
#version 330
void point_color( inout vec4 color )
{
}
)"";

const char* pointVert = R""(
#version 330
void point_vert(inout vec4 VertexVIEW)
{
    gl_PointSize = 2.0;
}

)"";

const char* voxelFrag = R""(
#version 330
void voxel_color( inout vec4 color )
{
}
)"";

const char* voxelVert = R""(
#version 330
void voxel_vert(inout vec4 VertexVIEW)
{
}

)"";

class MyParticleList
{
protected:
    struct MyParticle {
        openvdb::Vec3R p, v;
        openvdb::Real  r;
    };
    openvdb::Real           mRadiusScale;
    openvdb::Real           mVelocityScale;
    std::vector<MyParticle> mParticleList;
public:

    typedef openvdb::Vec3R  PosType;

    MyParticleList(openvdb::Real rScale = 1, openvdb::Real vScale = 1)
        : mRadiusScale(rScale), mVelocityScale(vScale) {}
    void add(const openvdb::Vec3R &p, const openvdb::Real &r,
        const openvdb::Vec3R &v = openvdb::Vec3R(0, 0, 0))
    {
        MyParticle pa;
        pa.p = p;
        pa.r = r;
        pa.v = v;
        mParticleList.push_back(pa);
    }
    /// @return coordinate bbox in the space of the specified transfrom
    openvdb::CoordBBox getBBox(const openvdb::GridBase& grid) {
        openvdb::CoordBBox bbox;
        openvdb::Coord &min = bbox.min(), &max = bbox.max();
        openvdb::Vec3R pos;
        openvdb::Real rad, invDx = 1 / grid.voxelSize()[0];
        for (size_t n = 0, e = this->size(); n < e; ++n) {
            this->getPosRad(n, pos, rad);
            const openvdb::Vec3d xyz = grid.worldToIndex(pos);
            const openvdb::Real   r = rad * invDx;
            for (int i = 0; i < 3; ++i) {
                min[i] = openvdb::math::Min(min[i], openvdb::math::Floor(xyz[i] - r));
                max[i] = openvdb::math::Max(max[i], openvdb::math::Ceil(xyz[i] + r));
            }
        }
        return bbox;
    }
    //typedef int AttributeType;
    // The methods below are only required for the unit-tests
    openvdb::Vec3R pos(int n)   const { return mParticleList[n].p; }
    openvdb::Vec3R vel(int n)   const { return mVelocityScale * mParticleList[n].v; }
    openvdb::Real radius(int n) const { return mRadiusScale * mParticleList[n].r; }

    //////////////////////////////////////////////////////////////////////////////
    /// The methods below are the only ones required by tools::ParticleToLevelSet
    /// @note We return by value since the radius and velocities are modified
    /// by the scaling factors! Also these methods are all assumed to
    /// be thread-safe.

    /// Return the total number of particles in list.
    ///  Always required!
    size_t size() const { return mParticleList.size(); }

    /// Get the world space position of n'th particle.
    /// Required by ParticledToLevelSet::rasterizeSphere(*this,radius).
    void getPos(size_t n, openvdb::Vec3R&pos) const { pos = mParticleList[n].p; }


    void getPosRad(size_t n, openvdb::Vec3R& pos, openvdb::Real& rad) const {
        pos = mParticleList[n].p;
        rad = mRadiusScale * mParticleList[n].r;
    }
    void getPosRadVel(size_t n, openvdb::Vec3R& pos, openvdb::Real& rad, openvdb::Vec3R& vel) const {
        pos = mParticleList[n].p;
        rad = mRadiusScale * mParticleList[n].r;
        vel = mVelocityScale * mParticleList[n].v;
    }
    // The method below is only required for attribute transfer
    void getAtt(size_t n, openvdb::Index32& att) const { att = openvdb::Index32(n); }
};

// Populate the given grid with a narrow-band level set representation of a sphere.
// The width of the narrow band is determined by the grid's background value.
// (Example code only; use tools::createSphereSDF() in production.)
template<class GridType>
void
makeSphere(GridType& grid, float radius, const openvdb::Vec3f& c)
{
    using ValueT = typename GridType::ValueType;
    // Distance value for the constant region exterior to the narrow band
    const ValueT outside = grid.background();
    // Distance value for the constant region interior to the narrow band
    // (by convention, the signed distance is negative in the interior of
    // a level set)
    const ValueT inside = -outside;
    // Use the background value as the width in voxels of the narrow band.
    // (The narrow band is centered on the surface of the sphere, which
    // has distance 0.)
    int padding = int(openvdb::math::RoundUp(openvdb::math::Abs(outside)));
    // The bounding box of the narrow band is 2*dim voxels on a side.
    int dim = int(radius + padding);
    // Get a voxel accessor.
    typename GridType::Accessor accessor = grid.getAccessor();
    // Compute the signed distance from the surface of the sphere of each
    // voxel within the bounding box and insert the value into the grid
    // if it is smaller in magnitude than the background value.
    openvdb::Coord ijk;
    int &i = ijk[0], &j = ijk[1], &k = ijk[2];
    for (i = c[0] - dim; i < c[0] + dim; ++i) {
        const float x2 = openvdb::math::Pow2(i - c[0]);
        for (j = c[1] - dim; j < c[1] + dim; ++j) {
            const float x2y2 = openvdb::math::Pow2(j - c[1]) + x2;
            for (k = c[2] - dim; k < c[2] + dim; ++k) {
                // The distance from the sphere surface in voxels
                const float dist = openvdb::math::Sqrt(x2y2
                    + openvdb::math::Pow2(k - c[2])) - radius;
                // Convert the floating-point distance to the grid's value type.
                ValueT val = ValueT(dist);
                // Only insert distances that are smaller in magnitude than
                // the background value.
                if (val < inside || outside < val) continue;
                // Set the distance for voxel (i,j,k).
                accessor.setValue(ijk, val);
            }
        }
    }
    // Propagate the outside/inside sign information from the narrow band
    // throughout the grid.
    openvdb::tools::signedFloodFill(grid.tree());
}

osg::Vec4 randomColor()
{
    float r = (float)rand() / (float)RAND_MAX;
    float g = (float)rand() / (float)RAND_MAX;
    float b = (float)rand() / (float)RAND_MAX;
    return osg::Vec4(r, g, b, 1.0f);
}

osg::Vec4 getColorForLevel(unsigned int level)
{
    static std::unordered_map< unsigned int, osg::Vec4 > colors;
    auto itr = colors.find(level);
    osg::Vec4 color;
    if (itr != colors.end())
    {
        color = itr->second;
    }
    else
    {
        color = randomColor();
        colors[level] = color;
    }
    return color;
}

struct Measurement
{
    osg::Vec3d position;
    float range;
    float intensity;
    double time;
    osg::Vec3ub color;
    unsigned int cls;
};

typedef std::vector< Measurement > MeasurementList;

bool LoadMeasurements(const std::string& filename, MeasurementList& output)
{
    output.clear();

    FILE* in = fopen(filename.c_str(), "r");

    if (!in) {
        std::cout << "Error opening file: " << filename << '\n';
        return false;
    }

    Measurement m;
    int numLines = 0;
    // Skip the first line
    char line[1024];
    fgets(line, 1024, in);
    while (fscanf(in, "%lf,%lf,%lf,%f,%f,%lf,%u,%hhu,%hhu,%hhu\n",
        &m.position.x(), &m.position.y(), &m.position.z(),
        &m.range, &m.intensity, &m.time, &m.cls,
        &m.color.r(), &m.color.g(), &m.color.b()
    ) == 10)
    {
        output.push_back(m);
        numLines++;
    }
    fclose(in);
    return true;
}


osg::Node* RenderBoundingBox(const osg::BoundingBoxd& boundingBox, const osg::Vec4& color)
{
    const int index[24] = {
                0, 1, 1, 2, 2, 3, 3, 0,
                4, 5, 5, 6, 6, 7, 7, 4,
                0, 4, 1, 5, 2, 6, 3, 7
    };

    std::vector< osg::Vec3 > corners;
    LineDrawable* d = new LineDrawable(GL_LINES);
    d->setUseGPU(false);
    corners.push_back(osg::Vec3(boundingBox.xMin(), boundingBox.yMin(), boundingBox.zMin()));
    corners.push_back(osg::Vec3(boundingBox.xMax(), boundingBox.yMin(), boundingBox.zMin()));
    corners.push_back(osg::Vec3(boundingBox.xMax(), boundingBox.yMax(), boundingBox.zMin()));
    corners.push_back(osg::Vec3(boundingBox.xMin(), boundingBox.yMax(), boundingBox.zMin()));
    corners.push_back(osg::Vec3(boundingBox.xMin(), boundingBox.yMin(), boundingBox.zMax()));
    corners.push_back(osg::Vec3(boundingBox.xMax(), boundingBox.yMin(), boundingBox.zMax()));
    corners.push_back(osg::Vec3(boundingBox.xMax(), boundingBox.yMax(), boundingBox.zMax()));
    corners.push_back(osg::Vec3(boundingBox.xMin(), boundingBox.yMax(), boundingBox.zMax()));

    for (int i = 0; i < 24; ++i)
        d->pushVertex(corners[index[i]]);

    d->setColor(color);
    d->finish();
    return d;
}

void AddCube(const osg::Vec3f& position, float width, osg::Vec3Array* verts, osg::Vec4Array* colors, osg::DrawElementsUInt* de)
{
    float halfLenth = (float)width / 2.0f;
    float minX = position.x() - halfLenth;
    float maxX = position.x() + halfLenth;
    float minY = position.y() - halfLenth;
    float maxY = position.y() + halfLenth;
    float minZ = position.z() - halfLenth;
    float maxZ = position.z() + halfLenth;

    unsigned int startIndex = verts->size();

    verts->push_back(osg::Vec3(minX, minY, minZ));
    verts->push_back(osg::Vec3(maxX, minY, minZ));
    verts->push_back(osg::Vec3(maxX, maxY, minZ));
    verts->push_back(osg::Vec3(minX, maxY, minZ));

    verts->push_back(osg::Vec3(minX, minY, maxZ));
    verts->push_back(osg::Vec3(maxX, minY, maxZ));
    verts->push_back(osg::Vec3(maxX, maxY, maxZ));
    verts->push_back(osg::Vec3(minX, maxY, maxZ));

    osg::Vec4 color = randomColor();

    for (unsigned int i = 0; i < 8; i++)
    {
        colors->push_back(color);
    }

    // Bottom
    de->push_back(startIndex + 0); de->push_back(startIndex + 2); de->push_back(startIndex + 1);
    de->push_back(startIndex + 0); de->push_back(startIndex + 3); de->push_back(startIndex + 2);

    // Top
    de->push_back(startIndex + 4); de->push_back(startIndex + 5); de->push_back(startIndex + 6);
    de->push_back(startIndex + 4); de->push_back(startIndex + 6); de->push_back(startIndex + 7);

    // Right
    de->push_back(startIndex + 1); de->push_back(startIndex + 2); de->push_back(startIndex + 6);
    de->push_back(startIndex + 1); de->push_back(startIndex + 6); de->push_back(startIndex + 5);

    // Left
    de->push_back(startIndex + 3); de->push_back(startIndex + 0); de->push_back(startIndex + 4);
    de->push_back(startIndex + 3); de->push_back(startIndex + 4); de->push_back(startIndex + 7);

    // Front
    de->push_back(startIndex + 0); de->push_back(startIndex + 1); de->push_back(startIndex + 5);
    de->push_back(startIndex + 0); de->push_back(startIndex + 5); de->push_back(startIndex + 4);

    // Back
    de->push_back(startIndex + 3); de->push_back(startIndex + 7); de->push_back(startIndex + 6);
    de->push_back(startIndex + 3); de->push_back(startIndex + 6); de->push_back(startIndex + 2);
}

void GenerateRandomPoints(const osg::BoundingBoxd& bounds, unsigned int numPoints, std::vector<osg::Vec3d>& points)
{
    points.reserve(numPoints);
    for (unsigned int i = 0; i < numPoints; i++)
    {
        float x = randomBetween(bounds.xMin(), bounds.xMax());
        float y = randomBetween(bounds.yMin(), bounds.yMax());
        float z = randomBetween(bounds.zMin(), bounds.zMax());
        points.emplace_back(x, y, z);
    }
}


static int s_numPoints = 100000;
static int s_rate = 0.0;
static osg::Timer_t s_lastInsertTime;
static osg::Group* s_root = new osg::Group;
static osg::Node* voxelDatabaseNode = nullptr;
static osg::Node* insertNode = nullptr;
static int s_renderDepth = 0;
static float s_renderResolution = 0.5;
static float s_isoValue = 0.0f;
static float s_adaptivity = 0.5f;
static float s_bulletSize = 1.0;
static bool s_showSpheres = false;

openvdb::FloatGrid::Ptr voxelGrid;

float insertX = 0.0f, insertY = 0.0f, insertZ = 0.0f;
float insertWidth = 100.0f, insertDepth = 100.0f, insertHeight = 100.0;

osg::Node* RenderGrid(openvdb::FloatGrid::Ptr grid, float resolution)
{
    osg::Geometry* geometry = new osg::Geometry;
    geometry->setUseVertexBufferObjects(true);
    geometry->setUseDisplayList(false);

    osg::Vec3Array* verts = new osg::Vec3Array;
    geometry->setVertexArray(verts);

    osg::Vec4Array* colors = new osg::Vec4Array();
    geometry->setColorArray(colors, osg::Array::BIND_PER_VERTEX);

    osg::DrawElementsUInt* de = new osg::DrawElementsUInt(GL_TRIANGLES);
    geometry->addPrimitiveSet(de);

    osgEarth::ShaderUtils::installDefaultShader(geometry->getOrCreateStateSet());

    for (auto iter = grid->beginValueOn(); iter; ++iter) {
        openvdb::Vec3d worldCoord = grid->indexToWorld(iter.getCoord());
        AddCube(osg::Vec3(worldCoord.x(), worldCoord.y(), worldCoord.z()), resolution, verts, colors, de);
    }

    return geometry;
}

osg::Node* RenderGridUsingVolumeToMesh(openvdb::FloatGrid::Ptr grid)
{
    std::vector<Vec3s> points;
    std::vector<Vec3I> triangles;
    std::vector<Vec4I> quads;
    tools::volumeToMesh(*grid, points, triangles, quads, s_isoValue, s_adaptivity, true);

    osg::Geometry* geometry = new osg::Geometry;
    geometry->setUseVertexBufferObjects(true);
    geometry->setUseDisplayList(false);

    osg::Vec3Array* verts = new osg::Vec3Array;
    geometry->setVertexArray(verts);
    for (unsigned int i = 0; i < points.size(); i++)
    {
        verts->push_back(osg::Vec3f(points[i].x(), points[i].y(), points[i].z()));
    }

    osg::Vec4Array* colors = new osg::Vec4Array();
    geometry->setColorArray(colors, osg::Array::BIND_OVERALL);
    colors->push_back(osg::Vec4(1, 0, 0, 1));

    osg::DrawElementsUInt* de = new osg::DrawElementsUInt(GL_TRIANGLES);
    geometry->addPrimitiveSet(de);

    for (unsigned int i = 0; i < quads.size(); i++)
    {
        de->push_back(quads[i][0]);
        de->push_back(quads[i][1]);
        de->push_back(quads[i][2]);

        de->push_back(quads[i][0]);
        de->push_back(quads[i][2]);
        de->push_back(quads[i][3]);
    }

    for (unsigned int i = 0; i < triangles.size(); i++)
    {
        de->push_back(triangles[i][0]);
        de->push_back(triangles[i][1]);
        de->push_back(triangles[i][2]);
    }

    osgUtil::SmoothingVisitor sv;
    geometry->accept(sv);

    auto material = new osg::Material;
    material->setDiffuse(osg::Material::FRONT, osg::Vec4(1, 1, 1, 1));
    material->setAmbient(osg::Material::FRONT, osg::Vec4(1, 1, 1, 1));
    geometry->getOrCreateStateSet()->setAttributeAndModes(material);

    // Add phong lighting.
    PhongLightingEffect* phong = new PhongLightingEffect();
    phong->attach(geometry->getOrCreateStateSet());

    osgEarth::Registry::shaderGenerator().run(geometry, osgEarth::Registry::stateSetCache());

    return geometry;
}

osg::Node* RenderGridUsingSpheres(openvdb::FloatGrid::Ptr grid)
{
    std::vector<openvdb::Vec4s> spheres;
    tools::fillWithSpheres(*grid, spheres, 20000, false);

    cout << "Got " << spheres.size() << " spheres" << std::endl;

    osg::Group* group = new osg::Group;
    for (const auto& sphere : spheres)
    {
        osg::BoundingBox bounds;
        bounds.expandBy(osg::BoundingSphere(osg::Vec3f(sphere.x(), sphere.y(), sphere.z()), sphere.w()));
        group->addChild(RenderBoundingBox(bounds, Color::Yellow));
    }
    return group;

}

osg::Node* RenderGridSampled(openvdb::FloatGrid::Ptr grid, float resolution)
{
    // https://www.openvdb.org/documentation/doxygen/codeExamples.html#sXformTools
    auto outputGrid = FloatGrid::create(0.0);
    outputGrid->setTransform(openvdb::math::Transform::createLinearTransform(resolution));
    outputGrid->setGridClass(openvdb::GRID_LEVEL_SET);

    const openvdb::math::Transform
        &sourceXform = voxelGrid->transform(),
        &targetXform = outputGrid->transform();
    // Compute a source grid to target grid transform.
    // (For this example, we assume that both grids' transforms are linear,
    // so that they can be represented as 4 x 4 matrices.)
    openvdb::Mat4R xform =
        sourceXform.baseMap()->getAffineMap()->getMat4() *
        targetXform.baseMap()->getAffineMap()->getMat4().inverse();
    std::cout << "Resampling " << std::endl;

    // Create the transformer.
    openvdb::tools::GridTransformer transformer(xform);
    // Resample using nearest-neighbor interpolation.
    //transformer.transformGrid<openvdb::tools::PointSampler, openvdb::FloatGrid>(*voxelGrid, *outputGrid);
    transformer.transformGrid<openvdb::tools::BoxSampler, openvdb::FloatGrid>(*voxelGrid, *outputGrid);

    std::cout << "Input grid has " << voxelGrid->tree().leafCount() << " leaves" << std::endl;
    std::cout << "Output grid has " << outputGrid->tree().leafCount() << " leaves" << std::endl;

    // Prune the target tree for optimal sparsity.
    outputGrid->tree().prune();

    return RenderGrid(outputGrid, resolution);
}

static void RedrawVoxels()
{
    if (voxelDatabaseNode)
    {
        s_root->removeChild(voxelDatabaseNode);
    }
    osg::Group* group = new osg::Group;
    //voxelDatabaseNode = RenderGridUsingVolumeToMesh(voxelGrid);
    voxelDatabaseNode = group;
    group->addChild(RenderGridUsingVolumeToMesh(voxelGrid));
    if (s_showSpheres)
    {
        group->addChild(RenderGridUsingSpheres(voxelGrid));
    }

    s_root->addChild(voxelDatabaseNode);
}


// An event handler that will print out the elevation at the clicked point
struct ShootHandler : public osgGA::GUIEventHandler
{
    ShootHandler()
    {
    }

    void shoot(float x, float y, osgViewer::View* view)
    {
        osg::Vec3d world;
        osgUtil::LineSegmentIntersector::Intersections hits;
        if (view->computeIntersections(x, y, hits))
        {
            world = hits.begin()->getWorldIntersectPoint();
            auto cutGrid = openvdb::tools::createLevelSetSphere<FloatGrid>(s_bulletSize, openvdb::Vec3f(world.x(), world.y(), world.z()), voxelGrid->voxelSize()[0]);
            openvdb::tools::csgDifference(*voxelGrid, *cutGrid);
            RedrawVoxels();
        }
    }

    bool handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa)
    {
        osgViewer::View* view = static_cast<osgViewer::View*>(aa.asView());

        if (ea.getEventType() == ea.DOUBLECLICK && ea.getButton() == ea.LEFT_MOUSE_BUTTON)
        {
            shoot(ea.getX(), ea.getY(), view);
            return true;
        }
        return false;
    }
};




class ImGuiHandler : public OsgImGuiHandler
{
public:
    ImGuiHandler()
    {
    }

protected:
    virtual void drawUi(osg::RenderInfo& renderInfo)
    {
        // ImGui code goes here...
        //ImGui::ShowDemoWindow();

        bool voxelsDirty = false;

        //voxelsDirty |= ImGui::SliderInt("Render Depth", &s_renderDepth, 0, tree.getTreeDepth());
        voxelsDirty |= ImGui::SliderFloat("Render Resolution", &s_renderResolution, 0.5, 500.0);
        voxelsDirty |= ImGui::SliderFloat("Iso Value", &s_isoValue, 0.0, 5.0);
        voxelsDirty |= ImGui::SliderFloat("Adaptivity Value", &s_adaptivity, 0.0, 5.0);

        bool insertBoundsDirty = false;

        ImGui::Separator();
        insertBoundsDirty |= ImGui::SliderFloat("X", &insertX, -500.0, 500.0);
        insertBoundsDirty |= ImGui::SliderFloat("Y", &insertY, -500.0, 500.0);
        insertBoundsDirty |= ImGui::SliderFloat("Z", &insertZ, -500.0, 500.0);

        insertBoundsDirty |= ImGui::SliderFloat("Width", &insertWidth, 5.0, 500.0);
        insertBoundsDirty |= ImGui::SliderFloat("Depth", &insertDepth, 5.0, 500.0);
        insertBoundsDirty |= ImGui::SliderFloat("Height", &insertHeight, 5.0, 500.0);

        insertBoundsDirty |= ImGui::SliderFloat("Bullet Size", &s_bulletSize, 1.0, 20.0);

        voxelsDirty = ImGui::Checkbox("Show spheres", &s_showSpheres);

        osg::BoundingBoxd insertBounds = osg::BoundingBoxd(insertX, insertY, insertZ, insertX + insertWidth, insertY + insertDepth, insertZ + insertHeight);
        float timeSinceLastInsert = osg::Timer::instance()->delta_s(s_lastInsertTime, osg::Timer::instance()->tick());
        ImGui::SliderInt("Inserts/s", &s_rate, 0, 10);
        ImGui::SliderInt("Point Count", &s_numPoints, 100, 30000);
        float insertTime = 1.0f / (float)s_rate;
        if (ImGui::Button("Insert") || (s_rate > 0.0 && timeSinceLastInsert > insertTime))
        {
            voxelsDirty = true;
            std::vector< osg::Vec3d > points;
            GenerateRandomPoints(insertBounds, s_numPoints, points);
            auto acc = voxelGrid->getAccessor();
            for (const auto& p : points)
            {
                openvdb::Vec3d point{ p.x(), p.y(), p.z() };
                const auto idx = voxelGrid->worldToIndex(point);
                const auto coord = openvdb::Coord(idx[0], idx[1], idx[2]);
                acc.setValueOn(coord, 1.0);
            }
            s_lastInsertTime = osg::Timer::instance()->tick();
        }

        if (ImGui::Button("Add measurements"))
        {
            voxelsDirty = true;
            //AddMeasurements(tree);
        }

        if (ImGui::Button("Cut"))
        {
            openvdb::BBoxd bbox(openvdb::Vec3d(insertBounds.xMin(), insertBounds.yMin(), insertBounds.zMin()),
                                openvdb::Vec3d(insertBounds.xMax(), insertBounds.yMax(), insertBounds.zMax()));
            Transform::Ptr transform = Transform::createLinearTransform(0.3);
            auto cutGrid = openvdb::tools::createLevelSetBox<FloatGrid>(bbox, *transform );
            openvdb::tools::csgDifference(*voxelGrid, *cutGrid);
            voxelsDirty = true;
        }

        if (ImGui::Button("Union"))
        {
            openvdb::BBoxd bbox(openvdb::Vec3d(insertBounds.xMin(), insertBounds.yMin(), insertBounds.zMin()),
                openvdb::Vec3d(insertBounds.xMax(), insertBounds.yMax(), insertBounds.zMax()));
            Transform::Ptr transform = Transform::createLinearTransform(0.3);
            auto cutGrid = openvdb::tools::createLevelSetBox<FloatGrid>(bbox, *transform);
            openvdb::tools::csgUnion(*voxelGrid, *cutGrid);
            voxelsDirty = true;
        }

        if (voxelsDirty)
        {
            RedrawVoxels();
        }

        if (insertBoundsDirty || !insertNode)
        {
            if (insertNode)
            {
                s_root->removeChild(insertNode);
            }
            insertNode = RenderBoundingBox(insertBounds, Color::Red);
            s_root->addChild(insertNode);
        }
    }
};

class VertexCollectionVisitor : public osg::NodeVisitor
{
public:

    std::vector<Vec3s> *points;

    VertexCollectionVisitor(TraversalMode traversalMode = TRAVERSE_ALL_CHILDREN):
        osg::NodeVisitor(traversalMode)
    {
    }

    void apply(osg::Node& node)
    {
        traverse(node);
    }

    void apply(osg::Transform& transform)
    {
        osg::Matrix matrix;
        if (!_matrixStack.empty()) matrix = _matrixStack.back();

        transform.computeLocalToWorldMatrix(matrix, this);

        pushMatrix(matrix);

        traverse(transform);

        popMatrix();
    }

    void apply(osg::Drawable& drawable)
    {
        osg::Geometry* geometry = drawable.asGeometry();
        if (geometry)
        {
            osg::Vec3Array* verts = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
            if (verts)
            {
                if (_matrixStack.empty())
                {
                    for (osg::Vec3Array::iterator it = verts->begin(); it != verts->end(); ++it)
                        addVertex(osg::Vec3d(*it));
                }
                else
                {
                    osg::Matrix& matrix = _matrixStack.back();
                    for (osg::Vec3Array::iterator it = verts->begin(); it != verts->end(); ++it)
                        addVertex(osg::Vec3d(*it) * matrix);
                }
            }
        }
    }

    inline void pushMatrix(osg::Matrix& matrix) { _matrixStack.push_back(matrix); }

    inline void popMatrix() { _matrixStack.pop_back(); }

protected:

    typedef std::vector<osg::Matrix> MatrixStack;
    MatrixStack _matrixStack;

    osg::Vec3d _anchor;
    bool _firstPoint = true;

    void addVertex(osg::Vec3d vertex)
    {
        // Anchor around origin just for demo purposes.
        if (_firstPoint)
        {
            _anchor = vertex;
            _firstPoint = false;
        }

        osg::Vec3d v = vertex - _anchor;
        points->push_back(Vec3s(v.x(), v.y(), v.z()));
    }
};

struct TriangleCollector
{
    std::vector<Vec3I> *triangles;

    TriangleCollector()
    {
    }

    void operator()(unsigned i0, unsigned i1, unsigned i2)
    {
        triangles->push_back(Vec3I(i0, i1, i2));
    }
};

struct MeshWrapper {

    std::vector<Vec3s> points;
    std::vector<Vec3I> triangles;

    size_t polygonCount() const {
        //cout << "Polygon count " << triangles.size() / 3 << std::endl;
        return triangles.size() / 3;
    }
    size_t pointCount() {
        //cout << "Point count " << points.size() << std::endl;
        return points.size();
    }
    size_t vertexCount(size_t n) const { // Vertex count for polygon n
        //cout << "Vertex count " << points.size() << std::endl;
        return 3;
    }

    // Return position pos in local grid index space for polygon n and vertex v
    void getIndexSpacePoint(size_t n, size_t v, openvdb::Vec3d& pos) const {
        /*
        ofVec3f pt = mesh->getVertex(mesh->getIndex(n * 3 + v));
        pos[0] = pt.x;
        pos[1] = pt.y;
        pos[2] = pt.z;
        */
        //cout << "Getting pos" << std::endl;
        pos = openvdb::Vec3d(triangles[n][v]);
    }

    MeshWrapper()
    {
    }
};

class MeshCollectionVisitor : public osg::NodeVisitor
{
public:

    MeshCollectionVisitor(TraversalMode traversalMode = TRAVERSE_ALL_CHILDREN):
        osg::NodeVisitor(traversalMode)
    {
    }

    void apply(osg::Node& node)
    {
        std::cout << "Traversing " << node.className() << std::endl;
        traverse(node);
    }

    /*
    virtual void apply(osg::Geode& node)
    {
        cout << "Found a geode " << std::endl;
    }
    */

    void apply(osg::Transform& transform)
    {
        osg::Matrix matrix;
        if (!_matrixStack.empty()) matrix = _matrixStack.back();

        transform.computeLocalToWorldMatrix(matrix, this);

        pushMatrix(matrix);

        traverse(transform);

        popMatrix();
    }

    void apply(osg::Drawable& drawable)
    {
        osg::Geometry* geometry = drawable.asGeometry();
        if (geometry)
        {
            auto grid = openvdb::createLevelSet<openvdb::FloatGrid>(0.5, 1.0);

            std::vector<Vec3s> points;
            std::vector<Vec3I> triangles;
            std::vector<Vec4I> quads;

            // Convert the geometry to triangles
            MeshConsolidator::convertToTriangles(*geometry);

            //Let's assume we've never hit this Geometry before so now we need to turn it into a mesh
            osg::Vec3Array* verts = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
            if (verts)
            {
                osg::Vec3d anchor = (*verts)[0];

                if (_matrixStack.empty())
                {
                    for (osg::Vec3Array::iterator it = verts->begin(); it != verts->end(); ++it)
                    {
                        osg::Vec3d v = osg::Vec3d(*it) - anchor;
                        points.push_back(Vec3s(v.x(), v.y(), v.z()));
                    }
                }
                else
                {
                    osg::Matrix& matrix = _matrixStack.back();
                    for (osg::Vec3Array::iterator it = verts->begin(); it != verts->end(); ++it)
                    {
                        osg::Vec3d v = (osg::Vec3d(*it) * matrix) - anchor;
                        points.push_back(Vec3s(v.x(), v.y(), v.z()));
                    }
                }
            }

            osg::TriangleIndexFunctor < TriangleCollector > collectTriangles;
            collectTriangles.triangles = &triangles;
            geometry->accept(collectTriangles);
            auto mesh = tools::meshToLevelSet<FloatGrid>(grid->transform(), points, triangles, quads);

            /*
            MeshWrapper mesher;
            mesher.points = points;
            mesher.triangles = triangles;
            auto mesh = tools::meshToVolume<openvdb::FloatGrid>(mesher, grid->transform(), 3.0f, 3.0f);
            */
            _meshes.push_back(mesh);

        }
    }

    inline void pushMatrix(osg::Matrix& matrix) { _matrixStack.push_back(matrix); }

    inline void popMatrix() { _matrixStack.pop_back(); }

    std::vector<FloatGrid::Ptr> _meshes;

protected:

    typedef std::vector<osg::Matrix> MatrixStack;
    MatrixStack _matrixStack;


};

FloatGrid::Ptr NodeToMesh(osg::Node* node)
{
    // The resolution of all levels need to match or we need to do a linear transform between them.
    //Transform::Ptr transform = Transform::createLinearTransform(0.05); // good for cow
    auto grid = openvdb::createLevelSet<openvdb::FloatGrid>(0.5, 1.0);

    MeshCollectionVisitor collector;
    node->accept(collector);

    cout << "Found " << collector._meshes.size() << " meshes" << std::endl;

    for (auto& mesh : collector._meshes)
    {
        openvdb::tools::csgUnion(*grid, *mesh);
    }


    /*
    for (auto& g : finder._results)
    {
        std::cout << "Doing geometry " << std::endl;
        MeshConsolidator::convertToTriangles(*g);

        std::vector<Vec3s> points;
        std::vector<Vec3I> triangles;
        std::vector<Vec4I> quads;

        VertexCollectionVisitor collectVerts;
        collectVerts.points = &points;
        g->accept(collectVerts);

        osg::TriangleIndexFunctor < TriangleCollector > collectTriangles;
        collectTriangles.triangles = &triangles;
        g->accept(collectTriangles);
        auto mesh = tools::meshToLevelSet<FloatGrid>(grid->transform(), points, triangles, quads);
        openvdb::tools::csgUnion(*grid, *mesh);
    }
    */
    return grid;
}


int
main(int argc, char** argv)
{
    osgEarth::initialize();
    osg::ArgumentParser arguments(&argc, argv);

    osgViewer::Viewer viewer(arguments);

    // Initialize grid types and point attributes types.
    openvdb::initialize();

    /*
    osg::ref_ptr< osg::Node > readNode = osgDB::readNodeFiles(arguments);
    if (readNode.valid())
    {
        voxelGrid = NodeToMesh(readNode.get());
    }
    else
    {
        voxelGrid = openvdb::tools::createLevelSetSphere<FloatGrid>(50.0f, openvdb::Vec3f(0.0, 0.0, 0.0), 0.3f);
    }
    */

    // TODO:  look at fillWithSpheres function in openvdb.
    // https://tinyhawkus.files.wordpress.com/2018/10/vdb_sphere_packing.jpg
    // that link talks about using it to generate spheres for collision detection.  Really cool.



#if 0
    MeasurementList measurements;
    LoadMeasurements("D:/geodata/Elbit/ESL_DVE_Data/ESA_DVEM_Data/netanya_tzafon_flyby_mode_4_day_2_v1_files_2_2_IDS_fgOpt_Hybrid/accumulated_point_cloud_gpsins.csv", measurements);
    cout << "Loaded " << measurements.size() << std::endl;

    voxelGrid = openvdb::FloatGrid::create(0.0);
    voxelGrid->setTransform(
        openvdb::math::Transform::createLinearTransform(/*voxel size=*/0.5));
    voxelGrid->setGridClass(openvdb::GRID_LEVEL_SET);

    auto acc = voxelGrid->getAccessor();

    osg::BoundingBoxd boundingBox;

    auto start_time = chrono::high_resolution_clock::now();
    for (const auto& m : measurements)
    {
        openvdb::Vec3d point{ m.position.y(), m.position.x(), -m.position.z() };
        const auto idx = voxelGrid->worldToIndex(point);
        const auto coord = openvdb::Coord(idx[0], idx[1], idx[2]);
        acc.setValueOn(coord, 1.0);
    }
    auto end_time = chrono::high_resolution_clock::now();
    long long totalTime = chrono::duration_cast<chrono::milliseconds>(end_time - start_time).count();
    cout << "Inserted " << measurements.size() << " in " << totalTime << "ms" << std::endl;

    auto nodeCount = voxelGrid->tree().nodeCount();
    for (unsigned int level = 0; level < nodeCount.size(); ++level)
    {
        cout << "Level " << level << " has " << nodeCount[level] << "nodes" << endl;
    }
    cout << "Leaf count = " << voxelGrid->tree().leafCount() << std::endl;
#endif

#if 1
    // Measurements as particles
    MeasurementList measurements;
    LoadMeasurements("D:/geodata/Elbit/ESL_DVE_Data/ESA_DVEM_Data/netanya_tzafon_flyby_mode_4_day_2_v1_files_2_2_IDS_fgOpt_Hybrid/accumulated_point_cloud_gpsins.csv", measurements);
    cout << "Loaded " << measurements.size() << std::endl;

    MyParticleList pa;
    for (const auto& m : measurements)
    {
        openvdb::Vec3d point{ m.position.y(), m.position.x(), -m.position.z() };
        pa.add(point, 1.0);
    }

    voxelGrid = openvdb::createLevelSet<openvdb::FloatGrid>(0.5, 1.0);
    openvdb::tools::ParticlesToLevelSet<openvdb::FloatGrid> raster(*voxelGrid);
    raster.setGrainSize(1);//a value of zero disables threading
    raster.rasterizeSpheres(pa);
    raster.finalize();
#endif
    viewer.setCameraManipulator(new osgGA::TrackballManipulator());

    viewer.setRealizeOperation(new GlewInitOperation);
    viewer.realize();

    viewer.setRealizeOperation(new osgEarth::GL3RealizeOperation());

    viewer.getEventHandlers().push_front(new ImGuiHandler());

    viewer.addEventHandler(new osgViewer::StatsHandler());
    viewer.addEventHandler(new osgViewer::LODScaleHandler());
    viewer.addEventHandler(new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()));
    viewer.setSceneData(s_root);

    auto lights = new osg::Group;
    osg::Light* light0 = new osg::Light(0);
    light0->setPosition(osg::Vec4(1, 1, 1, 0.0));
    light0->setAmbient(osg::Vec4(0.2, 0.2, 0.2, 1.0));
    light0->setDiffuse(osg::Vec4(1.0, 1.0, 0.9, 1.0));
    osg::LightSource* lightSource0 = new osg::LightSource();
    lightSource0->setLight(light0);
    lights->addChild(lightSource0);

    osg::Light* light1 = new osg::Light(1);
    light1->setPosition(osg::Vec4(-1, -1, -1, 0.0));
    light1->setAmbient(osg::Vec4(0.2, 0.2, 0.2, 1.0));
    light1->setDiffuse(osg::Vec4(1.0, 1.0, 0.9, 1.0));
    osg::LightSource* lightSource1 = new osg::LightSource();
    lightSource1->setLight(light1);
    lights->addChild(lightSource1);

    // Generate the necessary uniforms for the shaders.
    GenerateGL3LightingUniforms gen;
    lights->accept(gen);

    s_root->addChild(lights);

    //voxelDatabaseNode = RenderTree(tree, s_renderDepth);
    //voxelDatabaseNode = RenderGridSampled(voxelGrid, 0.5);
    //voxelDatabaseNode = RenderGridUsingVolumeToMesh(voxelGrid);
    //s_root->addChild(voxelDatabaseNode);
    RedrawVoxels();

    viewer.addEventHandler(new ShootHandler());

    osgEarth::Registry::shaderGenerator().run(s_root, osgEarth::Registry::stateSetCache());



    return viewer.run();
}