Fixing storage reference bug in KDTree

Sources/GeometriaAlgorithms/SpatialPartitioning/KDTree/KDTree.swift

@@ -110,6 +110,8 @@ public struct KDTree<Element: KDTreeLocatable> where Element.Vector: VectorCompa
 
     /// Inserts a given element on this k-d tree.
     public mutating func insert(_ element: Element) {
+        ensureUnique()
+
         guard let root else {
             root = Subdivision(
                 state: .empty(
@@ -128,12 +130,13 @@ public struct KDTree<Element: KDTreeLocatable> where Element.Vector: VectorCompa
         let path = subdivisionPath(forInserting: element)
         let reversed = path.reversed
 
-        ensureUnique()
         root.inserting(element, path: reversed.asPath)
     }
 
     /// If `element` is contained within this k-d tree, it is removed in-place.
     public mutating func remove(_ element: Element) where Element: Equatable {
+        ensureUnique()
+
         guard let root else {
             return
         }
@@ -156,11 +159,12 @@ public struct KDTree<Element: KDTreeLocatable> where Element.Vector: VectorCompa
 
     /// Removes an element at a given index in this k-d tree.
     public mutating func remove(at index: Index) {
+        ensureUnique()
+
         guard let root, root.pathExists(index.path) else {
             fatalError("Index \(index) is not part of this k-d tree.")
         }
 
-        ensureUnique()
         self.root = root.removing(at: index.path)
     }