octree.h

Go to the documentation of this file.

/*******************************************************************************
** Copyright (c) 2008 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/
/*******************************************************************************
** $RCSfile: octree.h,v $ $Revision: 1.4 $ $State: Exp $
*******************************************************************************/



#ifndef octree_H_
#define octree_H_

#include <stdarg.h>
#include <map>
#include <vector>

#include <cmdLine/cmdStdOutput.h>

#include "vrfobjcore/spatialVrfObjectManager.h"
#include "geometry/tdbextent.h"

template<class T> class DtOctreeNode;


#define OCTREE_DEBUG 0

#if OCTREE_DEBUG

static int octree_debug_level = 0;


static void DEBUG(const char* fmt, ...)
{
   char buffer[8192];
   va_list ap;
   va_start(ap, fmt);
   vsprintf(buffer, fmt, ap);
   va_end(ap);

   DtInfo("OCT %*s%s\n", octree_debug_level*2, "", buffer);
}

#  define DEBUG_PUSH do { octree_debug_level++; } while(0)
#  define DEBUG_POP do { octree_debug_level--; } while(0)
#else

static inline void DEBUG(const char* fmt, ...)
{ }

#  define DEBUG_PUSH do { } while(0)
#  define DEBUG_POP do { } while(0)
#endif

#define EXT_FMT "[%lf, %lf, %lf] - [%lf, %lf, %lf]"
#define EXT_ARGS(x) (x).minX(), (x).minY(), (x).minZ(), (x).maxX(), (x).maxY(), (x).maxZ()
#define OBJ_ARGS(x) ((const char*) (x)->objectName())


template<class T> class DtOctreeMember
{
public:
   DtOctreeMember(T* object, const DtExtent& extent)
      : myObject(object)
      , myExtent(extent)
      , boundNode(0)
   {
   }

   void bind(DtOctreeNode<T>* node);
   
   void unbind();

public:
   T* myObject;

   DtExtent myExtent;

   DtOctreeNode<T>* boundNode;

   typename std::list<DtOctreeMember*>::iterator boundIterator;
};


template<class T> class DtOctreeNode
{
protected: 

   typedef          std::list<DtOctreeMember<T>*>           MemberContainer;
   typedef typename std::list<DtOctreeMember<T>*>::iterator MemberIterator;

   static const int NUM_BRANCHES = 8;

   static const int MAX_CHILD_MEMBERS = 10;

   DtOctreeNode(const DtExtent &region)
      : myRegion(region)
      , myCenter(region.center())
      , myChildren(0)
   { 
      DEBUG("Create node " EXT_FMT, EXT_ARGS(region));
   }

   ~DtOctreeNode()
   {
      DEBUG("Destroy node " EXT_FMT, EXT_ARGS(myRegion));
      DEBUG_PUSH;

      if (myChildren)
      {
         for(int i=0; i<NUM_BRANCHES; i++)
            delete myChildren[i];
      }
      delete[] myChildren;

      DEBUG_POP;
   }

   
   virtual void visitObjects(const DtExtent& extent, DtSpatialSelectionFunctor &callback)
   {
      DEBUG(EXT_FMT "::visit(" EXT_FMT ", cb)", EXT_ARGS(myRegion), EXT_ARGS(extent));
      DEBUG_PUSH;

      MemberIterator iter, end;

      for(iter = myMembers.begin(), end = myMembers.end(); iter != end; ++iter)
      {
         DEBUG("Member %s " EXT_FMT, OBJ_ARGS((*iter)->myObject), EXT_ARGS((*iter)->myExtent));
         callback((*iter)->myObject);
      }

      if (myChildren)
      {
         int mask = intersectionMask(extent);
         for(int i=0; i<NUM_BRANCHES; i++)
         {
            if (myChildren[i] && (mask & (1<<i)))
               myChildren[i]->visitObjects(extent, callback);
         }
      }

      DEBUG_POP;
   }
   
   virtual void addMember(DtOctreeMember<T>* member)
   {
      DEBUG(EXT_FMT "::addMember(%s)", EXT_ARGS(myRegion), OBJ_ARGS(member->myObject));
      DEBUG_PUSH;
      
#if OCTREE_DEBUG
      if (!myRegion.contains(member->myExtent))
      {
         DEBUG("FAIL!  " EXT_FMT " does not contain " EXT_FMT, EXT_ARGS(myRegion), EXT_ARGS(member->myExtent));
      }
#endif

      if (myChildren)
      {
         int mask = intersectionMask(member->myExtent);
         int childIndex = childIndexFor(mask);

         if (childIndex == -1) {
            DEBUG("add as member (children, but won't fit)");
            member->bind(this);
         } else {
            addMemberToChild(member, childIndex);
         }
      } else {
         DEBUG("add as child member (not enough child members yet)");

         member->bind(this);

         if (numChildCandidates() >= MAX_CHILD_MEMBERS)
         {
            split();
         }
      }

      DEBUG_POP;
   }

   bool coalesceEmptyChildren()
   {
      DEBUG(EXT_FMT "::coalesceEmptyChildren()", EXT_ARGS(myRegion));
      DEBUG_PUSH;

      bool retVal = (myMembers.size() == 0);
      
      if (myChildren)
      {
         for(int i=0; i<NUM_BRANCHES; i++)
         {
            if (myChildren[i])
            {
               if (myChildren[i]->coalesceEmptyChildren())
               {
                  DEBUG("Deleting child %d\n", i);
                  delete myChildren[i];
                  myChildren[i] = 0;
               } else {
                  retVal = false;
               }
            }
         }
      }

      DEBUG_POP;

      return retVal;
   }

   int numChildCandidates()
   {
      int retVal = 0;
      
      MemberIterator iter, end;
      for(iter = myMembers.begin(), end = myMembers.end(); iter != end; ++iter)
      {
         if (childIndexFor(intersectionMask((*iter)->myExtent)) != -1)
         {
            retVal++;
         }
      }

      return retVal;
   }

   int intersectionMask(const DtExtent &extent) const
   {
      int result = 0xff;                  

      if (extent.minX() >= myCenter.x())
         result &= 0xf0;                  
      if (extent.maxX() <= myCenter.x())
         result &= 0x0f;                  

      if (extent.minY() >= myCenter.y())
         result &= 0xcc;                  
      if (extent.maxY() <= myCenter.y())
         result &= 0x33;                  

      if (extent.minZ() >= myCenter.z())
         result &= 0xaa;                  
      if (extent.maxZ() <= myCenter.z())
         result &= 0x55;                  

      if (result == 0)
      {
         DtWarn("Can't happen!  Octree intersection mask is 0!");
      }

      return result;
   }

   int childIndexFor(int mask)
   {
      if (!mask || (mask & (mask-1))) 
      {
         return -1;
      }

      int childIndex;
      for(childIndex=0; childIndex<NUM_BRANCHES; childIndex++)
      {
         if (mask & (1<<childIndex))
            return childIndex;
      }

      DtWarn("Can't happen: no valid child index in octree code.");
      return -1;
   }


   void addMemberToChild(DtOctreeMember<T>* member, int childIndex)
   {
      DEBUG(EXT_FMT "::addMemberToChild(%s, %d)", EXT_ARGS(myRegion), OBJ_ARGS(member->myObject), childIndex);
      DEBUG_PUSH;

      if (myChildren[childIndex] == 0)
      {
         DtPoint childCorner;

         if (childIndex & 4)
            childCorner.setX(myRegion.maxX());
         else
            childCorner.setX(myRegion.minX());

         if (childIndex & 2)
            childCorner.setY(myRegion.maxY());
         else
            childCorner.setY(myRegion.minY());

         if (childIndex & 1)
            childCorner.setZ(myRegion.maxZ());
         else
            childCorner.setZ(myRegion.minZ());

         myChildren[childIndex] = new DtOctreeNode(DtExtent(childCorner, myCenter));
      }

      myChildren[childIndex]->addMember(member);

      DEBUG_POP;
   }


   void split()
   {
      DEBUG(EXT_FMT "::split()", EXT_ARGS(myRegion));
      DEBUG_PUSH;

      myChildren = new DtOctreeNode<T>* [NUM_BRANCHES];

      for(int i=0; i<NUM_BRANCHES; i++)
      {
         myChildren[i] = 0;
      }

      MemberIterator iter, next, end;
      for(iter = myMembers.begin(), end = myMembers.end(); iter != end; iter = next)
      {
         next = iter;
         ++next;

         DtOctreeMember<T>* member = *iter;
         int childIndex = childIndexFor(intersectionMask(member->myExtent));

         if (childIndex != -1)
         {
            member->unbind();
            addMemberToChild(member, childIndex);
         }
      }

      DEBUG_POP;
   }

   const DtExtent myRegion;
   
   const DtPoint myCenter;

   DtOctreeNode<T> **myChildren;

   MemberContainer myMembers;

   template<class U> friend class DtOctree;
   template<class U> friend class DtOctreeMember;
   template<class U> friend class DtOctreeDebugger;
};


template<class T> class DtOctree
{
public:

   typedef          std::map<T*, DtOctreeMember<T>*>           MemberContainer;
   typedef typename std::map<T*, DtOctreeMember<T>*>::iterator MemberIterator;

   typedef          std::list<T*>                            OutsideAreaContainer;
   typedef typename std::list<T*>::iterator                  OutsideAreaIterator;

   DtOctree(const DtExtent &region)
   {
      myRootNode = new DtOctreeNode<T>(region);
   }

   virtual ~DtOctree()
   {
      delete myRootNode;

      MemberIterator iter, end;
      for(iter = myMemberMap.begin(), end = myMemberMap.end(); iter != end; ++iter)
      {
         delete (*iter).second;
      }
   }

   virtual void visitObjects(const DtExtent& extent, DtSpatialSelectionFunctor &callback)
   {
      if (myRootNode->myRegion.intersects(extent))
      {
         myRootNode->visitObjects(extent, callback);
      }

      OutsideAreaIterator iter, end;
      for(iter = myObjectsOutsideArea.begin(), end = myObjectsOutsideArea.end(); iter != end; ++iter)
      {
         callback(*iter);
      }
   }

   virtual void addObject(T* object, const DtExtent& extent)
   {
      DEBUG("DtOctree::addObject(%s, " EXT_FMT ")", OBJ_ARGS(object), EXT_ARGS(extent));
      DEBUG_PUSH;

      if (myRootNode->myRegion.contains(extent))
      {
         DtOctreeMember<T>* newMember = new DtOctreeMember<T>(object, extent);
         myRootNode->addMember(newMember);
         myMemberMap[object] = newMember;
      } else {
         DtWarn("Member %s (" EXT_FMT ") added outside octree (" EXT_FMT ")",
                OBJ_ARGS(object), EXT_ARGS(extent), EXT_ARGS(myRootNode->myRegion));
         myObjectsOutsideArea.push_back(object);
      }

      DEBUG_POP;
   }

   virtual void removeObject(T* object)
   {
      DEBUG("DtOctree::removeObject(%s)", OBJ_ARGS(object));
      DEBUG_PUSH;

      MemberIterator iter;
      iter = myMemberMap.find(object);
      if (iter != myMemberMap.end())
      {
         DtOctreeMember<T>* member = (*iter).second;
         member->unbind();
         myMemberMap.erase(iter);
         delete member;
      } else if (!removeObjectOutsideArea(object)) {

         //DtWarn("Tried to remove %s from octree, but couldn't find!\n", OBJ_ARGS(object));
      }

      DEBUG_POP;
   }

   virtual void updatePositions()
   {
      MemberIterator iter, end;

      for(iter = myMemberMap.begin(), end = myMemberMap.end(); iter != end; ++iter)
      {
         T* object = (*iter).first;
         DtOctreeMember<T>* member = (*iter).second;

         member->myExtent = object->extent();

         if (!myRootNode->myRegion.contains(member->myExtent))
         {

            DtWarn("Object %s (" EXT_FMT ") moving outside octree (" EXT_FMT ")",
                  OBJ_ARGS(object), EXT_ARGS(member->myExtent), EXT_ARGS(myRootNode->myRegion));
            removeObject(object);
            myObjectsOutsideArea.push_back(object);
         } else if (!member->boundNode->myRegion.contains(member->myExtent)) {

            member->unbind();
            myRootNode->addMember(member);
         } else {
         }            
      }

      myRootNode->coalesceEmptyChildren();
   }

   bool containsObject(T* object)
   {
      return myMemberMap.find(object) != myMemberMap.end();
   }

   int numObjects()
   {
      return myMemberMap.size();
   }

protected:
   bool removeObjectOutsideArea(T* object)
   {
      OutsideAreaIterator iter, end;
      for(iter = myObjectsOutsideArea.begin(), end = myObjectsOutsideArea.end(); iter != end; ++iter)
      {
         if (*iter == object)
         {
            myObjectsOutsideArea.erase(iter);
            return true;
         }
      }

      return false;
   }

   DtOctreeNode<T>* myRootNode;
   
   OutsideAreaContainer myObjectsOutsideArea;
   
   MemberContainer myMemberMap;
};


#if OCTREE_DEBUG

class DtDebugOctreeFunctor : public DtSpatialSelectionFunctor
{
public:
   DtDebugOctreeFunctor(DtSpatialSelectionFunctor& inner)
      : myInner(inner)
   {
   }  

   virtual bool operator()(const DtVrfObject* object)
   {
      mySeenObjects.insert(object);
      return myInner(object);
   }

   std::set<const DtVrfObject*> mySeenObjects;
   DtSpatialSelectionFunctor& myInner;
};

template<class T> class DtOctreeDebugger : public DtOctree<T>
{
public:
   DtOctreeDebugger(const DtExtent& extent)
      : DtOctree<T>(extent)
   { }

   void addObject(T* object, const DtExtent& extent)
   {
      allObjects[object] = extent;
      DtOctree<T>::addObject(object, extent);
   }

   void removeObject(T* object)
   {
      allObjects.erase(object);
      DtOctree<T>::removeObject(object);
   }

   void updatePositions()
   {
      std::map<T*, DtExtent>::iterator iter = allObjects.begin();
      std::map<T*, DtExtent>::iterator end  = allObjects.end();

      for(; iter != end; ++iter)
      {
         T* object = (*iter).first;
         DtExtent& extent = (*iter).second;

         extent = object->extent();
      }

      DtOctree<T>::updatePositions();
   }

   void visitObjects(const DtExtent& extent, DtSpatialSelectionFunctor &callback)
   {
      DtDebugOctreeFunctor debugFunctor(callback);
      DtOctree<T>::visitObjects(extent, debugFunctor);

      std::map<T*, DtExtent>::iterator iter = allObjects.begin();
      std::map<T*, DtExtent>::iterator end  = allObjects.end();

      for(; iter != end; ++iter)
      {
         T* object = (*iter).first;
         const DtExtent& objectExtent = (*iter).second;

         if (objectExtent.intersects(extent) && debugFunctor.mySeenObjects.count(object) == 0)
         {
            DEBUG("FAILED to visit object %s", OBJ_ARGS(object));
            DEBUG_PUSH;
            DEBUG("Old extent is " EXT_FMT, EXT_ARGS(objectExtent));
            DEBUG("New extent is " EXT_FMT, EXT_ARGS(object->extent()));
            DEBUG_POP;
         }
      }
   }
protected:
   std::map<T*, DtExtent> allObjects;
};


static DtOctree<DtVrfObject>* createOctree(const DtExtent& extent)
{
   return new DtOctreeDebugger<DtVrfObject>(extent);
}

#else

static DtOctree<DtVrfObject>* createOctree(const DtExtent& extent)
{
   return new DtOctree<DtVrfObject>(extent);
}

#endif


template<class T> void DtOctreeMember<T>::bind(DtOctreeNode<T>* node)
{
   if (boundNode)
   {
      DtWarn("Octree binding node for %s when already bound!", OBJ_ARGS(myObject));
      unbind();
   }

   boundNode = node;
   boundIterator = node->myMembers.insert(node->myMembers.end(), this);
}

template<class T> void DtOctreeMember<T>::unbind()
{
   if (boundNode)
   {
      boundNode->myMembers.erase(boundIterator);
      boundNode = NULL;
   }
}

#endif