Initialize at openlb-1-3

1 files changed, 756 insertions, 0 deletions

diff --git a/src/io/octree.hh b/src/io/octree.hh
new file mode 100644
index 0000000..2efb083
--- /dev/null
+++ b/src/io/octree.hh
@@ -0,0 +1,756 @@
+/*  This file is part of the OpenLB library
+ *
+ *  Copyright (C) 2015 Thomas Henn
+ *  E-mail contact: info@openlb.net
+ *  The most recent release of OpenLB can be downloaded at
+ *  <http://www.openlb.net/>
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  as published by the Free Software Foundation; either version 2
+ *  of the License, or (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public
+ *  License along with this program; if not, write to the Free
+ *  Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
+ *  Boston, MA  02110-1301, USA.
+ */
+
+/** \file
+ * Octree - adapted from http://www.flipcode.com/archives/Octree_Implementation.shtml
+ */
+
+#ifndef OCTREE_HH
+#define OCTREE_HH
+
+#include <iostream>
+
+#include "core/singleton.h"
+
+using namespace olb::util;
+/// All OpenLB code is contained in this namespace.
+namespace olb {
+
+template<typename T>
+class STLmesh;
+
+template<typename T>
+struct STLtriangle;
+
+template <typename T>
+class Octree;
+
+template <typename T>
+Octree<T>::Octree(Vector<T,3> center, T rad, STLmesh<T>* mesh, short maxDepth, T overlap, Octree<T>* parent)
+  : _center(center), _radius(rad), _mesh(mesh), _maxDepth(maxDepth),_isLeaf(false), _boundaryNode(false), _inside(false), _parent(parent), _child(nullptr)
+{
+
+
+  findTriangles(overlap);
+  //  cout << _triangles.size() << std::endl;
+  if (_triangles.size() > 0 && 0 < _maxDepth) {
+
+    _child = new Octree<T>*[8];
+
+    Vector<T,3> tmpCenter = _center;
+    T tmpRad = _radius/2.;
+    tmpCenter[0] = _center[0] - tmpRad;
+    tmpCenter[1] = _center[1] - tmpRad;
+    tmpCenter[2] = _center[2] + tmpRad;
+    _child[0] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] + tmpRad;
+    tmpCenter[1] = _center[1] - tmpRad;
+    tmpCenter[2] = _center[2] + tmpRad;
+    _child[1] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] - tmpRad;
+    tmpCenter[1] = _center[1] - tmpRad;
+    tmpCenter[2] = _center[2] - tmpRad;
+    _child[2] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] + tmpRad;
+    tmpCenter[1] = _center[1] - tmpRad;
+    tmpCenter[2] = _center[2] - tmpRad;
+    _child[3] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] - tmpRad;
+    tmpCenter[1] = _center[1] + tmpRad;
+    tmpCenter[2] = _center[2] + tmpRad;
+    _child[4] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] + tmpRad;
+    tmpCenter[1] = _center[1] + tmpRad;
+    tmpCenter[2] = _center[2] + tmpRad;
+    _child[5] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] - tmpRad;
+    tmpCenter[1] = _center[1] + tmpRad;
+    tmpCenter[2] = _center[2] - tmpRad;
+    _child[6] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+    tmpCenter[0] = _center[0] + tmpRad;
+    tmpCenter[1] = _center[1] + tmpRad;
+    tmpCenter[2] = _center[2] - tmpRad;
+    _child[7] = new Octree<T>(tmpCenter, tmpRad, _mesh, _maxDepth-1, overlap, this);
+
+  } else {
+    _isLeaf = true;
+    if (_triangles.size() > 0 ) {
+      _boundaryNode = true;
+    }
+  }
+}
+
+template <typename T>
+Octree<T>::~Octree()
+{
+  if (_maxDepth!=0 && !_isLeaf) {
+    for (int i=0; i<8; i++) {
+      delete _child[i];
+    }
+    delete[] _child;
+  }
+}
+
+template <typename T>
+void Octree<T>::findTriangles(T overlap)
+{
+  if (_parent == nullptr) {
+    _triangles.reserve(_mesh->triangleSize());
+    for (unsigned int i=0; i<_mesh->triangleSize(); ++i) {
+      if (AABBTri(_mesh->getTri(i))) {
+        _triangles.push_back(i);
+      }
+    }
+  } else {
+    std::vector<unsigned int>::iterator it;
+    for (it = _parent->_triangles.begin(); it!=_parent->_triangles.end(); ++it) {
+      if (AABBTri(_mesh->getTri(*it), overlap)) {
+        _triangles.push_back(*it);
+      }
+    }
+  }
+}
+
+template <typename T>
+bool Octree<T>::AABBTri(const STLtriangle<T>& tri, T overlap)
+{
+  std::vector<T> v0(3,T()), v1(3,T()), v2(3,T()), f0(3,T()), f1(3,T()), f2(3,T()), e(3, T());
+
+  /* Test intersection cuboids - triangle
+  * Intersection test after Christer Ericson - Real time Collision Detection p.
+  * TestTriangleAABB p.171 */
+  Vector<T,3> c(_center);
+  T eps = 2e-16;
+
+  for (int j=0; j<3; j++) {
+    v0[j] = tri.point[0].r[j]-_center[j];
+    v1[j] = tri.point[1].r[j]-_center[j];
+    v2[j] = tri.point[2].r[j]-_center[j];
+    e[j] = _radius*1.01 + overlap; // + std::numeric_limits<T>::epsilon(); // *1.01;
+  }
+  for (int j=0; j<3; j++) {
+    f0[j] = v1[j] - v0[j];
+    f1[j] = v2[j] - v1[j];
+    f2[j] = v0[j] - v2[j];
+  }
+  T p0=T(), p1=T(), r=T();
+  //test a00
+  p0 = v0[2]*v1[1]-v0[1]*v1[2];
+  p1 = v2[2]*v1[1]-v2[2]*v0[1]+v0[2]*v2[1]-v1[2]*v2[1];
+  r = e[1] * std::fabs(f0[2]) + e[2]*std::fabs(f0[1]);
+  T mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a01
+  p0 = v0[1]*v1[2]-v0[1]*v2[2]-v0[2]*v1[1]+v0[2]*v2[1];
+  p1 = -v1[1]*v2[2]+v1[2]*v2[1];
+  r = e[1] * std::fabs(f1[2]) + e[2]*std::fabs(f1[1]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a02
+  p0 = v0[1]*v2[2]-v0[2]*v2[1];
+  p1 = v0[1]*v1[2]-v0[2]*v1[1]+v1[1]*v2[2]-v1[2]*v2[1];
+  r = e[1]*std::fabs(f2[2]) + e[2]*std::fabs(f2[1]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a10
+  p0 = v0[0]*v1[2]-v0[2]*v1[0];
+  p1 = v0[0]*v2[2]-v0[2]*v2[0]-v1[0]*v2[2]+v1[2]*v2[0];
+  r = e[0]*std::fabs(f0[2]) + e[2]*std::fabs(f0[0]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a11
+  p0 = -v0[0]*v1[2]+v0[0]*v2[2]+v0[2]*v1[0]-v0[2]*v2[0];
+  p1 = v1[0]*v2[2]-v1[2]*v2[0];
+  r =  (T)(e[0]*std::fabs(f1[2])+e[2]*std::fabs(f1[0]));
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a12
+  p0 = -v0[0]*v2[2]+v0[2]*v2[0];
+  p1 = -v0[0]*v1[2]+v0[2]*v1[0]-v1[0]*v2[2]+v1[2]*v2[0];
+  r = e[0]*std::fabs(f2[2])+e[2]*std::fabs(f2[0]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a20
+  p0 = -v0[0]*v1[1]+v0[1]*v1[0];
+  p1 = -v0[0]*v2[1]+v0[1]*v2[0]+v1[0]*v2[1]-v1[1]*v2[0];
+  r = e[0]*std::fabs(f0[1])+e[1]*std::fabs(f0[0]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a21
+  p0 = v0[0]*v1[1]-v0[0]*v2[1]-v0[1]*v1[0]+v0[1]*v2[0];
+  p1 = -v1[0]*v2[1]+v1[1]*v2[0];
+  r = e[0]*std::fabs(f1[1])+e[1]*std::fabs(f1[0]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  // test a22
+  p0 = v0[0]*v2[1]-v0[1]*v2[0];
+  p1 = v0[0]*v1[1]-v0[1]*v1[0]+v1[0]*v2[1]-v1[1]*v2[0];
+  r = e[0]*std::fabs(f2[1])+e[1]*std::fabs(f2[0]);
+  mmm = std::max<T>(-std::max<T>(p0, p1), std::min<T>(p0, p1));
+  if (mmm > r+eps) {
+    return false;
+  }
+
+  if (std::max(std::max(v0[0], v1[0]), v2[0]) < -e[0] || std::min(std::min(v0[0], v1[0]), v2[0]) > e[0]) {
+    return false;
+  }
+  if (std::max(std::max(v0[1], v1[1]), v2[1]) < -e[1] || std::min(std::min(v0[1], v1[1]), v2[1]) > e[1]) {
+    return false;
+  }
+  if (std::max(std::max(v0[2], v1[2]), v2[2]) < -e[2] || std::min(std::min(v0[2], v1[2]), v2[2]) > e[2]) {
+    return false;
+  }
+
+  /* Test intersection cuboids - triangle plane*/
+  r = e[0]*std::fabs(tri.normal[0]) + e[1]*std::fabs(tri.normal[1]) + e[2]*std::fabs(tri.normal[2]);
+  T s =  tri.normal[0]*c[0] + tri.normal[1]*c[1] + tri.normal[2]*c[2] - tri.d;
+  return (fabs(s) <= r);
+}
+
+template<typename T>
+Octree<T>* Octree<T>::find(const Vector<T,3>& pt,const int& maxDepth)
+{
+  //  clout << pt[0] << " " << pt[1] << " " << pt[2] << std::endl;
+
+  if (_isLeaf || maxDepth == _maxDepth) {
+    if (std::abs(_center[0] - pt[0]) < _radius + std::numeric_limits<T>::epsilon() &&
+        std::abs(_center[1] - pt[1]) < _radius + std::numeric_limits<T>::epsilon() &&
+        std::abs(_center[2] - pt[2]) < _radius + std::numeric_limits<T>::epsilon()) {
+      //       clout << pt[0] << " " << pt[1] << " " << pt[2] << std::endl;
+      return this;
+    } else {
+      OstreamManager clout(std::cout, "Octree");
+      clout << "Point: " << std::setprecision(10) << pt[0]<< " " <<pt[1]<< " " <<pt[2]<< " " <<std::endl;
+      clout << "Center: " << std::setprecision(10) << _center[0] << " " << _center[1] << " " << _center[2] << " " << std::endl;
+      clout << "Radius: "  << std::setprecision(10)<< _radius << std::endl;
+      //throw std::runtime_error("[Octree->find] Point outside of geometry.");
+      return nullptr;
+    }
+  } else {
+    if (pt[0] < _center[0]) {
+      if (pt[1] < _center[1]) {
+        if (pt[2] < _center[2]) {
+          return _child[2]->find(pt, maxDepth);
+        } else {
+          return _child[0]->find(pt, maxDepth);
+        }
+      } else {
+        if (pt[2] < _center[2]) {
+          return _child[6]->find(pt, maxDepth);
+        } else {
+          return _child[4]->find(pt, maxDepth);
+        }
+      }
+    } else {
+      if (pt[1] < _center[1]) {
+        if (pt[2] < _center[2]) {
+          return _child[3]->find(pt, maxDepth);
+        } else {
+          return _child[1]->find(pt, maxDepth);
+        }
+      } else {
+        if (pt[2] < _center[2]) {
+          return _child[7]->find(pt, maxDepth);
+        } else {
+          return _child[5]->find(pt, maxDepth);
+        }
+      }
+    }
+  }
+}
+
+template<typename T>
+int Octree<T>::testIntersection(const Vector<T,3>& pt,const Vector<T,3>& dir, bool print)
+{
+  int intersections = 0;
+  Vector<T,3> q;
+  std::vector<Vector<T,3> > qs;
+  T a;
+#ifdef OLB_DEBUG
+  if (print) {
+    OstreamManager clout(std::cout, "Octree");
+    clout << "Center: " << _center[0] << " " << _center[1] << " "<< _center[2] << " Dir: " << dir[0] << " " << dir[1] << " "<< dir[2] << std::endl;
+    clout << "Tris: ";
+    for (unsigned k=0; k<_triangles.size(); ++k) {
+      clout << _triangles[k] << " ";
+    }
+    clout << std::endl;
+  }
+#endif
+  for (unsigned k=0; k<_triangles.size(); ++k) {
+    if (_mesh->getTri(_triangles[k]).testRayIntersect(pt, dir, q, a)) {
+      if (std::fabs(_center[0]-q[0]) <= _radius + std::numeric_limits<T>::epsilon() + 1/1000. * _radius && std::fabs(_center[1]-q[1]) <= _radius + std::numeric_limits<T>::epsilon() + 1/1000. * _radius && std::fabs(_center[2]-q[2]) <= _radius + std::numeric_limits<T>::epsilon() + 1/1000. * _radius) {
+        bool newpoint=true;
+        for (unsigned i=0; i<qs.size(); i++) {
+          newpoint = ( !util::nearZero(q[0]-qs[i][0]) || !util::nearZero(q[1]-qs[i][1]) || !util::nearZero(q[2]-qs[i][2]) );
+        }
+        if (newpoint) {
+          qs.push_back(q);
+          intersections++;
+        }
+#ifdef OLB_DEBUG
+        if (print) {
+          OstreamManager clout(std::cout, "Octree");
+          clout << "Q: " << q[0] << " " <<q[1] << " "<<q[2] << " inside "<< intersections << std::endl;
+        }
+#endif
+      }
+#ifdef OLB_DEBUG
+      else if (print) {
+        OstreamManager clout(std::cout, "Octree");
+        clout << "Q: " << q[0] << " " <<q[1] << " "<<q[2] << " outside"<<std::endl;
+      }
+#endif
+    }
+  }
+#ifdef OLB_DEBUG
+  if (intersections == 0 && print) {
+    OstreamManager clout(std::cout, "Octree");
+    clout << "No Intersection found!" << std::endl;
+  }
+#endif
+  return intersections;
+}
+
+template<typename T>
+void Octree<T>::checkRay(const Vector<T,3>& pt,const Vector<T,3>& dir, unsigned short& rayInside)
+{
+  unsigned short left=0, right=0;
+  Vector<T,3> dirNormed(dir);
+  dirNormed.normalize();
+  dirNormed*= _radius * 2.;
+  Vector<T,3> q;
+  std::vector<Vector<T,3> > qs;
+  T a = 1.;
+
+  for (unsigned int k=0; k<_triangles.size(); ++k) {
+    if (_mesh->getTri(_triangles[k]).testRayIntersect(pt, dirNormed, q, a, 0.) && a<1.) {
+      bool newpoint=true;
+      for (unsigned int i=0; i<qs.size(); i++) {
+        newpoint &= ( !util::nearZero(q[0]-qs[i][0]) || !util::nearZero(q[1]-qs[i][1]) || !util::nearZero(q[2]-qs[i][2]) );
+      }
+      if (newpoint) {
+        qs.push_back(q);
+        if (a < .5) {
+          left++;
+        } else {
+          right++;
+        }
+      }
+    }
+  }
+  rayInside += left;
+  rayInside %=2;
+  setInside(rayInside);
+  rayInside += right;
+  rayInside %=2;
+}
+
+template<typename T>
+void Octree<T>::getCenterpoints(std::vector<std::vector<T> >& pts)
+{
+  if (_isLeaf) {
+    pts.push_back(_center);
+  } else {
+    for (int i=0; i<8; i++) {
+      _child[i]->getCenterpoints(pts);
+    }
+  }
+}
+
+template<typename T>
+void Octree<T>::getLeafs(std::vector<Octree<T>* >& pts)
+{
+  if (_isLeaf) {
+    pts.push_back(this);
+  } else {
+    for (int i=0; i<8; i++) {
+      _child[i]->getLeafs(pts);
+    }
+  }
+}
+
+template<typename T>
+bool Octree<T>::isLeaf()
+{
+  return _isLeaf;
+}
+
+template<typename T>
+void Octree<T>::write(const Vector<T,3>& pt,const std::string no)
+{
+  if (_triangles.size()>0 && (std::fabs(pt[0]-_center[0]) < _radius && std::fabs(pt[1]-_center[1]) < _radius && std::fabs(pt[2]-_center[2]) < _radius)) {
+    std::string fullName = singleton::directories().getVtkOutDir() + "Octree_" + no + ".stl";
+    std::ofstream f(fullName.c_str());
+    if (!f) {
+      std::cerr << "[Octree] could not open file: " << fullName << std::endl;
+    }
+    f << "solid ascii" << std::endl << std::flush;
+    std::vector<unsigned int>::iterator it = _triangles.begin();
+    for (; it != _triangles.end(); ++it) {
+      f << "facet normal" << _mesh->getTri(*it).normal[0] << " "  << _mesh->getTri(*it).normal[1] << " "  << _mesh->getTri(*it).normal[2] << " " <<std::endl;
+      f << "    outer loop\n";
+      f << "        vertex " << _mesh->getTri(*it).point[0].r[0] << " " << _mesh->getTri(*it).point[0].r[1] << " " << _mesh->getTri(*it).point[0].r[2] << "\n";
+      f << "        vertex " << _mesh->getTri(*it).point[1].r[0] << " " << _mesh->getTri(*it).point[1].r[1] << " " << _mesh->getTri(*it).point[1].r[2] << "\n";
+      f << "        vertex " << _mesh->getTri(*it).point[2].r[0] << " " << _mesh->getTri(*it).point[2].r[1] << " " << _mesh->getTri(*it).point[2].r[2] << "\n";
+      f << "    endloop\n";
+      f << "endfacet\n";
+    }
+    f.close();
+  }
+  if (!_isLeaf) {
+    for (int i=0; i<8; i++) {
+      std::stringstream istr;
+      istr << i;
+      _child[i]->write(pt, no+istr.str());
+    }
+  }
+}
+
+template<typename T>
+void Octree<T>::write(const int depth,const std::string no)
+{
+  if (_triangles.size()>0 && _maxDepth == depth) {
+    std::string fullName = singleton::directories().getVtkOutDir() + "Octree_" + no + ".stl";
+    std::ofstream f(fullName.c_str());
+    if (!f) {
+      std::cerr << "[Octree] could not open file: " << fullName << std::endl;
+    }
+    f << "solid ascii" << std::endl << std::flush;
+    std::vector<unsigned int>::iterator it = _triangles.begin();
+    for (; it != _triangles.end(); ++it) {
+      f << "facet normal" << _mesh->getTri(*it).normal[0] << " "  << _mesh->getTri(*it).normal[1] << " "  << _mesh->getTri(*it).normal[2] << " " <<std::endl;
+      f << "    outer loop\n";
+      f << "        vertex " << _mesh->getTri(*it).point[0].r[0] << " " << _mesh->getTri(*it).point[0].r[1] << " " << _mesh->getTri(*it).point[0].r[2] << "\n";
+      f << "        vertex " << _mesh->getTri(*it).point[1].r[0] << " " << _mesh->getTri(*it).point[1].r[1] << " " << _mesh->getTri(*it).point[1].r[2] << "\n";
+      f << "        vertex " << _mesh->getTri(*it).point[2].r[0] << " " << _mesh->getTri(*it).point[2].r[1] << " " << _mesh->getTri(*it).point[2].r[2] << "\n";
+      f << "    endloop\n";
+      f << "endfacet\n";
+    }
+    f.close();
+  }
+  if (!_isLeaf) {
+    for (int i=0; i<8; i++) {
+      std::stringstream istr;
+      istr << i;
+      _child[i]->write(depth, no+istr.str());
+    }
+  }
+}
+
+
+template<typename T>
+void Octree<T>::write(const std::string fName)
+{
+  int rank = 0;
+#ifdef PARALLEL_MODE_MPI
+  rank = singleton::mpi().getRank();
+#endif
+  if (rank==0) {
+    std::vector<Octree<T>* > leafs;
+    getLeafs(leafs);
+    typename std::vector<Octree<T>* >::iterator it = leafs.begin();
+
+    std::string fullName = singleton::directories().getVtkOutDir() + fName + ".vtk";
+    std::ofstream f(fullName.c_str());
+    if (!f) {
+      std::cerr << "[Particles3D] could not open file: " << fullName << std::endl;
+    }
+    f << "# vtk DataFile Version 2.0" << std::endl << std::flush;
+    f << "Octree"<< std::endl << std::flush;
+    f << "ASCII"<< std::endl << std::flush;
+    f << "DATASET UNSTRUCTURED_GRID"<< std::endl << std::flush;
+    std::stringstream points;
+    std::stringstream cells;
+    std::stringstream cell_types;
+    // std::stringstream point_data;
+    std::stringstream cell_data;
+    std::stringstream cell_leaf;
+    std::stringstream cell_boundary;
+
+    points << "POINTS " << leafs.size()*8 << " float" << std::endl;
+    cells << "CELLS " << leafs.size() << " " << leafs.size()*9 << std::endl;
+    cell_types << "CELL_TYPES " << leafs.size() << std::endl;
+    cell_data << "CELL_DATA " << leafs.size() << std::endl;
+    cell_data << "SCALARS insideout int" << std::endl;
+    cell_data << "LOOKUP_TABLE default" << std::endl;
+    cell_leaf << "SCALARS leaf int" << std::endl;
+    cell_leaf << "LOOKUP_TABLE default" << std::endl;
+    cell_boundary << "SCALARS boundary int" << std::endl;
+    cell_boundary << "LOOKUP_TABLE default" << std::endl;
+
+    Vector<T,3> center;
+    Vector<T,3> pt;
+
+    T rad;
+    int i=0;
+    for (; it != leafs.end(); ++it) {
+      center  = (*it)->getCenter();
+      rad = (*it)->getRadius();
+
+      pt[0] = center[0] - rad;
+      pt[1] = center[1] - rad;
+      pt[2] = center[2] - rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] + rad;
+      pt[1] = center[1] - rad;
+      pt[2] = center[2] - rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] - rad;
+      pt[1] = center[1] + rad;
+      pt[2] = center[2] - rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] + rad;
+      pt[1] = center[1] + rad;
+      pt[2] = center[2] - rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] - rad;
+      pt[1] = center[1] - rad;
+      pt[2] = center[2] + rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] + rad;
+      pt[1] = center[1] - rad;
+      pt[2] = center[2] + rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] - rad;
+      pt[1] = center[1] + rad;
+      pt[2] = center[2] + rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " ";
+
+      pt[0] = center[0] + rad;
+      pt[1] = center[1] + rad;
+      pt[2] = center[2] + rad;
+      points << pt[0]<< " " << pt[1]<< " " << pt[2] << " "  << std::endl;
+
+      cells << "8 ";
+      for (int j=0; j<8; j++) {
+        cells << i+j << " ";
+      }
+      i+=8;
+      cells << std::endl;
+
+      cell_types << 11 << std::endl;
+
+      cell_data << (*it)->getInside() << " "<< std::endl;
+      cell_leaf << (*it)->getMaxdepth() << " "<< std::endl;
+      cell_boundary << (*it)->getBoundaryNode() << " " << std::endl;
+    }
+
+    f << points.str() << cells.str() << cell_types.str() << cell_data.str() << cell_leaf.str() << cell_boundary.str();
+
+    // f << "POINT_DATA 0\nCELL_DATA 0\n" << std::endl;
+    f.close();
+  }
+  OstreamManager clout(std::cout, "Octree");
+  /*if (_verbose)*/ clout << "Write ... OK" << std::endl;
+}
+
+template<typename T>
+bool Octree<T>::closestIntersectionSphere(const Vector<T,3>& pt, const T& rad, const Vector<T,3>& direction, Vector<T,3>& q, T& a, STLtriangle<T>& tri)
+{
+  a = std::numeric_limits<T>::infinity();
+  T alpha = T();
+  std::vector<T> qtmp(3, T());
+  bool found = false;
+  for (unsigned int k=0; k<_triangles.size(); ++k) {
+    if (_mesh->getTri(_triangles[k]).testMovingSphereIntersect(pt, rad, direction, qtmp, alpha)) {
+      if (alpha < a) {
+        a = alpha;
+        q = qtmp;
+        found = true;
+        tri = _mesh->getTri(_triangles[k]);
+      }
+    }
+  }
+  return found;
+}
+
+template<typename T>
+bool Octree<T>::closestIntersection(const Vector<T,3>& pt, const Vector<T,3>& direction, Vector<T,3>& q, T& a, STLtriangle<T>& tri, const T& rad, bool print)
+{
+  a = std::numeric_limits<T>::infinity();
+  T alpha = T();
+  Vector<T,3> qtmp;
+  bool found = false;
+#ifdef OLB_DEBUG
+  if (print) {
+    std::cout << "Tri Size: " << _triangles.size() << std::endl;
+  }
+#endif
+
+  for (unsigned int k=0; k<_triangles.size(); ++k) {
+    if (_mesh->getTri(_triangles[k]).testRayIntersect(pt, direction, qtmp, alpha, rad)) {
+      if (print) {
+        std::cout << "Found intersection!" << std::endl;
+      }
+      if (alpha < a) {
+        a = alpha;
+        q = qtmp;
+        found = true;
+        tri = _mesh->getTri(_triangles[k]);
+      }
+    }
+  }
+  //  std::cout << a << std::endl;
+  return found;
+}
+
+template<typename T>
+bool Octree<T>::closestIntersection(const Vector<T,3>& pt, const Vector<T,3>& direction, Vector<T,3>& q, T& a)
+{
+  STLtriangle<T> tri;
+  return closestIntersection(pt, direction, q, a, tri, 0.);
+}
+
+template<typename T>
+void Octree<T>::intersectRayNode(const Vector<T,3>& pt, const Vector<T,3>& dir, Vector<T,3>& s)
+{
+  T t,d;
+  s*=T();
+  //Plane Normals outside
+
+  if (dir[0] > 0.) {
+    // n = {1, 0, 0}
+    d = _center[0] + _radius;
+    t = (d - pt[0])/dir[0];
+    s = pt + t*dir;
+    if (std::fabs(s[1]-_center[1]) < _radius && std::fabs(s[2]-_center[2]) < _radius) {
+      return;
+    }
+  } else if (dir[0] < 0.) {
+    // n = {-1, 0, 0}
+    d = _center[0] - _radius;
+    t = (d - pt[0])/dir[0];
+    s = pt + t*dir;
+    if (std::fabs(s[1]-_center[1]) < _radius && std::fabs(s[2]-_center[2]) < _radius) {
+      return;
+    }
+  }
+
+  if (dir[1] > 0.) {
+    d = _center[1] + _radius;
+    t = (d - pt[1])/dir[1];
+    s = pt + t*dir;
+    if (std::fabs(s[0]-_center[0]) < _radius && std::fabs(s[2]-_center[2]) < _radius) {
+      return;
+    }
+  } else if (dir[1] < 0.) {
+    // n = {0, 0, -1}
+    d = _center[1] - _radius;
+    t = (d - pt[1])/dir[1];
+    s = pt + t*dir;
+    if (std::fabs(s[0]-_center[0]) < _radius && std::fabs(s[2]-_center[2]) < _radius) {
+      return;
+    }
+  }
+
+  if (dir[2] > 0.) {
+    // n = {0, 0, 1}
+    d = _center[2] + _radius;
+    t = (d - pt[2])/dir[2];
+    s = pt + t*dir;
+    if (std::fabs(s[0]-_center[0]) < _radius && std::fabs(s[1]-_center[1]) < _radius) {
+      return;
+    }
+  } else if (dir[2] < 0.) {
+    // n = {0, 0, -1}
+    d = _center[2] - _radius;
+    t = (d - pt[2])/dir[2];
+    s = pt + t*dir;
+    if (std::fabs(s[0]-_center[0]) < _radius && std::fabs(s[1]-_center[1]) < _radius) {
+      return;
+    }
+  }
+}
+
+template <typename T>
+void Octree<T>::print()
+{
+  OstreamManager clout(std::cout, "Octree");
+  clout << "radius=" << _radius << "; center=(" << _center[0] << "," << _center[1] << "," << _center[2] << ")" << std::endl;
+}
+
+template <typename T>
+void Octree<T>::trianglesOnLine(const Vector<T,3>& pt1, const Vector<T,3>& pt2, std::set<unsigned int>& tris)
+{
+  tris.clear();
+  std::vector<T> line = pt2-pt1;
+  std::vector<T> s = pt1;
+  T lineNorm2 = line[0] * line[0] + line[1] * line[1] + line[2] * line[2];
+  T dist2 = T();
+  Octree<T>* node = NULL;
+  int it = 0;
+  while (dist2 < lineNorm2 && it < 50) {
+    node = find(s);
+    tris.insert(node->_triangles.begin(), node->_triangles.end());
+    node->intersectRayNode(s, line, s);
+    for (int i=0; i<3; i++) {
+      s[i] = s[i] + line[i]*_radius*0.001 /* *node->getRadius()*/;
+    }
+    it++;
+    dist2 = (pt1[0]-s[0])*(pt1[0]-s[0]) + (pt1[1]-s[1])*(pt1[1]-s[1]) +  (pt1[2]-s[2])*(pt1[2]-s[2]);
+  }
+
+
+}
+
+}
+
+#endif