Commits

Daniel K. O.  committed 646b8c8

finished quadtree implementation

  • Participants
  • Parent commits c769050

Comments (0)

Files changed (9)

File include/kode/Any.hpp

 
 
         template<class T>
-        class Holder : public Base {
+        struct Holder : public Base {
         public:
             Holder(const T& val) :
                 value{val}
 
 
             Holder& operator=(const Holder&) = delete;
-        private:
+
             T value;
-
+            
         };
 
         Base* content;
     T*
     any_cast(Any* ptr) noexcept
     {
-        return ptr && typeid(T)==ptr->type() ? ptr->content : nullptr;
+        return ptr && typeid(T)==ptr->type()
+            ? &static_cast<Any::Holder<T>*>(ptr->content)->value
+            : nullptr;
     }
 
 
     const T*
     any_cast(const Any* ptr) noexcept
     {
-        return ptr && typeid(T)==ptr->type() ? ptr->content : nullptr;
+        return ptr && typeid(T)==ptr->type()
+            ? &static_cast<const Any::Holder<T>*>(ptr->content)->value
+            : nullptr;
     }
 
 
     template<class T>
-    T any_cast(const Any& thing)
+    T
+    any_cast(const Any& thing)
     {
         using cnonref = typename std::add_const<typename std::remove_reference<T>::type>::type;
         cnonref * p = any_cast<cnonref>(&thing);
 
 
     template<class T>
-    T any_cast(Any& thing)
+    T
+    any_cast(Any& thing)
     {
         using nonref = typename std::remove_reference<T>::type;
         nonref * p = any_cast<nonref>(&thing);
 
 
     template<class T>
-    T any_cast(Any&& thing)
+    T
+    any_cast(Any&& thing)
     {
         using nonref = typename std::remove_reference<T>::type;
         nonref * p = any_cast<nonref>(&thing);

File include/kode/collision/Geom.hpp

 
         friend class Body;
         friend class Space;
+        friend class QuadTreeSpace;
+        friend class QuadCell;
 
     protected:
 

File include/kode/collision/QuadTreeSpace.hpp

 
 namespace kode {
 
+    class QuadCell;
+
     class QuadTreeSpace : public Space {
     public:
         enum class Split {
         };
 
         QuadTreeSpace(Split m);
+        ~QuadTreeSpace();
 
         void findPairs(const std::function<void(Geom&, Geom&)>& callback) override;
 
     private:
 
-        struct Cell {
-            Vector2 min;
-            Vector2 max;
-            std::vector<Geom*> geoms;
-            std::array<std::unique_ptr<Cell>, 4> children;
-
-            Cell(const Vector2& min, const Vector2& max);
-
-            Vector2 getCenter() const noexcept;
-        };
 
 
         Split mode = Split::XY;
+        Vector2 center = {0,0};
+        Vector2 hsize = {10000, 10000};
+        unsigned maxDepth = 8;
 
-        Vector2 min = {-1000, -1000};
-        Vector2 max = {1000, 1000};
-        std::unique_ptr<Cell> root;
+        QuadCell* root = nullptr;
+
 
         void notifyChange(Geom* g) override;
 
 
         void forgetGeom(Geom* g) noexcept override;
 
+        using Space::checkCollision;
+        friend class QuadCell;
+
     };
 
 }

File include/kode/collision/Space.hpp

 
         static bool checkCollision(const Geom& a, const Geom& b) noexcept;
 
-        const Any& getGeomData(const Geom* g)const noexcept;
-        Any& getGeomData(Geom* g) noexcept;
-
     public:
 
         using iterator = std::vector<Geom*>::iterator;

File include/kode/kode.hpp

 
 #include <kode/collision/Space.hpp>
 #include <kode/collision/HashSpace.hpp>
+#include <kode/collision/QuadTreeSpace.hpp>
 #include <kode/collision/SimpleSpace.hpp>
 
 

File samples/KODEOgre/free-spheres.cpp

 
 struct Demo : public App {
 
-    SimpleSpace space;
+    //SimpleSpace space;
     //HashSpace space;
+    QuadTreeSpace space = QuadTreeSpace::Split::XZ;
     Collider collider;
 
     ogre::Plane ground1 = { 0.1, 1, 0.1, 0};

File src/collision/QuadTreeSpace.cpp

    limitations under the License.
 */
 
+#include <utility>
+#include <tuple>
+#include <algorithm>
+
 #include <kode/collision/QuadTreeSpace.hpp>
 
 #include <kode/collision/Geom.hpp>
 
+using std::tie;
+using std::pair;
+
 namespace kode {
 
     constexpr Vector2 diag2 = {1,1};
 
     using Split = QuadTreeSpace::Split;
 
+    struct QuadCell {
+        Vector2 center;
+        Vector2 hsize;
+        std::vector<Geom*> geoms;
+        std::array<std::unique_ptr<QuadCell>, 4> children;
+        QuadCell* parent;
+        size_t total = 0;
+
+        QuadCell(const Vector2& center, const Vector2& hsize, QuadCell* p = nullptr);
+
+        void insert(Geom* g,
+                    const Vector2& gMin,
+                    const Vector2& gMax,
+                    unsigned maxDepth);
+
+        void take(Geom* g);
+        void giveUp(Geom* g);
+
+        Vector2 getMin() const noexcept;
+        Vector2 getMax() const noexcept;
+
+        bool contains(const Vector2& min, const Vector2& max) const noexcept;
+
+        void findPairs(const std::function<void(Geom&,Geom&)>& callback);
+        void findLocalPairs(Geom* g, const std::function<void(Geom&,Geom&)>& callback);
+    };
+
     namespace {
 
         constexpr
         filter(const Vector3& v, Split m) noexcept
         {
             return m==Split::XY ? v.xy()
-                : m==Split::XZ ? v.xz()
-                : v.yz();
+                 : m==Split::XZ ? v.xz()
+                 : v.yz();
+        }
+
+        inline
+        pair<Vector2,Vector2>
+        split(const Vector2& center, const Vector2& hsize, unsigned idx) noexcept
+        {
+            return {{center.x + (idx & 1 ? hsize.x/2 : -hsize.x/2),
+                     center.y + (idx & 2 ? hsize.y/2 : -hsize.y/2)},
+                    hsize/2};
         }
     }
 
 
-    QuadTreeSpace::Cell::Cell(const Vector2& mi, const Vector2& ma) :
-        min{mi},
-        max{ma}
+    QuadCell::QuadCell(const Vector2& cen,
+                       const Vector2& hsz,
+                       QuadCell* p) :
+        center{cen},
+        hsize{hsz},
+        parent{p}
     {}
 
 
     Vector2
-    QuadTreeSpace::Cell::getCenter() const noexcept
+    QuadCell::getMin() const noexcept
     {
-        return (min+max)/2;
+        return center - hsize;
     }
 
 
+    Vector2
+    QuadCell::getMax() const noexcept
+    {
+        return center + hsize;
+    }
+
+
+    bool
+    QuadCell::contains(const Vector2& gMin,
+                       const Vector2& gMax) const noexcept
+    {
+        return getMin().x <= gMin.x && gMax.x <= getMax().x
+            && getMin().y <= gMin.y && gMax.y <= getMax().y;
+    }
+
+
+    void
+    QuadCell::insert(Geom* g,
+                     const Vector2& gMin,
+                     const Vector2& gMax,
+                     unsigned maxDepth)
+    {
+        if (!maxDepth) // no choice but to insert here
+            return take(g);
+
+
+        // optimization: if center is inside the the interval
+        // don't try to insert in children
+        if ((gMin.x <= center.x && center.x <= gMax.x)
+            ||
+            (gMax.y <= center.y && center.y <= gMax.y))
+            return take(g);
+
+
+        for (unsigned i=0; i<children.size(); ++i) {
+
+            if (!children[i]) {
+                Vector2 cCenter, cHsize;
+                tie(cCenter, cHsize) = split(center, hsize, i);
+                children[i].reset(new QuadCell{cCenter, cHsize, this});
+            }
+
+            if (children[i]->contains(gMin, gMax))
+                return children[i]->insert(g, gMin, gMax, maxDepth-1);
+
+        }
+
+
+        // no child took ownership, so we keep it
+        take(g);
+    }
+
+
+    void
+    QuadCell::take(Geom* g)
+    {
+        geoms.push_back(g);
+        try {
+            g->spaceData = this;
+        }
+        catch (...) {
+            geoms.pop_back();
+            throw;
+        }
+
+        ++total;
+        // go up telling ancestors they got a geom
+        for (QuadCell* a=parent; a; a=a->parent)
+            ++a->total;
+    }
+
+
+    void
+    QuadCell::giveUp(Geom* g)
+    {
+        auto i = std::remove(geoms.begin(), geoms.end(), g);
+
+        assert((i!=geoms.end()) && "BUG: removing geom from a quadtree cell that doesn't contain it");
+
+        g->spaceData.clear();
+
+        geoms.erase(i);
+
+        // go up telling ancestors they lost a geom
+        for (QuadCell* a=parent; a; a=a->parent)
+            --a->total;
+    }
+
+
+    void
+    QuadCell::findPairs(const std::function<void(Geom&,Geom&)>& callback)
+    {
+        for (size_t i=0; i<geoms.size(); ++i) {
+            if (!geoms[i]->isEnabled())
+                continue;
+
+            for (size_t j=i+1; j<geoms.size(); ++j) {
+                if (!geoms[j]->isEnabled())
+                    continue;
+
+                if (QuadTreeSpace::checkCollision(*geoms[i], *geoms[j]))
+                    callback(*geoms[i], *geoms[j]);
+            }
+        }
+
+        // check geoms against ancestors
+        for (Geom* g : geoms) {
+            if (!g->isEnabled())
+                continue;
+
+            for (QuadCell* a=parent; a; a=a->parent)
+                a->findLocalPairs(g, callback);
+        }
+
+        // recurse
+        for (auto& child : children)
+            if (child)
+                child->findPairs(callback);
+
+    }
+
+
+    void
+    QuadCell::findLocalPairs(Geom* h, const std::function<void(Geom&,Geom&)>& callback)
+    {
+        for (Geom* g : geoms) {
+            if (!g->isEnabled())
+                continue;
+            if (QuadTreeSpace::checkCollision(*h, *g))
+                callback(*h, *g);
+        }
+    }
+
+
+
 
 
     QuadTreeSpace::QuadTreeSpace(Split m) :
         mode{m},
-        root{new Cell{min, max}}
+        root{new QuadCell{center, hsize}}
+    {}
+
+
+    QuadTreeSpace::~QuadTreeSpace()
     {
+        delete root;
     }
 
 
     {
         Space::rememberGeom(g);
 
-        Vector2 gMin = filter(g->getAABB().getMin(), mode);
-        Vector2 gMax = filter(g->getAABB().getMax(), mode);
-        getGeomData(g) = this;
+        if (g->getAABB().isFinite())
+            root->insert(g, filter(g->getAABB().getMin(), mode),
+                         filter(g->getAABB().getMax(), mode),
+                         maxDepth);
+        else {
+            // force insertion on the root
+            root->take(g);
+        }
     }
 
 
     QuadTreeSpace::forgetGeom(Geom* g) noexcept
     {
         Space::forgetGeom(g);
-        
+
+        any_cast<QuadCell*>(g->spaceData)->giveUp(g);
     }
 
 
     void
     QuadTreeSpace::notifyChange(Geom* g)
     {
-        
+        any_cast<QuadCell*>(g->spaceData)->giveUp(g);
+
+        if (g->getAABB().isFinite())
+            root->insert(g, filter(g->getAABB().getMin(), mode),
+                         filter(g->getAABB().getMax(), mode),
+                         maxDepth);
+        else {
+            // force insertion on the root
+            root->take(g);
+        }
     }
 
 
     void
     QuadTreeSpace::findPairs(const std::function<void(Geom&, Geom&)>& callback)
     {
-        
+        root->findPairs(callback);
     }
 }

File src/collision/Space.cpp

     }
 
 
-    const Any&
-    Space::getGeomData(const Geom* g) const noexcept
-    {
-        return g->spaceData;
-    }
-
-
-    Any&
-    Space::getGeomData(Geom* g) noexcept
-    {
-        return g->spaceData;
-    }
 }

File tests/test_interval.cpp

 
 BOOST_AUTO_TEST_CASE( infinite_interval )
 {
-    Interval a{Interval::Infinite};
+    Interval a{Interval::Infinite()};
 
     BOOST_CHECK(!a.isEmpty());
     BOOST_CHECK(!a.isFinite());