分解能可変octreeを作ったよ

はじめに

octreeは通常，干渉判定の高速化や近傍点探索の高速化を目的として使われるものですが，筆者はどちらかというと無制限環境での地図の動的作成がやりたいので，空間分解能を積極的に上げたり落としたりできるようなデータ構造と処理を作ってみました．
場所によって空間分割数が異なることを許容するので，いわゆるMorton数を使った領域探索の高速化はできなくなりますが，筆者の意図した用途では問題ないのでよしとします．

本記事に載せているコードは全てZeoのzeo_vec3d_octree.cに実装しています．

octreeのデータ構造

octreeの空間分割単位となるノードはoctantと呼ばれます．そのデータ構造 zVec3DOctant は，次のように定義しました．

typedef struct{
  zVec3DOctant *suboctant[8];
  zAABox3D region;
  zVec3DList points;
} zVec3DOctant;

このoctantの大本となるデータ構造が，octreeである zVec3DOctree です．全体の根になるoctantを一つ持てば良いのですが，キーワードとなる 空間分解能 resolution もメンバ変数に持たせることにしました．

typedef struct{
  double resolution;
  zVec3DOctant root;
} zVec3DOctree;

分解能可変にするためのoctant操作

領域内の点の有無に応じてsuboctantをピンポイントで作る処理が必要です．これを行う関数 _zVec3DOctantAllocSuboctant() と _zVec3DOctantAddPoint() を示します．

static zVec3DOctant *_zVec3DOctantAllocSuboctant(zVec3DOctant *octant, const zVec3D *point)
{
  byte xb, yb, zb, i;

  i = ( xb = point->c.x > octant->center.c.x ? 0x1 : 0 ) |
      ( yb = point->c.y > octant->center.c.y ? 0x2 : 0 ) |
      ( zb = point->c.z > octant->center.c.z ? 0x4 : 0 );
  if( !octant->suboctant[i] ){
    if( !( octant->suboctant[i] = zAlloc( zVec3DOctant, 1 ) ) ){
      ZALLOCERROR();
      return NULL;
    }
    _zVec3DOctantInit( octant->suboctant[i] );
    _zVec3DOctantSetRegion( octant->suboctant[i],
      xb ? octant->center.c.x : zAABox3DXMin(&octant->region),
      yb ? octant->center.c.y : zAABox3DYMin(&octant->region),
      zb ? octant->center.c.z : zAABox3DZMin(&octant->region),
      xb ? zAABox3DXMax(&octant->region) : octant->center.c.x,
      yb ? zAABox3DYMax(&octant->region) : octant->center.c.y,
      zb ? zAABox3DZMax(&octant->region) : octant->center.c.z );
    zVec3DCopy( &octant->_norm, &octant->suboctant[i]->_norm );
  }
  return octant->suboctant[i];
}

static zVec3DOctant *_zVec3DOctantAddPoint(zVec3DOctant *octant, const zVec3D *point, double resolution)
{
  zVec3DOctant *suboctant;

  if( _zVec3DOctantIsSmallest( octant, resolution ) )
    return zVec3DListAdd( &octant->points, point ) ? octant : NULL;
  if( !( suboctant = _zVec3DOctantAllocSuboctant( octant, point ) ) ) return NULL;
  return _zVec3DOctantAddPoint( suboctant, point, resolution );
}

後者の _zVec3DOctantAddPoint() が，与えられた点 point をoctreeに登録する関数です．既に最小サイズのoctantであれば，点をリストに追加します．そうでなければ，_zVec3DOctantAllocSuboctant() に渡して点の存在する領域のsuboctantを生成した後に，再帰的に登録処理を行います．

_zVec3DOctantAllocSuboctant() では，まず点が8象限のどこにあるかを判定し（領域内の各軸方向中点をとって，それとの大小を比較するだけです），小さい側にあれば0，大きい側にあれば1のフラグを立てます．生成すべきsuboctantのindexは，このフラグのビット操作によって計算できます（ここの考え方はMorton数に少しだけ似ています）．またそのsuboctantの領域も，それらのビットによって直ちに決まります．

分解能を動的可変にするために，octantを分割/統合する処理が必要です．分割は次の関数で行います．

static bool _zVec3DOctantDivide(zVec3DOctant *octant, double resolution)
{
  zVec3DListCell *cp;
  zVec3D p;
  byte i;
  zVec3DOctant *suboctant;

  if( !octant ) return true;
  if( _zVec3DOctantIsSmallest( octant, resolution ) ) return true;
  while( !zListIsEmpty( &octant->points ) ){
    zListDeleteHead( &octant->points, &cp );
    if( !( suboctant = _zVec3DOctantAllocSuboctant( octant, &cp->data ) ) ) return false;
    zListInsertHead( &suboctant->points, cp );
  }
  for( i=0; i<8; i++ )
    if( !_zVec3DOctantDivide( octant->suboctant[i], resolution ) ) return false;
  return true;
}

リストに登録されている点を一つずつ参照しながら，その点が属する領域のsuboctantに移譲していきます．該当するsuboctantが無ければ新たに生成します．ここの仕組みは _zVec3DOctantAddPoint() と同じですが，さらにsuboctantを再帰的に分割していく処理が加わっています．

逆にoctantを統合する関数は，次のものです．

static void _zVec3DOctantMerge(zVec3DOctant *octant, double resolution)
{
  int i;

  if( !octant ) return;
  for( i=0; i<8; i++ )
    _zVec3DOctantMerge( octant->suboctant[i], resolution );
  if( _zVec3DOctantIsSmallest( octant, resolution ) ){
    for( i=0; i<8; i++ )
      if( octant->suboctant[i] ){
        zListAppend( &octant->points, &octant->suboctant[i]->points );
        _zVec3DOctantDestroy( octant->suboctant[i] );
        zFree( octant->suboctant[i] );
      }
  }
}

先に再帰的にsuboctantをそれぞれ統合した後，もし今のoctantが指定の空間分解能よりも小さい大きさのものであれば，suboctantが持つ点リストをもらってまとめた上で，suboctantを全て破棄します．

octree分解能変更操作

前節のoctant操作は全てstatic関数として定義しました．ユーザが用いるのは，zVec3DOctreeを操作する関数のみとなります．

まず，次の関数でoctreeを初期化します．基本的に，カバーする全直方体領域と，空間分解能を指定しているだけです．

zVec3DOctree *zVec3DOctreeInit(zVec3DOctree *octree, double xmin, double ymin, double zmin, double xmax, double ymax, double zmax, double resolution)
{
  _zVec3DOctantInit( &octree->root );
  _zVec3DOctantSetRegion( &octree->root, xmin, ymin, zmin, xmax, ymax, zmax );
  zVec3DOctreeSetResolution( octree, resolution );
  return octree;
}

使い終わったら次の関数で破棄します．

void zVec3DOctreeDestroy(zVec3DOctree *octree)
{
  _zVec3DOctantDestroy( &octree->root );
}

点の追加は，前述の_zVec3DOctantAddPoint()をoctreeのrootに適用します．

zVec3DOctant *zVec3DOctreeAddPoint(zVec3DOctree *octree, zVec3D *point)
{
  if( !_zVec3DOctantPointIsInside( &octree->root, point ) ){
    ZRUNERROR( ZEO_ERR_OCTREE_POINT_OUTOFREGION );
    return NULL;
  }
  return _zVec3DOctantAddPoint( &octree->root, point, octree->resolution );
}

また，分解能の変更は，今よりも小さくするならば_zVec3DOctantDivide()を，大きくするならば_zVec3DOctantMerge()をrootに適用すれば良いです．

bool zVec3DOctreeChangeResolution(zVec3DOctree *octree, double resolution)
{
  if( resolution < octree->resolution ){
    zVec3DOctreeSetResolution( octree, resolution );
    return _zVec3DOctantDivide( &octree->root, octree->resolution );
  }
  zVec3DOctreeSetResolution( octree, resolution );
  _zVec3DOctantMerge( &octree->root, octree->resolution );
  return true;
}

上記の機能を実際に使って，点群の追加と分解能の変更を行ってみました．点群はStanford bunnyから取ったものです．