Commits

Daniel K. O. committed 1448204

finished hash space implementation

  • Participants
  • Parent commits 830b683

Comments (0)

Files changed (12)

 .libs
 aclocal.m4
 autom4te.cache
+compile
 config.guess
 config.h
 config.h.in

include/kode/collision/HashSpace.hpp

    limitations under the License.
 */
 
+#include <utility>
+
 #include <kode/collision/Space.hpp>
 
 namespace kode {
         int maxLevel = 20;
     public:
 
+        void setLimits(int min, int max);
+        std::pair<int,int> getLimits() const noexcept;
+
         void findPairs(const std::function<void(Geom&, Geom&)>& callback) override;
 
     };

include/kode/collision/Space.hpp

         void rememberGeom(Geom* g);
         void forgetGeom(Geom* g) noexcept;
 
+
+        bool checkCollision(const Geom& a, const Geom& b) const noexcept;
+
     public:
 
         using iterator = std::vector<Geom*>::iterator;

include/kode/joints/Contact.hpp

         Real minBounceVel = 0;
         Real softERP;
         Real softCFM;
-        Real slip1;
-        Real slip2;
+        Real slip1 = 0;
+        Real slip2 = 0;
 
         bool useSoftERP     = false;
         bool useSoftCFM     = false;

include/kode/kode.hpp

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

samples/KODEOgre/chain.cpp

 
 struct Demo : public App {
 
-    SimpleSpace space;
+    HashSpace space;
     Collider collider;
 
     vector<BodyBox> chain;

samples/KODEOgre/free-spheres.cpp

 struct Demo : public App {
 
     SimpleSpace space;
+    //HashSpace space;
     Collider collider;
 
     ogre::Plane ground1 = { 0.1, 1, 0.1, 0};
     void
     step() override
     {
+        if (keyboard->isKeyDown(OIS::KC_LSHIFT) && keyboard->isKeyDown(OIS::KC_SPACE))
+            throwBall();
+
+
         // check for collisions
         vector<Contact> contacts;
         vector<ContactPoint> points;

src/collision/HashSpace.cpp

 */
 
 #include <unordered_map>
+#include <unordered_set>
 #include <cstdint>
 #include <algorithm>
 #include <cmath>
 #include <kode/AxisAlignedBox.hpp>
 #include <kode/collision/Geom.hpp>
 
+
+using std::vector;
+using std::pair;
+using std::make_pair;
+using std::unordered_multimap;
+using std::unordered_set;
+
+
 namespace kode {
 
     namespace {
 
 
         struct Hasher {
+            inline
             size_t
             operator()(const Key& key) const noexcept
             {
                     p3 * h(key.y) ^
                     p4 * h(key.z);
             }
+
+            inline
+            size_t
+            operator()(const pair<Geom*,Geom*>& p) const noexcept
+            {
+                std::hash<Geom*> h;
+                return h(p.first) ^ h(p.second);
+            }
         };
 
-
-        struct AABBidx {
+        // quantized aabb
+        struct QAABB {
             int32_t minX, maxX, minY, maxY, minZ, maxZ;
         };
 
 
-        struct Value {
+        struct GeomData {
+            QAABB aabb;
             Geom* geom;
-            AABBidx idx;
+            int level;
         };
 
     
             std::frexp (m, &level);
             return level;
         }
+
+        inline
+        QAABB
+        quantize(const AxisAlignedBox& b, Real cellSize)
+        {
+            return {static_cast<int>(Math::floor(b.getMin().x/cellSize)),
+                    static_cast<int>(Math::floor(b.getMax().x/cellSize)),
+                    static_cast<int>(Math::floor(b.getMin().y/cellSize)),
+                    static_cast<int>(Math::floor(b.getMax().y/cellSize)),
+                    static_cast<int>(Math::floor(b.getMin().z/cellSize)),
+                    static_cast<int>(Math::floor(b.getMax().z/cellSize))};
+        }
     }
 
 
         if (geoms.size() < 2)
             return;
 
-        std::vector<Geom*> big_geoms;
-        std::unordered_multimap<Key, Value, Hasher> table;
+        vector<Geom*> bigGeoms;
+        unordered_multimap<Key, Geom*, Hasher> table;
+        vector<GeomData> gdata;
+        gdata.reserve(geoms.size()); // prevent reallocations
+
         for (Geom* g : geoms) {
             if (!g->isEnabled())
                 continue;
             if (level > maxLevel) {
                 bigGeoms.push_back(g);
             } else {
-                
+                // TODO: if the AABB is too small but too far away, the
+                // quantized AABB will overflow
+                const Real cellSize = std::ldexp(1, level);
+                const QAABB aabb = quantize(g->getAABB(), cellSize);
+
+                gdata.push_back(GeomData{aabb, g, level});
+
+                for (int x=aabb.minX; x<=aabb.maxX; ++x)
+                    for (int y=aabb.minY; y<=aabb.maxY; ++y)
+                        for (int z=aabb.minZ; z<=aabb.maxZ; ++z) {
+                            Key k = {level, x, y, z};
+                            table.insert(make_pair(k, g));
+                        }
             }
         }
+
+        // now scan the geoms looking for collisions in the table
+        // keep already checked pairs in checked
+        unordered_set<pair<Geom*,Geom*>, Hasher> checked(gdata.size());
+
+        for (GeomData& gd : gdata) {
+            Geom* g1 = gd.geom;
+
+            QAABB aabb = gd.aabb;
+            // walk up in the hierarchy
+            for (int level = gd.level; level <= maxLevel; ++level) {
+
+                for (int x=aabb.minX; x<=aabb.maxX; ++x)
+                    for (int y=aabb.minY; y<=aabb.maxY; ++y)
+                        for (int z=aabb.minZ; z<=aabb.maxZ; ++z) {
+
+                            const Key k = {level, x, y, z};
+                            auto b = table.bucket(k);
+                            for (auto i=table.begin(b); i!=table.end(b); ++i) {
+                                Geom* g2 = i->second;
+                                // check if testing against itself, or out-of-order
+                                if (g1 >= g2)
+                                    continue;
+                                pair<Geom*,Geom*> k = {g1, g2};
+                                if (checked.count(k))
+                                    continue;
+                                else
+                                    checked.insert(k);
+                                if (checkCollision(*g1, *g2))
+                                    callback(*g1, *g2);
+                            }
+                        }
+
+                // update aabb to the next level
+                aabb.minX >>= 1;
+                aabb.maxX >>= 1;
+                aabb.minY >>= 1;
+                aabb.maxY >>= 1;
+                aabb.minZ >>= 1;
+                aabb.maxZ >>= 1;
+            }
+        }
+
+        // check all geoms against all the big geoms
+        for (GeomData& gd : gdata) {
+            Geom* a = gd.geom;
+            for (Geom* b : bigGeoms)
+                if (checkCollision(*a, *b))
+                    callback(*a, *b);
+        }
+
+        // quadratic check on all big geoms
+        for (size_t i=0; i<bigGeoms.size(); ++i)
+            for (size_t j=i+1; j<bigGeoms.size(); ++j)
+                if (checkCollision(*bigGeoms[i], *bigGeoms[j]))
+                    callback(*bigGeoms[i], *bigGeoms[j]);
+
     }
 
 
+    void
+    HashSpace::setLimits(int min, int max)
+    {
+        if (min > max)
+            throw std::invalid_argument{"max level must not be smaller than min level"};
+        minLevel = min;
+        maxLevel = max;
+    }
+
+
+    std::pair<int,int>
+    HashSpace::getLimits() const noexcept
+    {
+        return {minLevel, maxLevel};
+    }
 }

src/collision/Plane.cpp

     bool
     Plane::rejectAABB(const AxisAlignedBox& box) const noexcept
     {
+        if (box.isInfinite())
+            return false;
+
         for (unsigned i=0; i<8; ++i)
             if (distance(box.getCorner(i)) <= 0)
                 return false;

src/collision/SimpleSpace.cpp

     SimpleSpace::findPairs(const std::function<void(Geom&, Geom&)>& callback)
     {
         for (size_t i=0; i<geoms.size(); ++i) {
-
-            const AxisAlignedBox& abox = geoms[i]->getAABB();
-
             for (size_t j=i+1; j<geoms.size(); ++j) {
-
-                const AxisAlignedBox& bbox = geoms[j]->getAABB();
-
-                // TODO: ask both geoms to do AABB rejection
-                if ( abox.intersects(bbox) )
+                if (checkCollision(*geoms[i], *geoms[j]))
                     callback(*geoms[i], *geoms[j]);
-
             }
-
         }
     }
 

src/collision/Space.cpp

         forgetGeom(&g);
         g.forgetSpace();
     }
+
+
+    bool
+    Space::checkCollision(const Geom& a, const Geom& b) const noexcept
+    {
+        // convenience: avoid contacts between geoms from the same body
+        if (a.getBody() && a.getBody() == b.getBody())
+            return false;
+
+        // check filter/category
+        if (!(a.getCategory() & b.getFilter())
+            &&
+            !(b.getCategory() & a.getFilter()))
+            return false;
+
+        // check if AABBs are disjoints
+        if (!a.getAABB().intersects(b.getAABB()))
+            return false;
+
+        // check if geom can reject the AABB
+        if (a.rejectAABB(b.getAABB()) || b.rejectAABB(a.getAABB()))
+            return false;
+
+        return true;
+    }
 }

src/solvers/QuickStepCore.hpp

 
                         // compute lambda and clamp it to [lo,hi].
                         // also clamp delta
+
                         const Real newLambda = oldLambda + delta;
 
                         if (newLambda < lo) {