Merge branch 'master' of /home/lavaux/Dropbox/gitRoot/CosmoToolbox

sample/testBSP.cpp

@@ -0,0 +1,12 @@
+#include "bsp_simple.hpp"
+
+int main(int argc, char **argv)
+{
+  CosmoTool::simple_bsp::BSP<int, double, 2> bsp;
+  double p[5][2] = { { 0, 0}, {1, 0}, {1, 1}, {0, 1}, {0, 0} };
+
+  for (int q = 0; q < 4; q++)
+    bsp.insert(p+q, p+q+2, q);
+
+  return 0;
+}

src/bsp_simple.hpp

@@ -0,0 +1,165 @@
+#ifndef __COSMOTOOL_SIMPLE_BSP_HPP
+#define __COSMOTOOL_SIMPLE_BSP_HPP
+
+#include <stdlib.h>
+#include <cmath>
+#include "algo.hpp"
+#include <queue>
+#include <exception>
+
+namespace CosmoTool
+{
+  
+  namespace simple_bsp
+  {
+
+    template<typename T, typename PType, int N>
+    struct space
+    {
+      typedef T data_t;
+      typedef PType point_t;
+      typedef PType coord_t[N];
+      static const int dim = N;
+    };
+
+    template<typename SType>
+    struct Plane
+    { 
+      typename SType::coord_t n;
+      typename SType::point_t d;
+    };
+
+    template<typename SType>
+    typename SType::point_t dot_product(const typename SType::coord_t& c1,
+				      const typename SType::coord_t& c2)
+    {
+      typename SType::point_t A = 0;
+
+      for(int j = 0; j < SType::dim; j++)
+	A += c1[j]*c2[j];
+      return A;
+    }
+
+    template<typename SType>
+    struct Node {
+      Plane<SType> plane;
+      Node<SType> *minus, *plus;
+      typename SType::data_t data;
+    };
+
+    template<typename SType>
+    void normal2(typename SType::coord_t p[2], typename SType::coord_t& n)
+    {
+      typename SType::point_t d;
+      using CosmoTool::square;
+
+      n[0] = p[1][1]-p[0][1];
+      n[1] = -p[1][0]+p[0][0];
+      d = std::sqrt(square(n[0])+square(n[1]));
+      n[0] /= d;
+      n[1] /= d;
+    }
+    
+    template<typename SType>
+    void normal3(typename SType::coord_t p[3], typename SType::coord_t& n)
+    {
+      typename SType::point_t delta0[3] = { p[1][0] - p[0][0], p[1][1] - p[0][1], p[1][2] - p[0][2] };
+      typename SType::point_t delta1[3] = { p[2][0] - p[0][0], p[2][1] - p[0][1], p[2][2] - p[0][2] };
+      typename SType::point_t d;
+      using CosmoTool::square;
+
+      n[0] = delta0[1] * delta1[2] - delta0[2] * delta1[1];
+      n[1] = delta0[2] * delta1[0] - delta0[0] * delta1[2];
+      n[2] = delta0[0] * delta1[1] - delta0[1] * delta1[0];
+      d = std::sqrt(square(n[0]) + square(n[1]) + square(n[2]));
+      n[0] /= d;
+      n[1] /= d;
+      n[2] /= d;
+    }
+    
+
+
+    template<typename SType>
+    struct Facet
+    {
+      typename SType::coord_t p[SType::dim];
+      typename SType::data_t data;
+
+      void center(typename SType::coord_t& c)
+      {
+	for (int j = 0; j < SType::dim; j++)
+	  {
+	    c[j] = 0;
+	    for (int i = 0; i < SType::dim; i++)
+	      {
+		c[j] += p[i][j]; 
+	      }
+	    c[j] /= SType::dim+1;
+	  }
+      }
+      
+      void normal(typename SType::coord_t& n)
+      {
+	if (SType::dim==2)
+	  {
+	    normal2<SType>(p, n);
+	    return;
+	  }
+	if (SType::dim == 3)
+	  {
+	    normal3<SType>(p, n);
+	    return;
+	  }
+	abort();
+      }
+
+    };
+
+
+    class InvalidPoint: public std::exception
+    {
+    };
+
+    template<typename T, typename PType, int N>
+    class BSP
+    {
+    public:
+      typedef space<T, PType, N> space_t;
+      typedef Plane<space_t> plane_t;
+      typedef Node<space_t> node_t;
+
+      node_t *root;
+      std::queue<node_t *> allocated;
+      
+      BSP() throw();
+      ~BSP();
+      void insert(Facet<space_t>& facet);
+
+      template<typename PIterator>
+      void insert(PIterator b, PIterator e, T data)
+      {
+	Facet<space_t> f;
+	int q = 0;
+
+	while (b != e && q < N+1)
+	  {
+	    for (int j = 0; j < N; j++)
+	      f.p[q][j] = (*b)[j];
+	    ++b;
+	    ++q;
+	  }
+	if (q != N)
+	  throw InvalidPoint();
+	
+	f.data = data;
+	insert(f);
+      }
+    };
+
+  };
+
+};
+
+#include "bsp_simple.tcc"
+
+#endif

src/bsp_simple.tcc

@@ -0,0 +1,105 @@
+#include <list>
+#include <queue>
+
+namespace CosmoTool
+{
+
+  namespace simple_bsp
+  {
+    
+    template<typename T, typename PType, int N>
+    BSP<T,PType,N>::BSP()
+     throw()
+    {
+      root = 0;
+    }
+
+    template<typename T, typename PType, int N>
+    BSP<T,PType,N>::~BSP()
+    {
+      while (!allocated.empty())
+	{
+	  node_t *r = allocated.front();
+	      
+	  allocated.pop();
+	  delete r;
+	}
+    }
+
+    template<typename T, typename PType, int N>
+    void BSP<T,PType,N>::insert(Facet<space_t>& f)
+    {
+      std::list<node_t **> subtrees;
+      std::list<node_t **> leaf_insert;
+
+      if (root != 0)
+	subtrees.push_back(&root);
+      else
+	leaf_insert.push_back(&root);
+
+      // Find the point of insertion. Do not bother to split triangle for this
+      // implementation.
+      while (!subtrees.empty())
+	{
+	  std::list<node_t **> new_subtrees;
+	  typename std::list<node_t **>::iterator iter = subtrees.begin();
+
+	  while (iter != subtrees.end())
+	    {
+	      typename space_t::point_t dp;
+	      bool cond_plus = false, cond_minus = false;
+	      node_t *current = *(*iter);
+	      
+	      for (int j = 0; j < N; j++)
+		{
+		  dp = dot_product<space_t>(f.p[j],current->plane.n) + current->plane.d;
+		  cond_plus = cond_plus || (dp > 0);
+		  cond_minus = cond_minus || (dp <= 0);
+		}
+	      
+	      bool joint = cond_plus && cond_minus;
+	      bool not_joint = (!cond_plus) && (!cond_minus);
+	      
+	      if (joint || not_joint)
+		{
+		  // Crawl and add another subtree
+		  *iter = &(current->minus);
+		  if (current->plus != 0)
+		    new_subtrees.push_back(&current->plus);
+		  else
+		    leaf_insert.push_back(&current->plus);
+		}
+	      else
+		{
+		  if (cond_plus)
+		    *iter = &current->plus;
+		  else
+		    *iter = &current->minus;
+		}
+	      if (*(*iter) == 0)
+		{
+		  leaf_insert.push_back(*iter);
+		  iter = subtrees.erase(iter);
+		}
+	      else
+		++iter;
+	    }
+	  if (!new_subtrees.empty())
+	    subtrees.splice(subtrees.end(), new_subtrees);
+	}
+      
+      node_t * current = new node_t;
+      f.normal(current->plane.n);
+      current->plane.d = -dot_product<space_t>((f.p[0]),current->plane.n);
+      
+      for (typename std::list<node_t **>::iterator i = leaf_insert.begin();
+	   i != leaf_insert.end();
+	   ++i)
+	*(*i) = current;
+
+      allocated.push(current);
+   }
+
+  };
+
+};