Octree in C

octree.c

// A C port of the example at https://iq.opengenus.org/octree

#include <stdlib.h>
#include "octree.h"

Octree * octree_point(int x, int y, int z) {
  Octree * p = malloc(sizeof(Octree));

  p->point = malloc(sizeof(Point3D));
  p->point->x = x;
  p->point->y = y;
  p->point->z = z;

  p->from = NULL;
  p->to = NULL;
  for (int i = 0; i < 8; i++) p->children[i] = NULL;
  return p;
}

Octree * octree_empty() { return octree_point(-1, -1, -1); }

Octree * octree_new(int from_x, int from_y, int from_z, int to_x, int to_y, int to_z) {
  Octree * o = malloc(sizeof(Octree));
  o->point = NULL;

  o->from = malloc(sizeof(Point3D));
  o->from->x = from_x;
  o->from->y = from_y;
  o->from->z = from_z;

  o->to = malloc(sizeof(Point3D));
  o->to->x = to_x;
  o->to->y = to_y;
  o->to->z = to_z;

  for (int i = 0; i < 8; i++) o->children[i] = octree_empty();

  return o;
}

void octree_insert(Octree * octree, int x, int y, int z) {
  if (                      //
  x < octree->from->x ||    //
  x > octree->to->x ||      //
  y < octree->from->y ||    //
  y > octree->to->y ||      //
  z < octree->from->z ||    //
  z > octree->to->z)
    return;

  int midx = (octree->from->x + octree->to->x) >> 1;
  int midy = (octree->from->y + octree->to->y) >> 1;
  int midz = (octree->from->z + octree->to->z) >> 1;

  int pos = -1;
  if (x <= midx) {
    if (y <= midy) {
      if (z <= midz)
        pos = TOP_LEFT_FRONT;
      else
        pos = TOP_LEFT_BACK;
    } else {
      if (z <= midz)
        pos = BOTTOM_LEFT_FRONT;
      else
        pos = BOTTOM_LEFT_BACK;
    }
  } else {
    if (y <= midy) {
      if (z <= midz)
        pos = TOP_RIGHT_FRONT;
      else
        pos = TOP_RIGHT_BACK;
    } else {
      if (z <= midz)
        pos = BOTTOM_RIGHT_FRONT;
      else
        pos = BOTTOM_RIGHT_BACK;
    }
  }

  if (octree->children[pos]->point == NULL) {
    // if region node
    octree_insert(octree->children[pos], x, y, z);
    return;
  } else if (octree->children[pos]->point->x == -1) {
    // if empty node
    octree_free(octree->children[pos]);
    octree->children[pos] = octree_point(x, y, z);
    return;
  } else {
    int x_ = octree->children[pos]->point->x;
    int y_ = octree->children[pos]->point->y;
    int z_ = octree->children[pos]->point->z;

    octree_free(octree->children[pos]);
    octree->children[pos] = NULL;

    if (pos == TOP_LEFT_FRONT) {
      octree->children[pos] = octree_new(octree->from->x, octree->from->y, octree->from->z, midx, midy, midz);
    } else if (pos == TOP_RIGHT_FRONT) {
      octree->children[pos] = octree_new(midx + 1, octree->from->y, octree->from->z, octree->to->x, midy, midz);
    } else if (pos == BOTTOM_RIGHT_FRONT) {
      octree->children[pos] = octree_new(midx + 1, midy + 1, octree->from->z, octree->to->x, octree->to->y, midz);
    } else if (pos == BOTTOM_LEFT_FRONT) {
      octree->children[pos] = octree_new(octree->from->x, midy + 1, octree->from->z, midx, octree->to->y, midz);
    } else if (pos == TOP_LEFT_BACK) {
      octree->children[pos] = octree_new(octree->from->x, octree->from->y, midz + 1, midx, midy, octree->to->z);
    } else if (pos == TOP_RIGHT_BACK) {
      octree->children[pos] = octree_new(midx + 1, octree->from->y, midz + 1, octree->to->x, midy, octree->to->z);
    } else if (pos == BOTTOM_RIGHT_BACK) {
      octree->children[pos] = octree_new(midx + 1, midy + 1, midz + 1, octree->to->x, octree->to->y, octree->to->z);
    } else if (pos == BOTTOM_LEFT_BACK) {
      octree->children[pos] = octree_new(octree->from->x, midy + 1, midz + 1, midx, octree->to->y, octree->to->z);
    }

    octree_insert(octree->children[pos], x_, y_, z_);
    octree_insert(octree->children[pos], x, y, z);
  }
}

bool octree_find(Octree * octree, int x, int y, int z) {
  if (x < octree->from->x ||    //
  x > octree->to->x ||          //
  y < octree->from->y ||        //
  y > octree->to->y ||          //
  z < octree->from->z ||        //
  z > octree->to->z)
    return false;

  int midx = (octree->from->x + octree->to->x) >> 1;
  int midy = (octree->from->y + octree->to->y) >> 1;
  int midz = (octree->from->z + octree->to->z) >> 1;
  int pos = -1;

  if (x <= midx) {
    if (y <= midy) {
      if (z <= midz)
        pos = TOP_LEFT_FRONT;
      else
        pos = TOP_LEFT_BACK;
    } else {
      if (z <= midz)
        pos = BOTTOM_LEFT_FRONT;
      else
        pos = BOTTOM_LEFT_BACK;
    }
  } else {
    if (y <= midy) {
      if (z <= midz)
        pos = TOP_RIGHT_FRONT;
      else
        pos = TOP_RIGHT_BACK;
    } else {
      if (z <= midz)
        pos = BOTTOM_RIGHT_FRONT;
      else
        pos = BOTTOM_RIGHT_BACK;
    }
  }

  if (octree->children[pos]->point == NULL) {
    // if region node
    return octree_find(octree->children[pos], x, y, z);
  } else if (octree->children[pos]->point->x == -1) {
    // if empty node
    return false;
  } else {
    if (                                       //
    x == octree->children[pos]->point->x &&    //
    y == octree->children[pos]->point->y &&    //
    z == octree->children[pos]->point->z)
      return true;
  }

  return false;
}

void octree_free(Octree * octree) {
  if (octree) {
    if (octree->point) {
      free(octree->point);
      octree->point = NULL;
    }

    if (octree->from) {
      free(octree->from);
      octree->from = NULL;
    }

    if (octree->to) {
      free(octree->to);
      octree->to = NULL;
    }

    for (int i = 0; i < 8; i++) {
      if (octree->children[i]) {
        octree_free(octree->children[i]);
        octree->children[i] = NULL;
      }
    };

    free(octree);
    octree = NULL;
  }
}

octree.h

#ifndef OCTREE_H
#define OCTREE_H

enum OCTREE_REGION {
  TOP_LEFT_FRONT,
  TOP_RIGHT_FRONT,
  BOTTOM_LEFT_FRONT,
  BOTTOM_RIGHT_FRONT,
  TOP_LEFT_BACK,
  TOP_RIGHT_BACK,
  BOTTOM_LEFT_BACK,
  BOTTOM_RIGHT_BACK,
};

typedef struct {  int x, y, z;} Point3D;
typedef struct octree Octree;

struct octree {
  Point3D * point;    // NULL means regional, -1,-1,-1 means empty
  Point3D * from;     // top left front
  Point3D * to;       // bottom right back
  Octree * children[8];
};

Octree * octree_point(int x, int y, int z);
Octree * octree_empty();
Octree * octree_new(int from_x, int from_y, int from_z, int to_x, int to_y, int to_z);
void octree_insert(Octree * octree, int x, int y, int z);
void octree_free(Octree * octree);
bool octree_find(Octree * octree, int x, int y, int z);

#endif