Can now update AABBTree however something weird is happening where the tree grows for the same numbeer of objects...

lib/trees/aabb_node.rb

@@ -18,20 +18,24 @@ class IMICFPS
       def insert_subtree(leaf)
         if leaf?
           new_node = AABBNode.new(parent: nil, object: nil, bounding_box: @bounding_box.union(leaf.bounding_box))
+
           new_node.a = self
           new_node.b = leaf
 
           return new_node
         else
-          cost_a = @b.bounding_box.volume + @a.bounding_box.union(leaf.bounding_box).volume
           cost_a = @a.bounding_box.volume + @b.bounding_box.union(leaf.bounding_box).volume
+          cost_b = @b.bounding_box.volume + @a.bounding_box.union(leaf.bounding_box).volume
 
-          if cost_a < cost_b
-            self.a = @a.insert_subtree(leaf)
-          elsif cost_b < cost_a
+          if cost_a == cost_b
+            cost_a = @a.proximity(leaf)
+            cost_b = @b.proximity(leaf)
+          end
+
+          if cost_b < cost_a
             self.b = @b.insert_subtree(leaf)
           else
-            raise "FIXME"
+            self.a = @a.insert_subtree(leaf)
           end
 
           @bounding_box = @bounding_box.union(leaf.bounding_box)
@@ -54,6 +58,44 @@ class IMICFPS
       end
 
       def remove_subtree(leaf)
+        if leaf
+          return self
+        else
+          if leaf.parent == self
+            other_child = other(leaf)
+            other_child.parent = @parent
+            return other_child
+          else
+            leaf.disown_child(leaf)
+            return self
+          end
+        end
+      end
+
+      def other(leaf)
+        @a == leaf ? @b : @a
+      end
+
+      def disown_child(leaf)
+        value = other(leaf)
+        raise "Can not replace child of a leaf!" if @parent.leaf?
+        raise "Node is not a child of parent!" unless leaf.child_of?(@parent)
+
+        if @parent.a == self
+          @parent.a = value
+        else
+          @parent.b = value
+        end
+
+        @parent.update_bounding_box
+      end
+
+      def child_of?(leaf)
+        self == leaf.a || self == leaf.b
+      end
+
+      def proximity(leaf)
+        (@bounding_box - leaf.bounding_box).sum.abs
       end
 
       def update_bounding_box

lib/trees/aabb_tree.rb

@@ -1,8 +1,13 @@
 class IMICFPS
   class AABBTree
+    include IMICFPS::AABBTreeDebug
+
+    attr_reader :root, :objects, :branches, :leaves
     def initialize
       @objects = {}
       @root = nil
+      @branches = 0
+      @leaves   = 0
     end
 
     def insert(object, bounding_box)
@@ -14,26 +19,15 @@ class IMICFPS
       @objects[object] = leaf
 
       if @root
-        @root.insert_subtree(leaf)
+        @root = @root.insert_subtree(leaf)
       else
         @root = leaf
       end
     end
 
-    def update
-      needs_update = []
-
-      @objects.each do |object, node|
-        next unless object.is_a?(Entity)
-        unless object.normalized_bounding_box == node.bounding_box
-          needs_update << object
-        end
-      end
-
-      needs_update.each do |object|
-        remove(object)
-        insert(object, object.normalized_bounding_box)
-      end
+    def update(object, bounding_box)
+      remove(object)
+      insert(object, bounding_box)
     end
 
     # Returns a list of all collided objects inside Bounding Box
@@ -41,15 +35,15 @@ class IMICFPS
       items = []
       if @root
         items = @root.search_subtree(bounding_box)
+        items.map! {|e| e.object}
       end
 
-      items.map! {|e| e.object}
       return items
     end
 
     def remove(object)
-      leaf = @objects.delete(object)
-      @root.remove_subtree(leaf) if leaf
+      leaf  = @objects.delete(object)
+      @root = @root.remove_subtree(leaf) if leaf
     end
   end
 end

lib/trees/aabb_tree_debug.rb

@@ -0,0 +1,26 @@
+class IMICFPS
+  # Gets included into AABBTree
+  module AABBTreeDebug
+    def inspect
+      @branches, @leaves = 0, 0
+      if @root
+        node = @root
+
+        debug_search(node.a)
+        debug_search(node.b)
+      end
+
+      puts "<#{self.class}:#{self.object_id}> has #{@branches} branches and #{@leaves} leaves"
+    end
+
+    def debug_search(node)
+      if node.leaf?
+        @leaves += 1
+      else
+        @branches += 1
+        debug_search(node.a)
+        debug_search(node.b)
+      end
+    end
+  end
+end