diff --git a/.gitignore b/.gitignore
index e4b8799..15765d1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
-*.dat
\ No newline at end of file
+*.dat
+profile.*
\ No newline at end of file
diff --git a/i-mic-fps.rb b/i-mic-fps.rb
index 8e03ac9..24ab757 100644
--- a/i-mic-fps.rb
+++ b/i-mic-fps.rb
@@ -62,6 +62,8 @@ require_relative "lib/common_methods"
 
 require_relative "lib/math/vector"
 require_relative "lib/math/bounding_box"
+
+require_relative "lib/trees/aabb_tree_debug"
 require_relative "lib/trees/aabb_tree"
 require_relative "lib/trees/aabb_node"
 
diff --git a/lib/managers/collision_manager.rb b/lib/managers/collision_manager.rb
index f473cf9..1a4669f 100644
--- a/lib/managers/collision_manager.rb
+++ b/lib/managers/collision_manager.rb
@@ -10,15 +10,25 @@ class IMICFPS
     end
 
     def add(entity)
-      @aabb_tree.insert(entity, entity.normalized_bounding_box)
+      @aabb_tree.insert(entity, entity.bounding_box)
     end
 
     def update
-      @aabb_tree.update
+      @game_state.entities.each do |entity|
+        next unless entity.is_a?(Entity)
+        next unless node = @aabb_tree.objects[entity]
+
+        unless entity.bounding_box == node.bounding_box
+          @aabb_tree.update(entity, entity.bounding_box)
+        end
+      end
 
       check_broadphase
 
       @physics_manager.update
+
+      # binding.irb
+      p @aabb_tree
     end
 
     def remove(entity)
@@ -30,7 +40,7 @@ class IMICFPS
       broadphase = {}
 
       @game_state.entities.each do |entity|
-        search = @aabb_tree.search(entity.normalized_bounding_box)
+        search = @aabb_tree.search(entity.bounding_box)
         if search.size > 0
           search.reject! {|ent| ent == entity}
           broadphase[entity] = search
@@ -38,7 +48,7 @@ class IMICFPS
       end
 
       broadphase.each do |entity, _collisions|
-        _collisions.reject! {|ent| !entity.normalized_bounding_box.intersect(ent.normalized_bounding_box)}
+        _collisions.reject! {|ent| !entity.bounding_box.intersect(ent.bounding_box)}
         # TODO: mesh aabb tree vs other mesh aabb tree check
         # TODO: triangle vs other triangle check
         _collisions.each do |ent|
diff --git a/lib/math/bounding_box.rb b/lib/math/bounding_box.rb
index 955b87a..e8d6822 100644
--- a/lib/math/bounding_box.rb
+++ b/lib/math/bounding_box.rb
@@ -26,13 +26,14 @@ class IMICFPS
       return temp
     end
 
-    # returns boolean
+    # returns whether both bounding boxes intersect
     def intersect(other)
       (@min.x <= other.max.x && @max.x >= other.min.x) &&
       (@min.y <= other.max.y && @max.y >= other.min.y) &&
       (@min.z <= other.max.z && @max.z >= other.min.z)
     end
 
+    # returns the difference between both bounding boxes
     def difference(other)
       temp = BoundingBox.new
       temp.min = @min - other.min
@@ -41,6 +42,13 @@ class IMICFPS
       return temp
     end
 
+    # returns whether the vector is inside of the bounding box
+    def contains(vector)
+      vector.x.between?(@min.x, @max.x) &&
+      vector.y.between?(@min.y, @max.y) &&
+      vector.z.between?(@min.z, @max.z)
+    end
+
     def volume
       width * height * depth
     end
@@ -82,5 +90,25 @@ class IMICFPS
 
       return temp
     end
+
+    def +(other)
+      box = BoundingBox.new
+      box.min = self.min + other.min
+      box.min = self.max + other.max
+
+      return box
+    end
+
+    def -(other)
+      box = BoundingBox.new
+      box.min = self.min - other.min
+      box.min = self.max - other.max
+
+      return box
+    end
+
+    def sum
+      @min.sum + @max.sum
+    end
   end
 end
\ No newline at end of file
diff --git a/lib/math/vector.rb b/lib/math/vector.rb
index 8f2c8ad..a641def 100644
--- a/lib/math/vector.rb
+++ b/lib/math/vector.rb
@@ -76,6 +76,10 @@ class IMICFPS
       self / Vector.new(mag, mag, mag)
     end
 
+    def sum
+      @x + @y + @z + @weight
+    end
+
     def to_a
       [@x, @y, @z, @weight]
     end
diff --git a/lib/objects/entity.rb b/lib/objects/entity.rb
index f7901dc..8ffafd7 100644
--- a/lib/objects/entity.rb
+++ b/lib/objects/entity.rb
@@ -8,7 +8,7 @@ class IMICFPS
     include CommonMethods
     attr_accessor :scale, :visible, :renderable, :backface_culling
     attr_reader :position, :rotation, :velocity
-    attr_reader :model, :name, :debug_color, :normalized_bounding_box
+    attr_reader :model, :name, :debug_color, :bounding_box
     def initialize(x: 0, y: 0, z: 0, bound_model: nil, scale: MODEL_METER_SCALE, backface_culling: true, auto_manage: true, manifest_file: nil)
       @position = Vector.new(x, y, z)
       @scale = scale
@@ -53,9 +53,9 @@ class IMICFPS
       @bound_model = model
       @bound_model.model.entity = self
       @bound_model.model.objects.each {|o| o.scale = self.scale}
-      @normalized_bounding_box = normalize_bounding_box_with_offset
+      @bounding_box = normalize_bounding_box_with_offset
 
-      box = normalize_bounding_box
+      # box = normalize_bounding_box
     end
 
     def model
@@ -79,7 +79,7 @@ class IMICFPS
       @delta_time = Gosu.milliseconds
 
       unless at_same_position?
-        @normalized_bounding_box = normalize_bounding_box_with_offset if model
+        @bounding_box = normalize_bounding_box_with_offset if model
       end
     end
 
diff --git a/lib/trees/aabb_node.rb b/lib/trees/aabb_node.rb
index b0a9c3a..dcf1392 100644
--- a/lib/trees/aabb_node.rb
+++ b/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
diff --git a/lib/trees/aabb_tree.rb b/lib/trees/aabb_tree.rb
index 6a86ce4..3443440 100644
--- a/lib/trees/aabb_tree.rb
+++ b/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
\ No newline at end of file
diff --git a/lib/trees/aabb_tree_debug.rb b/lib/trees/aabb_tree_debug.rb
new file mode 100644
index 0000000..7e541ba
--- /dev/null
+++ b/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
\ No newline at end of file