diff --git a/i-mic-fps.rb b/i-mic-fps.rb
index 199d95a..8e03ac9 100644
--- a/i-mic-fps.rb
+++ b/i-mic-fps.rb
@@ -63,6 +63,7 @@ require_relative "lib/common_methods"
 require_relative "lib/math/vector"
 require_relative "lib/math/bounding_box"
 require_relative "lib/trees/aabb_tree"
+require_relative "lib/trees/aabb_node"
 
 require_relative "lib/managers/input_mapper"
 require_relative "lib/managers/shader_manager"
diff --git a/lib/managers/collision_manager.rb b/lib/managers/collision_manager.rb
index c4a3e50..f473cf9 100644
--- a/lib/managers/collision_manager.rb
+++ b/lib/managers/collision_manager.rb
@@ -10,12 +10,14 @@ class IMICFPS
     end
 
     def add(entity)
-      @aabb_tree.add(entity.normalized_bounding_box, entity)
+      @aabb_tree.insert(entity, entity.normalized_bounding_box)
     end
 
     def update
-      lazy_check_collisions
       @aabb_tree.update
+
+      check_broadphase
+
       @physics_manager.update
     end
 
@@ -23,25 +25,24 @@ class IMICFPS
       @aabb_tree.remove(entity)
     end
 
-    def lazy_check_collisions
-      # Expensive AABB collision detection
+    def check_broadphase
+      @collisions.clear
+      broadphase = {}
+
       @game_state.entities.each do |entity|
-        @game_state.entities.each do |other|
-          next if entity == other
-          next if entity.is_a?(Terrain) || other.is_a?(Terrain)
-          next unless entity.collidable?
-          next unless other.collidable?
+        search = @aabb_tree.search(entity.normalized_bounding_box)
+        if search.size > 0
+          search.reject! {|ent| ent == entity}
+          broadphase[entity] = search
+        end
+      end
 
-          if entity.normalized_bounding_box.intersect(other.normalized_bounding_box)
-            entity.debug_color = Color.new(1.0,0.0,0.0)
-            other.debug_color = Color.new(1.0,0.0,0.0)
-
-           # @game_state.entities.delete(entity) unless entity.is_a?(Player)
-            # puts "#{entity} is intersecting #{b}" if entity.is_a?(Player)
-          else
-            entity.debug_color = Color.new(0,1,0)
-            other.debug_color = Color.new(0,1,0)
-          end
+      broadphase.each do |entity, _collisions|
+        _collisions.reject! {|ent| !entity.normalized_bounding_box.intersect(ent.normalized_bounding_box)}
+        # TODO: mesh aabb tree vs other mesh aabb tree check
+        # TODO: triangle vs other triangle check
+        _collisions.each do |ent|
+          @collisions[entity] = _collisions
         end
       end
     end
diff --git a/lib/managers/physics_manager.rb b/lib/managers/physics_manager.rb
index 9f3f92d..8c15081 100644
--- a/lib/managers/physics_manager.rb
+++ b/lib/managers/physics_manager.rb
@@ -6,7 +6,7 @@ class IMICFPS
 
     def update
       @collision_manager.collisions.each do |entity, versus|
-        versus.each do |versus|
+        versus.each do |other|
           resolve(entity, other)
         end
       end
diff --git a/lib/trees/aabb_node.rb b/lib/trees/aabb_node.rb
new file mode 100644
index 0000000..b0a9c3a
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/lib/trees/aabb_tree.rb b/lib/trees/aabb_tree.rb
index 192f26c..6a86ce4 100644
--- a/lib/trees/aabb_tree.rb
+++ b/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
\ No newline at end of file