ENH: implemented spatial hashing and added an abstract class

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTable.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   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.txt
+
+   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 "imstkSpatialHashTable.h"
+
+namespace imstk
+{
+
+SpatialHashTable::SpatialHashTable()
+{
+    m_cellSize[0] = 0.1;
+    m_cellSize[1] = 0.1;
+    m_cellSize[2] = 0.1;
+}
+
+}

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTable.h

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   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.txt
+
+   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 imstkSpatialHashTable_h
+#define imstkSpatialHashTable_h
+
+#include "imstkMath.h"
+
+namespace imstk
+{
+
+///
+/// \class SpatialHashTable
+///
+/// \brief Abstract class for spatial hash tables
+///
+class SpatialHashTable
+{
+public:
+    ///
+    /// \brief Protected constructor
+    /// \param x,y,z Dimensions for each cell
+    ///
+    virtual void setCellSize(double x, double y, double z);
+
+protected:
+    ///
+    /// \brief Rehash the hash table
+    ///
+    virtual void rehash();
+
+    ///
+    /// \brief Protected constructor
+    ///
+    SpatialHashTable();
+
+    double m_cellSize[3];
+};
+
+}
+
+#endif

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTableSeparateChaining.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   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.txt
+
+   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 "imstkSpatialHashTableSeparateChaining.h"
+
+namespace imstk
+{
+
+SpatialHashTableSeparateChaining::SpatialHashTableSeparateChaining()
+    : SpatialHashTable()
+{
+    m_table = std::make_shared<std::unordered_set<PointEntry>>();
+    this->clearTable();
+}
+
+void
+SpatialHashTableSeparateChaining::insertPoints(StdVectorOfVec3d points)
+{
+    // TODO: make more efficient
+    for (auto i = 0; i < points.size(); i++)
+    {
+        this->insertPoint(points[i]);
+    }
+}
+
+void
+SpatialHashTableSeparateChaining::insertPoint(Vec3d point)
+{
+    PointEntry entry;
+    entry.point = point;
+    entry.ID = m_currentID;
+    entry.cellSize = m_cellSize;
+
+    m_table->insert(entry);
+
+    m_currentID++;
+}
+
+void
+SpatialHashTableSeparateChaining::clearTable()
+{
+    m_table->clear();
+    m_currentID = 0;
+}
+
+std::vector<Vec3d>
+SpatialHashTableSeparateChaining::getPointsInAABB(Vec3d corner1, Vec3d corner2)
+{
+    auto min_x = std::fmin(corner1.x(), corner2.x());
+    auto max_x = std::fmax(corner1.x(), corner2.x());
+    auto min_y = std::fmin(corner1.y(), corner2.y());
+    auto max_y = std::fmax(corner1.y(), corner2.y());
+    auto min_z = std::fmin(corner1.z(), corner2.z());
+    auto max_z = std::fmax(corner1.z(), corner2.z());
+
+    std::unordered_set<PointEntry> tempPoints(0);
+    
+    // Coarse iteration (false positives may exist)
+    for (double x = min_x; x < max_x + m_cellSize[0]; x += m_cellSize[0])
+    for (double y = min_y; y < max_y + m_cellSize[1]; y += m_cellSize[1])
+    for (double z = min_z; z < max_z + m_cellSize[2]; z += m_cellSize[2])
+    {
+        PointEntry point;
+        point.point = Vec3d(x, y, z);
+        point.cellSize = m_cellSize;
+
+        auto bucket = m_table->bucket(point);
+
+        auto first = m_table->begin(bucket);
+        auto last = m_table->end(bucket);
+
+        for (auto p = first; p != last; ++p)
+        {
+            tempPoints.insert(*p);
+        }
+    }
+
+    // Allocate beforehand
+    std::vector<Vec3d> points(tempPoints.size());
+
+    unsigned int numPoints = 0;
+
+    // Fine iteration
+    for (auto p = tempPoints.begin(); p != tempPoints.end(); ++p)
+    {
+        Vec3d point = p->point;
+        if (point.x() >= min_x && point.x() <= max_x &&
+            point.y() >= min_y && point.y() <= max_y &&
+            point.z() >= min_z && point.z() <= max_z)
+        {
+            points[numPoints] = p->point;
+            numPoints++;
+        }
+    }
+
+    points.resize(numPoints);
+
+    return points;
+}
+
+void
+SpatialHashTableSeparateChaining::setLoadFactorMax(float loadFactorMax)
+{
+    m_loadFactorMax = loadFactorMax;
+    m_table->max_load_factor(m_loadFactorMax);
+    m_table->rehash(m_table->bucket_count());
+}
+
+void
+SpatialHashTableSeparateChaining::setCellSize(double x, double y, double z)
+{
+    m_cellSize[0] = x;
+    m_cellSize[1] = y;
+    m_cellSize[2] = z;
+
+    this->rehash();
+}
+
+void
+SpatialHashTableSeparateChaining::rehash()
+{
+    m_table->rehash(m_table->bucket_count());
+}
+
+}
+

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTableSeparateChaining.h

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   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.txt
+
+   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 imstkSpatialHashTableSeparateChaining_h
+#define imstkSpatialHashTableSeparateChaining_h
+
+#include <memory>
+#include <unordered_set>
+
+#include "imstkMath.h"
+#include "imstkSpatialHashTable.h"
+
+namespace imstk
+{
+
+struct PointEntry
+{
+    Vec3d point;
+    unsigned long ID;
+    double * cellSize;
+};
+
+}
+
+namespace std
+{
+
+template<> struct hash<imstk::PointEntry>
+{
+    size_t operator()(const imstk::PointEntry& point) const
+    {
+        unsigned int x = (unsigned int)(point.point.x() / point.cellSize[0]);
+        unsigned int y = (unsigned int)(point.point.y() / point.cellSize[1]);
+        unsigned int z = (unsigned int)(point.point.z() / point.cellSize[2]);
+
+        return (104729 * x + 104743 * y + 104759 * z);
+    }
+};
+
+template<> struct equal_to<imstk::PointEntry>
+{
+    size_t operator()(const imstk::PointEntry& point1, const imstk::PointEntry& point2) const
+    {
+        if (point1.point != point2.point)
+        {
+            return false;
+        }
+
+        if (point1.ID != point2.ID)
+        {
+            return false;
+        }
+
+        return true;
+   }
+};
+
+}
+
+namespace imstk
+{
+
+///
+/// \class SpatialHashTableSeparateChaining
+///
+/// \brief Implementation of SpatialHashTable using separate chaining
+///
+class SpatialHashTableSeparateChaining : public SpatialHashTable
+{
+public:
+    ///
+    /// \brief Default constructor
+    ///
+    SpatialHashTableSeparateChaining();
+
+    ///
+    /// \brief Insert an array of points
+    /// \param points An array of point
+    ///
+    void insertPoints(StdVectorOfVec3d points);
+
+    ///
+    /// \brief Insert an array of points
+    /// \param point A point
+    ///
+    void insertPoint(Vec3d point);
+
+    ///
+    /// \brief Sets the max load factor
+    /// \param loadFactorMax The new capacity after a rehash
+    ///
+    void setLoadFactorMax(float loadFactorMax);
+
+    ///
+    /// \brief Finds all points in an AABB
+    /// \param corner1 One corner to the box
+    /// \param corner2 The other corner to the box
+    ///
+    std::vector<Vec3d> getPointsInAABB(Vec3d corner1, Vec3d corner2);
+
+    ///
+    /// \brief Clears the table
+    ///
+    void clearTable();
+
+    ///
+    /// \brief Protected constructor
+    /// \param x,y,z Dimensions for each cell
+    ///
+    virtual void setCellSize(double x, double y, double z);
+
+protected:
+    ///
+    /// \brief Rehash the hash table
+    ///
+    virtual void rehash();
+
+    ///
+    /// \brief Hash function
+    /// \param point A point
+    ///
+    inline unsigned int generateHash(Vec3d point);
+
+    float m_loadFactorMax = 10.0f;
+
+    unsigned long m_currentID = 0;
+
+    std::shared_ptr<std::unordered_set<PointEntry>> m_table;
+};
+
+}
+
+#endif