Partially working AABB Tree! (needs much more work)

lib/trees/aabb_node.rb

@@ -0,0 +1,72 @@
+class IMICFPS
+  class AABBTree
+    class AABBNode
+      attr_accessor :bounding_box, :parent, :object, :a, :b
+      def initialize(parent:, object:, bounding_box:)
+        @parent = parent
+        @object = object
+        @bounding_box = bounding_box
+
+        @a = nil
+        @b = nil
+      end
+
+      def leaf?
+        @object
+      end
+
+      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
+
+          if cost_a < cost_b
+            self.a = @a.insert_subtree(leaf)
+          elsif cost_b < cost_a
+            self.b = @b.insert_subtree(leaf)
+          else
+            raise "FIXME"
+          end
+
+          @bounding_box = @bounding_box.union(leaf.bounding_box)
+
+          return self
+        end
+      end
+
+      def search_subtree(bounding_box, items = [])
+        if @bounding_box.intersect(bounding_box)
+          if leaf?
+            items << self
+          else
+            @a.search_subtree(bounding_box, items)
+            @b.search_subtree(bounding_box, items)
+          end
+        end
+
+        return items
+      end
+
+      def remove_subtree(leaf)
+      end
+
+      def update_bounding_box
+        node = self
+
+        unless node.leaf?
+          node.bounding_box = node.a.bounding_box.union(node.b.bounding_box)
+
+          while(node = node.parent)
+            node.bounding_box = node.a.bounding_box.union(node.b.bounding_box)
+          end
+        end
+      end
+    end
+  end
+end

lib/trees/aabb_tree.rb

@@ -5,67 +5,51 @@ class IMICFPS
       @root = nil
     end
 
-    def add(bounding_box, object)
+    def insert(object, bounding_box)
       raise "BoundingBox can't be nil!" unless bounding_box
       raise "Object can't be nil!" unless object
+      raise "Object already in tree!" if @objects[object]
+
+      leaf = AABBNode.new(parent: nil, object: object, bounding_box: bounding_box.dup)
+      @objects[object] = leaf
 
       if @root
-        @root.insert_subtree(bounding_box.dup, object)
+        @root.insert_subtree(leaf)
       else
-        @root = AABBNode.new(parent: nil, object: object, bounding_box: BoundingBox.new)
+        @root = leaf
       end
     end
 
     def update
+      needs_update = []
+
       @objects.each do |object, node|
-        unless object.bounding_box == node.bounding_box
-          puts "#{object.class} mutated!"
-          remove(node)
-          add(object)
+        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
     end
 
     # Returns a list of all collided objects inside Bounding Box
     def search(bounding_box)
       items = []
-      @root.search_subtree(bounding_box)
+      if @root
+        items = @root.search_subtree(bounding_box)
+      end
+
+      items.map! {|e| e.object}
+      return items
     end
 
     def remove(object)
-      @root.remove_subtree(@objects[object])
-      @objects[object] = nil
-    end
-
-    class AABBNode
-      attr_accessor :bounding_box, :parent, :object, :a, :b
-      def initialize(parent:, object:, bounding_box:)
-        @parent = parent
-        @object = object
-        @bounding_box = bounding_box
-
-        @a = nil
-        @b = nil
-      end
-
-      def make_leaf
-        @a = nil
-        @b = nil
-      end
-
-      def make_branch(node_a, node_b)
-
-      end
-
-      def insert_subtree(bounding_box, object)
-        # p "#{bounding_box} -> #{object.class}"
-      end
-
-      def remove_subtree(node)
-      end
-
-      def search_subtree(bounding_box)
-      end
+      leaf = @objects.delete(object)
+      @root.remove_subtree(leaf) if leaf
     end
   end
 end