// Usage example for bullet:
void Physics::addVoxelTree()
{
   // I'm using a modifed RawVolume here which returns m_pData
   boost::uint8_t * voxels = volume->getVoxels();
   VoxTreeCoord minCoord(0,0,0);
   VoxTreeCoord maxCoord(volume->getWidth() - 1, volume->getHeight() - 1, volume->getDepth() - 1);
   VoxTreeCoord stride(1, volume->getWidth(), volume->getHeight() * volume->getWidth());

   // This is the bit that does all the magic:
   VoxTree<boost::uint8_t> tree(voxels, minCoord, maxCoord, stride);

   // This is the bit that makes it work in bullet:
   addVoxelNode(tree.root());
}

void Physics::addVoxelNode(VoxTreeNode * node)
{
   switch(node->mValue)
   {
   case VoxTreeNode::nodeMixed:
      addVoxelNode(node->mChildren[0]);
      addVoxelNode(node->mChildren[1]);
      break;
   case VoxTreeNode::nodeFull:
      {
         btVector3 low(btScalar(node->mMinCoord.x), btScalar(node->mMinCoord.y), btScalar(node->mMinCoord.z));
         low -= btVector3(0.5f, 0.5f, 0.5f);
         btVector3 high(btScalar(node->mMaxCoord.x), btScalar(node->mMaxCoord.y), btScalar(node->mMaxCoord.z));
         high += btVector3(0.5f, 0.5f, 0.5f);
         addStaticBox(0.5f * (low + high), 0.5f * (high - low));
      }
      break;
   default:
      break;
   }
}

void Physics::addStaticBox(const btVector3 & pos, const btVector3 & halfSize)
{
   btCollisionShape * colShape = new btBoxShape(halfSize);
   btRigidBody::btRigidBodyConstructionInfo rbInfo(0.0f,0,colShape);
   btRigidBody * body = new btRigidBody(rbInfo);
   btTransform trans;
   trans.setIdentity();
   trans.setOrigin(pos);
   body->setWorldTransform(trans);
   mDynamicsWorld->addRigidBody(body);
}

#ifndef MOUSE_VOXTREE_H
#define MOUSE_VOXTREE_H

#include "VoxTreeNode.h"
#include "VoxTreeCoord.h"

/**
VoxTree.h
Header for class VoxTree
*/
template <class Voxel>
class VoxTree
{
public:
    VoxTree(Voxel * voxels, VoxTreeCoord minCoord, VoxTreeCoord maxCoord, VoxTreeCoord stride);
    virtual ~VoxTree();

   VoxTreeNode * root() { return mRoot; }
private:
   VoxTree(const VoxTree&);
   VoxTree& operator=(const VoxTree&);

   void populate(VoxTreeNode * node);
   void findBestSplit(VoxTreeNode * node, int & pos, int & quality, int axis);
   void eval(VoxTreeNode * node);
   Voxel * at(int x, int y, int z) { return mVoxels + z * mStride.z + y * mStride.y + x * mStride.x; }

private:
   VoxTreeNode * mRoot;
   Voxel * mVoxels;
   VoxTreeCoord mMinCoord;
   VoxTreeCoord mMaxCoord;
   VoxTreeCoord mStride;
};

template <class Voxel>
VoxTree<Voxel>::VoxTree(Voxel * voxels, VoxTreeCoord minCoord, VoxTreeCoord maxCoord, VoxTreeCoord stride) :
mRoot(0),
mVoxels(voxels),
mMinCoord(minCoord),
mMaxCoord(maxCoord),
mStride(stride)
{
   mRoot = new VoxTreeNode(minCoord, maxCoord);
   populate(mRoot);
}

template <class Voxel>
VoxTree<Voxel>::~VoxTree()
{
   delete mRoot;
}

template <class Voxel>
void VoxTree<Voxel>::populate(VoxTreeNode * node)
{
   eval(node);

   if (node->mValue == VoxTreeNode::nodeMixed)
   {
      int xpos;
      int xqual;
      int ypos;
      int yqual;
      int zpos;
      int zqual;
      findBestSplit(node, xpos, xqual, 0);
      findBestSplit(node, ypos, yqual, 1);
      findBestSplit(node, zpos, zqual, 2);

      VoxTreeCoord split(0,0,0);
      if (xqual > yqual && xqual > zqual)
      {
         split = node->mMaxCoord;
         split.x = xpos;
         node->mChildren[0] = new VoxTreeNode(node->mMinCoord, split);
         split = node->mMinCoord;
         split.x = xpos + 1;
         node->mChildren[1] = new VoxTreeNode(split, node->mMaxCoord);
      }
      else if (yqual > zqual)
      {
         split = node->mMaxCoord;
         split.y = ypos;
         node->mChildren[0] = new VoxTreeNode(node->mMinCoord, split);
         split = node->mMinCoord;
         split.y = ypos + 1;
         node->mChildren[1] = new VoxTreeNode(split, node->mMaxCoord);
      }
      else
      {
         split = node->mMaxCoord;
         split.z = zpos;
         node->mChildren[0] = new VoxTreeNode(node->mMinCoord, split);
         split = node->mMinCoord;
         split.z = zpos + 1;
         node->mChildren[1] = new VoxTreeNode(split, node->mMaxCoord);
      }

      populate(node->mChildren[0]);
      populate(node->mChildren[1]);
   }
}

template <class Voxel>
void VoxTree<Voxel>::findBestSplit(VoxTreeNode * node, int & pos, int & quality, int axis)
{
   quality = 0;
   pos = 0;
   int a0 = axis;
   int a1 = (axis + 1) % 3;
   int a2 = (axis + 2) % 3;
   VoxTreeCoord coord(0,0,0);
   for (coord.pos[a0] = node->mMinCoord.pos[a0]; coord.pos[a0] < node->mMaxCoord.pos[a0]; ++coord.pos[a0])
   {
      int qual = 0;
      for (coord.pos[a1] = node->mMinCoord.pos[a1]; coord.pos[a1] <= node->mMaxCoord.pos[a1]; ++coord.pos[a1])
      {
         for (coord.pos[a2] = node->mMinCoord.pos[a2]; coord.pos[a2] <= node->mMaxCoord.pos[a2]; ++coord.pos[a2])
         {
            Voxel * v1 = at(coord.x, coord.y, coord.z);
            Voxel * v2 = v1 + mStride.pos[a0];
            if ((*v1 == 0) != (*v2 == 0))
            {
               ++qual;
            }
         }
      }
      if (qual > quality)
      {
         pos = coord.pos[a0];
         quality = qual;
      }
   }
}

template <class Voxel>
void VoxTree<Voxel>::eval(VoxTreeNode * node)
{
   bool foundSet = false;
   bool foundUnset = false;
   for (int z = node->mMinCoord.z; z <= node->mMaxCoord.z; ++z)
   {
      for (int y = node->mMinCoord.y; y <= node->mMaxCoord.y; ++y)
      {
         for (int x = node->mMinCoord.x; x <= node->mMaxCoord.x; ++x)
         {
            if (*at(x, y, z))
            {
               foundSet = true;
            }
            else
            {
               foundUnset = true;
            }
            if (foundSet && foundUnset)
            {
               node->mValue = VoxTreeNode::nodeMixed;
               return;
            }
         }
      }
   }
   if (foundSet)
   {
      node->mValue = VoxTreeNode::nodeFull;
   }
   else
   {
      node->mValue = VoxTreeNode::nodeEmpty;
   }
}

#endif

#ifndef MOUSE_VOXTREENODE_H
#define MOUSE_VOXTREENODE_H

#include "VoxTreeCoord.h"

/**
VoxTreeNode.h
Header for class VoxTreeNode
*/
class VoxTreeNode
{
public:
   enum NodeValue
   {
      nodeUnknown,
      nodeEmpty,
      nodeFull,
      nodeMixed
   };
public:
    VoxTreeNode(VoxTreeCoord minCoord, VoxTreeCoord maxCoord);
    virtual ~VoxTreeNode();
public:
   VoxTreeCoord mMinCoord;
   VoxTreeCoord mMaxCoord;
   NodeValue mValue;
   VoxTreeNode * mChildren[2];
};

VoxTreeNode::VoxTreeNode(VoxTreeCoord minCoord, VoxTreeCoord maxCoord) :
mValue(nodeUnknown),
mMinCoord(minCoord),
mMaxCoord(maxCoord)
{
   mChildren[0] = mChildren[1] = 0;
}

VoxTreeNode::~VoxTreeNode()
{
   delete mChildren[0];
   delete mChildren[1];
}

#endif

#ifndef MOUSE_VOXTREECOORD_H
#define MOUSE_VOXTREECOORD_H

/**
VoxTreeCoord.h
Header for class VoxTreeCoord
*/
class VoxTreeCoord
{
public:
   VoxTreeCoord(int x, int y, int z) : x(x), y(y), z(z) {}
   virtual ~VoxTreeCoord() {}

   VoxTreeCoord operator+(const VoxTreeCoord & co) { return VoxTreeCoord(x + co.x, y + co.y, z + co.z); }
   VoxTreeCoord operator-(const VoxTreeCoord & co) { return VoxTreeCoord(x - co.x, y - co.y, z - co.z); }
   VoxTreeCoord operator+=(const VoxTreeCoord & co) { x += co.x; y += co.y; z += co.z; return *this; }
   VoxTreeCoord operator-=(const VoxTreeCoord & co) { x -= co.x; y -= co.y; z -= co.z; return *this; }

public:
   union
   {
      struct
      {
         int x;
         int y;
         int z;
      };
      int pos[3];
   };
};

#endif