Add a geometry pool, shape hierarchy, addittion and deletion.

src/util.h

@@ -33,6 +33,7 @@ static void vec_grow(vector_t *v, int min_capacity) {
     int new_cap = v->capacity ? v->capacity * 2 : 8;
     if (new_cap < min_capacity) new_cap = min_capacity;
     uint8_t *new_data = (uint8_t*) ALLOC(new_cap * v->stride);
+    assert(new_data != NULL);
     if (v->data) {
         memcpy(new_data, v->data, v->count * v->stride);
         FREE(v->data);
@@ -61,6 +62,28 @@ static void vec_pop(vector_t *v) {
     if (v->count > 0) v->count--;
 }
 
+static void vec_remove_ordered(vector_t *v, int index) {
+    if (index < 0 || index >= v->count) return;
+    if (index < v->count - 1) {
+        memmove(v->data + index * v->stride,
+                v->data + (index + 1) * v->stride,
+                (v->count - index - 1) * v->stride);
+    }
+    v->count--;
+}
+
+static void *vec_insert(vector_t *v, int index) {
+    if (index < 0 || index > v->count) return NULL;
+    if (v->count >= v->capacity) vec_grow(v, v->count + 1);
+    if (index < v->count) {
+        memmove(v->data + (index + 1) * v->stride,
+                v->data + index * v->stride,
+                (v->count - index) * v->stride);
+    }
+    v->count++;
+    return v->data + index * v->stride;
+}
+
 /**
  * Remove the element at index by swapping in the last element (O(1)).
  * Order is not preserved.