Michael Ludwig avatar Michael Ludwig committed 032bdc3

Implement implicit quadtree over spatial hash. It still needs to be cleaned up, but the algorithm is there.

Comments (0)

Files changed (5)

ferox-math/src/main/java/com/ferox/math/Const.java

 
 @Documented
 @Retention(RetentionPolicy.RUNTIME)
-@Target({ ElementType.PARAMETER, ElementType.METHOD })
+@Target({ ElementType.PARAMETER, ElementType.METHOD, ElementType.FIELD })
 public @interface Const { }

ferox-math/src/main/java/com/ferox/math/bounds/Octree.java

  */
 @SuppressWarnings("unchecked")
 public class Octree<T> implements SpatialIndex<T> {
-    public static final int DEFAULT_MAX_DEPTH = 6;
+    public static final int DEFAULT_MAX_DEPTH = 8;
     
     private int queryNumber;
 
         return rootBounds.contains(dataBounds);
     }
     
+    public void clear() {
+        clear(root);
+    }
+    
+    private void clear(Node<T> node) {
+        if (node.items != null)
+            node.items.clear(true);
+        if (node.children != null) {
+            for (int i = 0; i < 8; i++) {
+                if (node.children[i] != null)
+                    clear(node.children[i]);
+            }
+        }
+    }
+    
     /*
      * Bit flags that describe where in the octree grid a
      * child node exists. If POS_a is true, then the child

ferox-math/src/main/java/com/ferox/math/bounds/QuadTree.java

+package com.ferox.math.bounds;
+
+import java.util.Arrays;
+
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
+import com.ferox.math.bounds.Frustum.FrustumIntersection;
+
+public class QuadTree<T> {//implements SpatialIndex<T> {
+    private static final int POS_X = 0x1;
+    private static final int POS_Y = 0x2;
+    
+    // complete quadtree nodes, keyed by hashed node ids packed into bits
+    // - values are the number of children in each node
+    private final int[] quadtree;
+    private final int depth;
+    
+    // grid of leaf nodes
+    private final Cell[] spatialHash;
+    private final int maxCellDimension;
+    
+    private final double widthScaleFactor;
+    private final double heightScaleFactor;
+    
+    private final double widthOffset;
+    private final double heightOffset;
+    
+    private final AxisAlignedBox rootBounds;
+    
+    // items in the quadtree
+    private Object[] elements;
+    private int[] queryIds;
+    private double[] aabbs;
+    private int size;
+    
+    private int queryIdCounter;
+    
+    public QuadTree() {
+        this(100, 40);
+    }
+    
+    public QuadTree(double sideLength, double height) {
+        this(sideLength, height, 8);
+    }
+    
+    public QuadTree(double sideLength, double height, int depth) {
+        this(new AxisAlignedBox(new Vector3(-sideLength / 2.0, -height / 2.0, -sideLength / 2.0),
+                                new Vector3(sideLength / 2.0, height / 2.0, sideLength / 2.0)), depth);
+    }
+    
+//    public QuadTree(@Const AxisAlignedBox aabb, double objSize) {
+//        
+//    }
+    
+    public QuadTree(@Const AxisAlignedBox aabb, int depth) {
+        this.depth = depth;
+        rootBounds = aabb.clone();
+        
+        maxCellDimension = 1 << (depth - 1);
+        spatialHash = new Cell[maxCellDimension * maxCellDimension];
+        elements = new Object[8];
+        queryIds = new int[8];
+        aabbs = new double[48];
+        size = 0;
+        queryIdCounter = 0;
+        
+        widthScaleFactor = maxCellDimension / (getFirstDimension(aabb.max) - getFirstDimension(aabb.min));
+        heightScaleFactor = maxCellDimension / (getSecondDimension(aabb.max) - getSecondDimension(aabb.min));
+        
+        widthOffset =  -getFirstDimension(aabb.min);
+        heightOffset = -getSecondDimension(aabb.min);
+        
+        int numNodes = getLevelOffset(depth);
+        quadtree = new int[numNodes];
+        
+        // mark quadtree leaves with negative indices, so that indices
+        // can be computed lazily later
+        int leafOffset = getLevelOffset(depth - 1);
+        Arrays.fill(quadtree, leafOffset, quadtree.length, -1);
+    }
+    
+    public boolean add(T element, @Const AxisAlignedBox bounds) {
+        if (!rootBounds.contains(bounds))
+            return false; // skip the element
+        
+        // add the item to the item list
+        int itemIndex = size;
+        if (itemIndex == elements.length) {
+            // grow items
+            int newSize = (int) (itemIndex * 1.5);
+            elements = Arrays.copyOf(elements, newSize);
+            queryIds = Arrays.copyOf(queryIds, newSize);
+            aabbs = Arrays.copyOf(aabbs, newSize * 6);
+        }
+        elements[itemIndex] = element;
+        queryIds[itemIndex] = 0;
+        
+        bounds.min.get(aabbs, itemIndex * 6);
+        bounds.max.get(aabbs, itemIndex * 6 + 3);
+        
+        size++;
+        
+        // we know these hashes will be within the valid cells, because the
+        // object is fully contained in the root bounds
+        int minX = hashCellX(bounds.min);
+        int minY = hashCellY(bounds.min);
+        int maxX = hashCellX(bounds.max);
+        int maxY = hashCellY(bounds.max);
+        
+        int hash;
+        Cell cell;
+        for (int y = minY; y <= maxY; y++) {
+            for (int x = minX; x <= maxX; x++) {
+                hash = hash(x, y);
+                cell = spatialHash[hash];
+                if (cell == null) {
+                    int qtIndex = getQuadLeafFromCell(x, y);
+                    cell = new Cell(this, qtIndex);
+                    spatialHash[hash] = cell;
+                    
+                    // also update the quad tree to point to this cell
+                    qtIndex += getLevelOffset(depth - 1);
+                    if (quadtree[qtIndex] != -1)
+                        throw new IllegalArgumentException("Quadtree leaf should not have any index");
+                    quadtree[qtIndex] = hash;
+                }
+                cell.add(this, itemIndex, bounds);
+            }
+        }
+        return true;
+    }
+    
+    public void clear() {
+        // fill quadtree counts with 0s, but only up to the leaf nodes because
+        // they hold cell indices, which don't change
+        int leafStartIndex = getLevelOffset(depth - 1);
+        Arrays.fill(quadtree, 0, leafStartIndex, 0);
+        
+        // clear spatial hash
+        int clearCount = 0;
+        int totalCount = 0;
+        Cell c;
+        int leafOffset = getLevelOffset(depth - 1);
+        for (int i = 0; i < spatialHash.length; i++) {
+            c = spatialHash[i];
+            if (c != null) {
+                totalCount++;
+                c.lifetime++;
+                
+                // check lifetime to help with GC'ing
+                if (c.lifetime > Cell.MAX_LIFETIME && c.size == 0) {
+                    // clear cell so that its contents get GC'ed
+                    spatialHash[i] = null;
+                    quadtree[leafOffset + c.quadTreeIndex] = -1;
+                    clearCount++;
+                }
+                
+                // only need to reset the size variable
+                c.size = 0;
+            }
+        }
+        
+        // empty global item bag
+        size = 0;
+        queryIdCounter = 0;
+        
+        System.out.println("cleared " + clearCount + " of " + totalCount + " of possible " + spatialHash.length);
+    }
+    
+    @SuppressWarnings("unchecked")
+    public void query(@Const AxisAlignedBox bounds, QueryCallback<T> callback) {
+        // hash x/z of bounds and do spatial hash query over intersecting cells
+        int minX = Math.max(0, hashCellX(bounds.min));
+        int minY = Math.max(0, hashCellY(bounds.min));
+        int maxX = Math.min(maxCellDimension - 1, hashCellX(bounds.max));
+        int maxY = Math.min(maxCellDimension - 1, hashCellY(bounds.max));
+        
+        int query = ++queryIdCounter;
+        AxisAlignedBox itemBounds = new AxisAlignedBox();
+        
+        Cell cell;
+        int item;
+        for (int y = minY; y <= maxY; y++) {
+            for (int x = minX; x <= maxX; x++) {
+                cell = spatialHash[hash(x, y)];
+                if (cell != null) {
+                    for (int i = 0; i < cell.size; i++) {
+                        item = cell.keys[i];
+                        
+                        // check query id, since the item could have crossed cell bounds
+                        // - this is valid in a single threaded situation
+                        if (queryIds[item] != query) {
+                            updateBounds(itemBounds, item);
+                            if (bounds.intersects(itemBounds)) {
+                                // we have an intersection, invoke the callback
+                                callback.process((T) elements[cell.keys[i]], itemBounds);
+                            }
+                            
+                            // record we've visited this item so other cells don't
+                            // attempt intersection checks
+                            queryIds[item] = query;
+                        }
+                    }
+                }
+            }
+        }
+    }
+    
+    public void query(Frustum f, QueryCallback<T> callback) {
+        // start at root quadtree and walk the tree to compute intersections,
+        // building in place an aabb for testing.
+        AxisAlignedBox root = new AxisAlignedBox(rootBounds);
+        AxisAlignedBox item = new AxisAlignedBox();
+        
+        query(f, callback, ++queryIdCounter, 0, 0, root, item, new PlaneState(), false);
+    }
+    
+    // FIXME: these arguments to be ordered better
+    @SuppressWarnings("unchecked")
+    private void query(Frustum f, QueryCallback<T> callback, int query, int level, int index, AxisAlignedBox nodeBounds, 
+                       AxisAlignedBox itemBounds, PlaneState planeState, boolean insideGuaranteed) {
+        // we assume that this node has items and nodeBounds has been updated to
+        // equal this node. we still have to check if the node intersects the frustum
+        if (!insideGuaranteed) {
+            FrustumIntersection test = nodeBounds.intersects(f, planeState);
+            if (test == FrustumIntersection.OUTSIDE) {
+                // node and it's children do not intersect, escape now
+                return;
+            } else if (test == FrustumIntersection.INSIDE) {
+                // all children nodes and items are guaranteed inside as well
+                insideGuaranteed = true;
+            }
+        }
+        
+        // at this point the node is within the frustum, so we have to visit
+        // its children if they have items underneath them
+        
+        // save planestate
+        int state = planeState.get();
+
+        if (level == depth - 1) {
+            // we are at a leaf node, to visit children, we process items in the 
+            // linked cell, we assume that the cell index is non-negative since
+            // we had to have passed a >0 check to get here
+            int item;
+            Cell cell = spatialHash[quadtree[getLevelOffset(level) + index]];
+            // process items
+            for (int i = 0; i < cell.size; i++) {
+                item = cell.keys[i];
+
+                // check query id, since the item could have crossed cell bounds
+                // - this is valid in a single threaded situation
+                if (queryIds[item] != query) {
+                    updateBounds(itemBounds, item);
+                    if (insideGuaranteed || itemBounds.intersects(f, planeState) != FrustumIntersection.OUTSIDE) {
+                        // we have an intersection, invoke the callback
+                        callback.process((T) elements[cell.keys[i]], itemBounds);
+                    }
+
+                    // record we've visited this item so other cells don't
+                    // attempt intersection checks
+                    queryIds[item] = query;
+
+                    // restore planestate for next item
+                    planeState.set(state);
+                }
+            }
+        } else {
+            int childOffset = getLevelOffset(level + 1);
+            int childIndex;
+            // visit children and check counts directly
+            for (int i = 0; i < 4; i++) {
+                childIndex = getChildIndex(index, i);
+                if (quadtree[childOffset + childIndex] > 0) {
+                    // visit child
+                    toChildBounds(i, nodeBounds);
+                    query(f, callback, query, level + 1, childIndex, nodeBounds, itemBounds, planeState, insideGuaranteed);
+                    restoreParentBounds(i, nodeBounds);
+
+                    // restore planestate for this node
+                    planeState.set(state);
+                }
+            }
+        }
+    }
+    
+    private static boolean inPositiveX(int index) {
+        return (index & POS_X) != 0;
+    }
+    private static boolean inPositiveY(int index) {
+        return (index & POS_Y) != 0;
+    }
+    
+    private void toChildBounds(int child, AxisAlignedBox bounds) {
+        if (inPositiveX(child)) {
+            // new min x is the center of the node
+            setFirstDimension(bounds.min, (getFirstDimension(bounds.min) + getFirstDimension(bounds.max)) / 2.0);
+        } else {
+            // new max x is the center of the node
+            setFirstDimension(bounds.max, (getFirstDimension(bounds.min) + getFirstDimension(bounds.max)) / 2.0);
+        }
+        
+        if (inPositiveY(child)) {
+            // new min y is the center of the node
+            setSecondDimension(bounds.min, (getSecondDimension(bounds.min) + getSecondDimension(bounds.max)) / 2.0);
+        } else {
+            // new max y is the center of the node
+            setSecondDimension(bounds.max, (getSecondDimension(bounds.min) + getSecondDimension(bounds.max)) / 2.0);
+        }
+    }
+    
+    private void restoreParentBounds(int child, AxisAlignedBox bounds) {
+        if (inPositiveX(child)) {
+            // new min x = min x - distance from min to max = 2 * min - max
+            setFirstDimension(bounds.min, 2 * getFirstDimension(bounds.min) - getFirstDimension(bounds.max));
+        } else {
+            // new max x = max x + distance from min to max = 2 * max - min
+            setFirstDimension(bounds.max, 2 * getFirstDimension(bounds.max) - getFirstDimension(bounds.min));
+        }
+        
+        if (inPositiveY(child)) {
+            // new min y = min y - distance from min to max = 2 * min - max
+            setSecondDimension(bounds.min, 2 * getSecondDimension(bounds.min) - getSecondDimension(bounds.max));
+        } else {
+            // new max y = max y + distance from min to max = 2 * max - min
+            setSecondDimension(bounds.max, 2 * getSecondDimension(bounds.max) - getSecondDimension(bounds.min));
+        }
+    }
+    
+    protected void setFirstDimension(Vector3 v, double d) {
+        v.x = d;
+    }
+    
+    protected void setSecondDimension(Vector3 v, double d) {
+        v.z = d;
+    }
+    
+    protected double getFirstDimension(@Const Vector3 v) {
+        return v.x;
+    }
+    
+    protected double getSecondDimension(@Const Vector3 v) {
+        return v.z;
+    }
+    
+    protected int hashCellX(@Const Vector3 v) {
+        // we add widthOffset to the coordinate value to get values into a positive-only range
+        return (int) ((getFirstDimension(v) + widthOffset) * widthScaleFactor);
+    }
+    
+    protected int hashCellY(@Const Vector3 v) {
+        return (int) ((getSecondDimension(v) + heightOffset) * heightScaleFactor);
+    }
+    
+    protected int hash(int cellX, int cellY) {
+        return cellX + maxCellDimension * cellY;
+    }
+    
+    public void updateBounds(AxisAlignedBox bounds, int index) {
+        int realIndex = index * 6;
+        bounds.min.set(aabbs, realIndex);
+        bounds.max.set(aabbs, realIndex + 3);
+    }
+    
+    private int getQuadLeafFromCell(int cellX, int cellY) {
+        // compute the center point of the cell, to use in a tree search,
+        // must also subtract away offsets to get into the root bounds space
+        double px = (cellX + 0.5) / widthScaleFactor - widthOffset;
+        double py = (cellY + 0.5) / heightScaleFactor - heightOffset;
+        
+        double minx = getFirstDimension(rootBounds.min);
+        double miny = getSecondDimension(rootBounds.min);
+        double maxx = getFirstDimension(rootBounds.max);
+        double maxy = getSecondDimension(rootBounds.max);
+        
+        // the center of the node
+        double cx = (minx + maxx) * 0.5;
+        double cy = (miny + maxy) * 0.5;
+        
+        int child;
+        int index = 0;
+        for (int i = 0; i < depth - 1; i++) {
+            child = 0;
+            // if px > cx then the cell is in the right/positive x half of this node
+            if (px >= cx) {
+                child |= POS_X;
+                // next node's min x is the current center x
+                minx = cx;
+            } else {
+                // next node's max x is the current center x
+                maxx = cx;
+            }
+            
+            if (py >= cy) {
+                child |= POS_Y;
+                // next node's min y is the current center y
+                miny = cy;
+            } else {
+                maxy = cy;
+            }
+            
+            // compute new center for next node
+            cx = (minx + maxx) * 0.5;
+            cy = (miny + maxy) * 0.5;
+
+            index = getChildIndex(index, child);
+        }
+        
+        return index;
+    }
+    
+    private int getLevelOffset(int level) {
+        // compute the index offset into the quadtree array for the given level
+        if (level == 0) {
+            // level = 0 is the root, which is the first element in the array
+            return 0;
+        } else {
+            // finite sum of the geometric series: 1 + 4 + 16 + ...
+            //  1. S = 4^0 + 4^1 + 4^2 + ... + 4^level-1
+            //  2. 4S = 4^1 + ... + 4^(level)
+            //  3. 4S = (S - 1) + 4^(level)
+            //  4. 3S = 4^(level) - 1
+            //  5. S = (4^(level) - 1) / 3
+            //  6. S = (2^(2level) - 1) / 3
+            return ((1 << (level << 1)) - 1) / 3;
+        }
+    }
+    
+    private int getChildIndex(int parentIndex, int child) {
+        // shift parent index left 2 bits, and OR in the child, this assumes:
+        // - parentIndex does not have its level offset added to it
+        // - child is between 0 and 3 (i.e. 2 bits required)
+        return (parentIndex << 2) | child;
+    }
+    
+    private int getParentIndex(int childIndex) {
+        // shift child index right 2 bits, this assumes:
+        // - child does not have its level offset added to it
+        return (childIndex >> 2);
+    }
+    
+    private static class Cell {
+        private static final int INCREMENT = 4;
+        private static final int MAX_LIFETIME = 15;
+        
+        private int[] keys;
+//        private double[] aabbs;
+        
+        private int size;
+        
+        private int lifetime;
+        
+        // this is the parent index of the quadtree index that actually holds
+        // this cell, because the leaves don't store count information
+        private final int quadTreeIndex;
+        
+        private Cell(QuadTree<?> tree, int quadLeaf) {
+            quadTreeIndex = quadLeaf; //tree.getParentIndex(quadLeaf);
+//            aabbs = new double[INCREMENT * 6];
+            keys = new int[INCREMENT];
+            size = 0;
+            lifetime = 0;
+        }
+        
+        public void add(QuadTree<?> tree, int item, @Const AxisAlignedBox bounds) {
+            if (size == keys.length - 1) {
+                // increase size
+                keys = Arrays.copyOf(keys, keys.length + INCREMENT);
+//                aabbs = Arrays.copyOf(aabbs, aabbs.length + INCREMENT * 6);
+            }
+            keys[size] = item;
+//            bounds.min.get(aabbs, size * 6);
+//            bounds.max.get(aabbs, size * 6 + 3);
+            size++;
+            
+            // update quadtree counts by 1
+            updateTree(tree, 1);
+        }
+        
+        /*public void updateBounds(AxisAlignedBox bounds, int index) {
+            int realIndex = index * 6;
+            bounds.min.set(aabbs, realIndex);
+            bounds.max.set(aabbs, realIndex + 3);
+        }*/
+        
+        private void updateTree(QuadTree<?> tree, int val) {
+            int index = quadTreeIndex;
+            // it's depth-2 because depth-1 is the leaf level, but we skip that one
+            for (int l = tree.depth - 2; l >= 0; l--) {
+                index = tree.getParentIndex(index);
+                tree.quadtree[tree.getLevelOffset(l) + index] += val;
+            }
+        }
+        
+        public void remove(QuadTree<?> tree, int item) {
+            for (int i = 0; i < size; i++) {
+                // search for the item to remove
+                if (keys[i] == item) {
+                    keys[i] = keys[size - 1];
+//                    System.arraycopy(aabbs, (size - 1) * 6, aabbs, i * 6, 6);
+                    size--;
+                    
+                    // decrement quadtree counts by 1
+                    updateTree(tree, -1);
+                    break;
+                }
+            }
+        }
+    }
+}

ferox-math/src/main/java/com/ferox/math/bounds/SpatialIndex.java

      * @param callback A QueryCallback to run on each item within the query
      * @throws NullPointerException if frustum or callback is null
      */
+    // FIXME: should Frustum be annotated with @Const? Where do I draw the line?
+    // part of me says no, since it's a more complex object that doesn't follow
+    // the rest of the pattern, but it's also a math object that could be mutated.
     public void query(Frustum f, QueryCallback<T> callback);
 }

ferox-math/src/test/java/com/ferox/math/MathTest.java

-package com.ferox.math;
-
-
-public class MathTest {
-    public static void main(String[] args) {
-        int numMatrices = 10000;
-        int numTests = 10000;
-        
-        int v = 0;
-        v += Long.MAX_VALUE;
-        
-        double[] dataDA = getData(numMatrices);
-        double[] dataDB = getData(numMatrices);
-        
-        Matrix3[] a2 = getMatrix2(dataDA);
-        Matrix3[] b2 = getMatrix2(dataDB);
-        
-        double d2 = test2(a2, b2);
-        double d4 = test4(dataDA, dataDB);
-        
-        System.out.println("Test results:");
-        System.out.println("\ttest2 = " + d2);
-        System.out.println("\ttest4 = " + d4);
-        System.out.println();
-        
-        for (int i = 0; i < 3; i++) {
-            System.out.println("Test " + (i + 1));
-            {
-                long now = System.currentTimeMillis();
-                for (int j = 0; j < numTests; j++) {
-                    test2(a2, b2);
-                }
-                System.out.println("Matrix3: " + (System.currentTimeMillis() - now));
-            }
-            {
-                long now = System.currentTimeMillis();
-                for (int j = 0; j < numTests; j++) {
-                    test4(dataDA, dataDB);
-                }
-                System.out.println("float[] Matrix3: " + (System.currentTimeMillis() - now));
-            }
-            System.out.println();
-        }
-    }
-    
-    public static double r() {
-        return Math.random() * 10;
-    }
-    
-    public static double[] getData(int num) {
-        double[] m = new double[9 * num];
-        for (int i = 0; i < m.length; i++)
-            m[i] = r();
-        return m;
-    }
-    
-    public static float[] cast(double[] d) {
-        float[] m = new float[d.length];
-        for (int i = 0; i < d.length; i++)
-            m[i] = (float) d[i];
-        return m;
-    }
-    
-    public static Matrix3[] getMatrix2(double[] data) {
-        Matrix3[] m = new Matrix3[data.length / 9];
-        for (int i = 0; i < m.length; i ++) {
-            m[i] = new Matrix3().set(data, i * 9, false);
-        }
-        
-        return m;
-    }
-    
-    public static double test2(Matrix3[] as, Matrix3[] bs) {
-        Matrix3 m = new Matrix3();
-        double d = 0;
-        for (int i = 0; i < as.length; i++) {
-            d += m.mul(as[i], bs[i]).inverse().determinant();
-        }
-        return d;
-    }
-    
-    public static double test4(double[] dataA, double[] dataB) {
-        Matrix3 m1 = new Matrix3();
-        Matrix3 m2 = new Matrix3();
-        double d = 0;
-        for (int i = 0; i < dataA.length; i += 9) {
-            m1.set(dataA, i, false);
-            m2.set(dataB, i, false);
-            
-            d += m1.mul(m2).inverse().determinant();
-        }
-        return d;
-    }
-}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.