source: src/tesselation.cpp@ 8bb475f

/*
 * tesselation.cpp
 *
 *  Created on: Aug 3, 2009
 *      Author: heber
 */

#include "tesselation.hpp"

// ======================================== Points on Boundary =================================

BoundaryPointSet::BoundaryPointSet()
{
  LinesCount = 0;
  Nr = -1;
}
;

BoundaryPointSet::BoundaryPointSet(TesselPoint *Walker)
{
  node = Walker;
  LinesCount = 0;
  Nr = Walker->nr;
}
;

BoundaryPointSet::~BoundaryPointSet()
{
  cout << Verbose(5) << "Erasing point nr. " << Nr << "." << endl;
  if (!lines.empty())
    cerr << "WARNING: Memory Leak! I " << *this << " am still connected to some lines." << endl;
  node = NULL;
}
;

void BoundaryPointSet::AddLine(class BoundaryLineSet *line)
{
  cout << Verbose(6) << "Adding " << *this << " to line " << *line << "."
      << endl;
  if (line->endpoints[0] == this)
    {
      lines.insert(LinePair(line->endpoints[1]->Nr, line));
    }
  else
    {
      lines.insert(LinePair(line->endpoints[0]->Nr, line));
    }
  LinesCount++;
}
;

ostream &
operator <<(ostream &ost, BoundaryPointSet &a)
{
  ost << "[" << a.Nr << "|" << a.node->Name << "]";
  return ost;
}
;

// ======================================== Lines on Boundary =================================

BoundaryLineSet::BoundaryLineSet()
{
  for (int i = 0; i < 2; i++)
    endpoints[i] = NULL;
  TrianglesCount = 0;
  Nr = -1;
}
;

BoundaryLineSet::BoundaryLineSet(class BoundaryPointSet *Point[2], int number)
{
  // set number
  Nr = number;
  // set endpoints in ascending order
  SetEndpointsOrdered(endpoints, Point[0], Point[1]);
  // add this line to the hash maps of both endpoints
  Point[0]->AddLine(this); //Taken out, to check whether we can avoid unwanted double adding.
  Point[1]->AddLine(this); //
  // clear triangles list
  TrianglesCount = 0;
  cout << Verbose(5) << "New Line with endpoints " << *this << "." << endl;
}
;

BoundaryLineSet::~BoundaryLineSet()
{
  int Numbers[2];
  Numbers[0] = endpoints[1]->Nr;
  Numbers[1] = endpoints[0]->Nr;
  for (int i = 0; i < 2; i++) {
    cout << Verbose(5) << "Erasing Line Nr. " << Nr << " in boundary point " << *endpoints[i] << "." << endl;
    // as there may be multiple lines with same endpoints, we have to go through each and find in the endpoint's line list this line set
    pair<LineMap::iterator, LineMap::iterator> erasor = endpoints[i]->lines.equal_range(Numbers[i]);
    for (LineMap::iterator Runner = erasor.first; Runner != erasor.second; Runner++)
      if ((*Runner).second == this) {
        endpoints[i]->lines.erase(Runner);
        break;
      }
    if (endpoints[i]->lines.empty()) {
      cout << Verbose(5) << *endpoints[i] << " has no more lines it's attached to, erasing." << endl;
      if (endpoints[i] != NULL) {
        delete(endpoints[i]);
        endpoints[i] = NULL;
      } else
        cerr << "ERROR: Endpoint " << i << " has already been free'd." << endl;
    } else
      cout << Verbose(5) << *endpoints[i] << " has still lines it's attached to." << endl;
  }
  if (!triangles.empty())
    cerr << "WARNING: Memory Leak! I " << *this << " am still connected to some triangles." << endl;
}
;

void
BoundaryLineSet::AddTriangle(class BoundaryTriangleSet *triangle)
{
  cout << Verbose(6) << "Add " << triangle->Nr << " to line " << *this << "."
      << endl;
  triangles.insert(TrianglePair(triangle->Nr, triangle));
  TrianglesCount++;
}
;

/** Checks whether we have a common endpoint with given \a *line.
 * \param *line other line to test
 * \return true - common endpoint present, false - not connected
 */
bool BoundaryLineSet::IsConnectedTo(class BoundaryLineSet *line)
{
  if ((endpoints[0] == line->endpoints[0]) || (endpoints[1] == line->endpoints[0]) || (endpoints[0] == line->endpoints[1]) || (endpoints[1] == line->endpoints[1]))
    return true;
  else
    return false;
};

/** Checks whether the adjacent triangles of a baseline are convex or not.
 * We sum the two angles of each normal vector with a ficticious normnal vector from this baselinbe pointing outwards.
 * If greater/equal M_PI than we are convex.
 * \param *out output stream for debugging
 * \return true - triangles are convex, false - concave or less than two triangles connected
 */
bool BoundaryLineSet::CheckConvexityCriterion(ofstream *out)
{
  Vector BaseLineCenter, BaseLineNormal, BaseLine, helper[2];
  // get the two triangles
  if (TrianglesCount != 2) {
    *out << Verbose(1) << "ERROR: Baseline " << this << " is connect to less than two triangles, Tesselation incomplete!" << endl;
    return false;
  }
  // have a normal vector on the base line pointing outwards
  *out << Verbose(3) << "INFO: " << *this << " has vectors at " << *(endpoints[0]->node->node) << " and at " << *(endpoints[1]->node->node) << "." << endl;
  BaseLineCenter.CopyVector(endpoints[0]->node->node);
  BaseLineCenter.AddVector(endpoints[1]->node->node);
  BaseLineCenter.Scale(1./2.);
  BaseLine.CopyVector(endpoints[0]->node->node);
  BaseLine.SubtractVector(endpoints[1]->node->node);
  *out << Verbose(3) << "INFO: Baseline is " << BaseLine << " and its center is at " << BaseLineCenter << "." << endl;

  BaseLineNormal.Zero();
  int i=0;
  class BoundaryPointSet *node = NULL;
  for(TriangleMap::iterator runner = triangles.begin(); runner != triangles.end(); runner++) {
    *out << Verbose(3) << "INFO: NormalVector of " << *(runner->second) << " is " << runner->second->NormalVector << "." << endl;
    BaseLineNormal.SubtractVector(&runner->second->NormalVector);   // we subtract as BaseLineNormal has to point inward in direction of [pi,2pi]
    node = runner->second->GetThirdEndpoint(this);
    if (node != NULL) {
      *out << Verbose(3) << "INFO: Third node for triangle " << *(runner->second) << " is " << *node << " at " << *(node->node->node) << "." << endl;
      helper[i].CopyVector(node->node->node);
      helper[i].SubtractVector(&BaseLineCenter);
      helper[i].MakeNormalVector(&BaseLine);  // we want to compare the triangle's heights' angles!
      *out << Verbose(4) << "INFO: Height vector with respect to baseline is " << helper[i] << "." << endl;
      i++;
    } else {
      *out << Verbose(2) << "WARNING: I cannot find third node in triangle, something's wrong." << endl;
      return true;
    }
  }
  *out << Verbose(3) << "INFO: BaselineNormal is " << BaseLineNormal << "." << endl;
  double angle = helper[0].Angle(&helper[1]);
  if (BaseLineNormal.ScalarProduct(&helper[1]) > 0) {
    angle = 2.*M_PI - angle;
  }
  *out << Verbose(3) << "The angle is " << angle << "." << endl;
  if ((angle - M_PI) > -MYEPSILON)
    return true;
  else
    return false;
}

/** Checks whether point is any of the two endpoints this line contains.
 * \param *point point to test
 * \return true - point is of the line, false - is not
 */
bool BoundaryLineSet::ContainsBoundaryPoint(class BoundaryPointSet *point)
{
  for(int i=0;i<2;i++)
    if (point == endpoints[i])
      return true;
  return false;
};

/** Returns other endpoint of the line.
 * \param *point other endpoint
 * \return NULL - if endpoint not contained in BoundaryLineSet, or pointer to BoundaryPointSet otherwise
 */
inline class BoundaryPointSet *BoundaryLineSet::GetOtherEndpoint(class BoundaryPointSet *point)
{
  if (endpoints[0] == point)
    return endpoints[1];
  else if (endpoints[1] == point)
    return endpoints[0];
  else
    return NULL;
};


ostream &
operator <<(ostream &ost, BoundaryLineSet &a)
{
  ost << "[" << a.Nr << "|" << a.endpoints[0]->node->Name << "," << a.endpoints[1]->node->Name << "]";
  return ost;
}
;

// ======================================== Triangles on Boundary =================================


BoundaryTriangleSet::BoundaryTriangleSet()
{
  for (int i = 0; i < 3; i++)
    {
      endpoints[i] = NULL;
      lines[i] = NULL;
    }
  Nr = -1;
}
;

BoundaryTriangleSet::BoundaryTriangleSet(class BoundaryLineSet *line[3], int number)
{
  // set number
  Nr = number;
  // set lines
  cout << Verbose(5) << "New triangle " << Nr << ":" << endl;
  for (int i = 0; i < 3; i++)
    {
      lines[i] = line[i];
      lines[i]->AddTriangle(this);
    }
  // get ascending order of endpoints
  map<int, class BoundaryPointSet *> OrderMap;
  for (int i = 0; i < 3; i++)
    // for all three lines
    for (int j = 0; j < 2; j++)
      { // for both endpoints
        OrderMap.insert(pair<int, class BoundaryPointSet *> (
            line[i]->endpoints[j]->Nr, line[i]->endpoints[j]));
        // and we don't care whether insertion fails
      }
  // set endpoints
  int Counter = 0;
  cout << Verbose(6) << " with end points ";
  for (map<int, class BoundaryPointSet *>::iterator runner = OrderMap.begin(); runner
      != OrderMap.end(); runner++)
    {
      endpoints[Counter] = runner->second;
      cout << " " << *endpoints[Counter];
      Counter++;
    }
  if (Counter < 3)
    {
      cerr << "ERROR! We have a triangle with only two distinct endpoints!"
          << endl;
      //exit(1);
    }
  cout << "." << endl;
}
;

BoundaryTriangleSet::~BoundaryTriangleSet()
{
  for (int i = 0; i < 3; i++) {
    cout << Verbose(5) << "Erasing triangle Nr." << Nr << endl;
    lines[i]->triangles.erase(Nr);
    if (lines[i]->triangles.empty()) {
      if (lines[i] != NULL) {
        cout << Verbose(5) << *lines[i] << " is no more attached to any triangle, erasing." << endl;
        delete (lines[i]);
        lines[i] = NULL;
      } else
        cerr << "ERROR: This line " << i << " has already been free'd." << endl;
    } else
      cout << Verbose(5) << *lines[i] << " is still attached to another triangle." << endl;
  }
}
;

/** Calculates the normal vector for this triangle.
 * Is made unique by comparison with \a OtherVector to point in the other direction.
 * \param &OtherVector direction vector to make normal vector unique.
 */
void BoundaryTriangleSet::GetNormalVector(Vector &OtherVector)
{
  // get normal vector
  NormalVector.MakeNormalVector(endpoints[0]->node->node, endpoints[1]->node->node, endpoints[2]->node->node);

  // make it always point inward (any offset vector onto plane projected onto normal vector suffices)
  if (NormalVector.Projection(&OtherVector) > 0)
    NormalVector.Scale(-1.);
};

/** Finds the point on the triangle \a *BTS the line defined by \a *MolCenter and \a *x crosses through.
 * We call Vector::GetIntersectionWithPlane() to receive the intersection point with the plane
 * This we test if it's really on the plane and whether it's inside the triangle on the plane or not.
 * The latter is done as follows: if it's really outside, then for any endpoint of the triangle and it's opposite
 * base line, the intersection between the line from endpoint to intersection and the base line will have a Vector::NormSquared()
 * smaller than the first line.
 * \param *out output stream for debugging
 * \param *MolCenter offset vector of line
 * \param *x second endpoint of line, minus \a *MolCenter is directional vector of line
 * \param *Intersection intersection on plane on return
 * \return true - \a *Intersection contains intersection on plane defined by triangle, false - zero vector if outside of triangle.
 */
bool BoundaryTriangleSet::GetIntersectionInsideTriangle(ofstream *out, Vector *MolCenter, Vector *x, Vector *Intersection)
{
  Vector CrossPoint;
  Vector helper;
  int i=0;

  if (Intersection->GetIntersectionWithPlane(out, &NormalVector, endpoints[0]->node->node, MolCenter, x)) {
    *out << Verbose(1) << "Alas! [Bronstein] failed - at least numerically - the intersection is not on the plane!" << endl;
    return false;
  }

  // Calculate cross point between one baseline and the line from the third endpoint to intersection
  do {
    CrossPoint.GetIntersectionOfTwoLinesOnPlane(out, endpoints[i%3]->node->node, endpoints[(i+1)%3]->node->node, endpoints[(i+2)%3]->node->node, Intersection);
    helper.CopyVector(endpoints[(i+1)%3]->node->node);
    helper.SubtractVector(endpoints[i%3]->node->node);
    i++;
    if (i>3)
      break;
  } while (CrossPoint.NormSquared() < MYEPSILON);
  if (i>3) {
    *out << Verbose(1) << "ERROR: Could not find any cross points, something's utterly wrong here!" << endl;
    exit(255);
  }
  CrossPoint.SubtractVector(endpoints[i%3]->node->node);

  // check whether intersection is inside or not by comparing length of intersection and length of cross point
  if ((CrossPoint.NormSquared() - helper.NormSquared()) > -MYEPSILON) { // inside
    return true;
  } else { // outside!
    Intersection->Zero();
    return false;
  }
};

/** Checks whether lines is any of the three boundary lines this triangle contains.
 * \param *line line to test
 * \return true - line is of the triangle, false - is not
 */
bool BoundaryTriangleSet::ContainsBoundaryLine(class BoundaryLineSet *line)
{
  for(int i=0;i<3;i++)
    if (line == lines[i])
      return true;
  return false;
};

/** Checks whether point is any of the three endpoints this triangle contains.
 * \param *point point to test
 * \return true - point is of the triangle, false - is not
 */
bool BoundaryTriangleSet::ContainsBoundaryPoint(class BoundaryPointSet *point)
{
  for(int i=0;i<3;i++)
    if (point == endpoints[i])
      return true;
  return false;
};

/** Checks whether three given \a *Points coincide with triangle's endpoints.
 * \param *Points[3] pointer to BoundaryPointSet
 * \return true - is the very triangle, false - is not
 */
bool BoundaryTriangleSet::IsPresentTupel(class BoundaryPointSet *Points[3])
{
  return (((endpoints[0] == Points[0])
            || (endpoints[0] == Points[1])
            || (endpoints[0] == Points[2])
          ) && (
            (endpoints[1] == Points[0])
            || (endpoints[1] == Points[1])
            || (endpoints[1] == Points[2])
          ) && (
            (endpoints[2] == Points[0])
            || (endpoints[2] == Points[1])
            || (endpoints[2] == Points[2])

          ));
};

/** Returns the endpoint which is not contained in the given \a *line.
 * \param *line baseline defining two endpoints
 * \return pointer third endpoint or NULL if line does not belong to triangle.
 */
class BoundaryPointSet *BoundaryTriangleSet::GetThirdEndpoint(class BoundaryLineSet *line)
{
  // sanity check
  if (!ContainsBoundaryLine(line))
    return NULL;
  for(int i=0;i<3;i++)
    if (!line->ContainsBoundaryPoint(endpoints[i]))
      return endpoints[i];
  // actually, that' impossible :)
  return NULL;
};

/** Calculates the center point of the triangle.
 * Is third of the sum of all endpoints.
 * \param *center central point on return.
 */
void BoundaryTriangleSet::GetCenter(Vector *center)
{
  center->Zero();
  for(int i=0;i<3;i++)
    center->AddVector(endpoints[i]->node->node);
  center->Scale(1./3.);
}

ostream &
operator <<(ostream &ost, BoundaryTriangleSet &a)
{
  ost << "[" << a.Nr << "|" << a.endpoints[0]->node->Name << ","
      << a.endpoints[1]->node->Name << "," << a.endpoints[2]->node->Name << "]";
  return ost;
}
;

// =========================================================== class TESSELPOINT ===========================================

/** Constructor of class TesselPoint.
 */
TesselPoint::TesselPoint()
{
  node = NULL;
  nr = -1;
  Name =  NULL;
};

/** Destructor for class TesselPoint.
 */
TesselPoint::~TesselPoint()
{
  Free((void **)&Name, "TesselPoint::~TesselPoint: *Name");
};

/** Prints LCNode to screen.
 */
ostream & operator << (ostream &ost, const TesselPoint &a)
{
  ost << "[" << (a.Name) << "|" << &a << "]";
  return ost;
};


// =========================================================== class POINTCLOUD ============================================

/** Constructor of class PointCloud.
 */
PointCloud::PointCloud()
{

};

/** Destructor for class PointCloud.
 */
PointCloud::~PointCloud()
{

};

// ============================ CandidateForTesselation =============================

/** Constructor of class CandidateForTesselation.
 */
CandidateForTesselation::CandidateForTesselation(TesselPoint *candidate, BoundaryLineSet* line, Vector OptCandidateCenter, Vector OtherOptCandidateCenter) {
  point = candidate;
  BaseLine = line;
  OptCenter.CopyVector(&OptCandidateCenter);
  OtherOptCenter.CopyVector(&OtherOptCandidateCenter);
};

/** Destructor for class CandidateForTesselation.
 */
CandidateForTesselation::~CandidateForTesselation() {
  point = NULL;
  BaseLine = NULL;
};

// =========================================================== class TESSELATION ===========================================

/** Constructor of class Tesselation.
 */
Tesselation::Tesselation()
{
  PointsOnBoundaryCount = 0;
  LinesOnBoundaryCount = 0;
  TrianglesOnBoundaryCount = 0;
}
;

/** Destructor of class Tesselation.
 * We have to free all points, lines and triangles.
 */
Tesselation::~Tesselation()
{
  cout << Verbose(1) << "Free'ing TesselStruct ... " << endl;
  for (TriangleMap::iterator runner = TrianglesOnBoundary.begin(); runner != TrianglesOnBoundary.end(); runner++) {
    if (runner->second != NULL) {
      delete (runner->second);
      runner->second = NULL;
    } else
      cerr << "ERROR: The triangle " << runner->first << " has already been free'd." << endl;
  }
}
;

/** Gueses first starting triangle of the convex envelope.
 * We guess the starting triangle by taking the smallest distance between two points and looking for a fitting third.
 * \param *out output stream for debugging
 * \param PointsOnBoundary set of boundary points defining the convex envelope of the cluster
 */
void
Tesselation::GuessStartingTriangle(ofstream *out)
{
  // 4b. create a starting triangle
  // 4b1. create all distances
  DistanceMultiMap DistanceMMap;
  double distance, tmp;
  Vector PlaneVector, TrialVector;
  PointMap::iterator A, B, C; // three nodes of the first triangle
  A = PointsOnBoundary.begin(); // the first may be chosen arbitrarily

  // with A chosen, take each pair B,C and sort
  if (A != PointsOnBoundary.end())
    {
      B = A;
      B++;
      for (; B != PointsOnBoundary.end(); B++)
        {
          C = B;
          C++;
          for (; C != PointsOnBoundary.end(); C++)
            {
              tmp = A->second->node->node->DistanceSquared(B->second->node->node);
              distance = tmp * tmp;
              tmp = A->second->node->node->DistanceSquared(C->second->node->node);
              distance += tmp * tmp;
              tmp = B->second->node->node->DistanceSquared(C->second->node->node);
              distance += tmp * tmp;
              DistanceMMap.insert(DistanceMultiMapPair(distance, pair<PointMap::iterator, PointMap::iterator> (B, C)));
            }
        }
    }
  //    // listing distances
  //    *out << Verbose(1) << "Listing DistanceMMap:";
  //    for(DistanceMultiMap::iterator runner = DistanceMMap.begin(); runner != DistanceMMap.end(); runner++) {
  //      *out << " " << runner->first << "(" << *runner->second.first->second << ", " << *runner->second.second->second << ")";
  //    }
  //    *out << endl;
  // 4b2. pick three baselines forming a triangle
  // 1. we take from the smallest sum of squared distance as the base line BC (with peak A) onward as the triangle candidate
  DistanceMultiMap::iterator baseline = DistanceMMap.begin();
  for (; baseline != DistanceMMap.end(); baseline++)
    {
      // we take from the smallest sum of squared distance as the base line BC (with peak A) onward as the triangle candidate
      // 2. next, we have to check whether all points reside on only one side of the triangle
      // 3. construct plane vector
      PlaneVector.MakeNormalVector(A->second->node->node,
          baseline->second.first->second->node->node,
          baseline->second.second->second->node->node);
      *out << Verbose(2) << "Plane vector of candidate triangle is ";
      PlaneVector.Output(out);
      *out << endl;
      // 4. loop over all points
      double sign = 0.;
      PointMap::iterator checker = PointsOnBoundary.begin();
      for (; checker != PointsOnBoundary.end(); checker++)
        {
          // (neglecting A,B,C)
          if ((checker == A) || (checker == baseline->second.first) || (checker
              == baseline->second.second))
            continue;
          // 4a. project onto plane vector
          TrialVector.CopyVector(checker->second->node->node);
          TrialVector.SubtractVector(A->second->node->node);
          distance = TrialVector.Projection(&PlaneVector);
          if (fabs(distance) < 1e-4) // we need to have a small epsilon around 0 which is still ok
            continue;
          *out << Verbose(3) << "Projection of " << checker->second->node->Name
              << " yields distance of " << distance << "." << endl;
          tmp = distance / fabs(distance);
          // 4b. Any have different sign to than before? (i.e. would lie outside convex hull with this starting triangle)
          if ((sign != 0) && (tmp != sign))
            {
              // 4c. If so, break 4. loop and continue with next candidate in 1. loop
              *out << Verbose(2) << "Current candidates: "
                  << A->second->node->Name << ","
                  << baseline->second.first->second->node->Name << ","
                  << baseline->second.second->second->node->Name << " leaves "
                  << checker->second->node->Name << " outside the convex hull."
                  << endl;
              break;
            }
          else
            { // note the sign for later
              *out << Verbose(2) << "Current candidates: "
                  << A->second->node->Name << ","
                  << baseline->second.first->second->node->Name << ","
                  << baseline->second.second->second->node->Name << " leave "
                  << checker->second->node->Name << " inside the convex hull."
                  << endl;
              sign = tmp;
            }
          // 4d. Check whether the point is inside the triangle (check distance to each node
          tmp = checker->second->node->node->DistanceSquared(A->second->node->node);
          int innerpoint = 0;
          if ((tmp < A->second->node->node->DistanceSquared(
              baseline->second.first->second->node->node)) && (tmp
              < A->second->node->node->DistanceSquared(
                  baseline->second.second->second->node->node)))
            innerpoint++;
          tmp = checker->second->node->node->DistanceSquared(
              baseline->second.first->second->node->node);
          if ((tmp < baseline->second.first->second->node->node->DistanceSquared(
              A->second->node->node)) && (tmp
              < baseline->second.first->second->node->node->DistanceSquared(
                  baseline->second.second->second->node->node)))
            innerpoint++;
          tmp = checker->second->node->node->DistanceSquared(
              baseline->second.second->second->node->node);
          if ((tmp < baseline->second.second->second->node->node->DistanceSquared(
              baseline->second.first->second->node->node)) && (tmp
              < baseline->second.second->second->node->node->DistanceSquared(
                  A->second->node->node)))
            innerpoint++;
          // 4e. If so, break 4. loop and continue with next candidate in 1. loop
          if (innerpoint == 3)
            break;
        }
      // 5. come this far, all on same side? Then break 1. loop and construct triangle
      if (checker == PointsOnBoundary.end())
        {
          *out << "Looks like we have a candidate!" << endl;
          break;
        }
    }
  if (baseline != DistanceMMap.end())
    {
      BPS[0] = baseline->second.first->second;
      BPS[1] = baseline->second.second->second;
      BLS[0] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
      BPS[0] = A->second;
      BPS[1] = baseline->second.second->second;
      BLS[1] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
      BPS[0] = baseline->second.first->second;
      BPS[1] = A->second;
      BLS[2] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);

      // 4b3. insert created triangle
      BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
      TrianglesOnBoundary.insert(TrianglePair(TrianglesOnBoundaryCount, BTS));
      TrianglesOnBoundaryCount++;
      for (int i = 0; i < NDIM; i++)
        {
          LinesOnBoundary.insert(LinePair(LinesOnBoundaryCount, BTS->lines[i]));
          LinesOnBoundaryCount++;
        }

      *out << Verbose(1) << "Starting triangle is " << *BTS << "." << endl;
    }
  else
    {
      *out << Verbose(1) << "No starting triangle found." << endl;
      exit(255);
    }
}
;

/** Tesselates the convex envelope of a cluster from a single starting triangle.
 * The starting triangle is made out of three baselines. Each line in the final tesselated cluster may belong to at most
 * 2 triangles. Hence, we go through all current lines:
 * -# if the lines contains to only one triangle
 * -# We search all points in the boundary
 *    -# if the triangle is in forward direction of the baseline (at most 90 degrees angle between vector orthogonal to
 *       baseline in triangle plane pointing out of the triangle and normal vector of new triangle)
 *    -# if the triangle with the baseline and the current point has the smallest of angles (comparison between normal vectors)
 *    -# then we have a new triangle, whose baselines we again add (or increase their TriangleCount)
 * \param *out output stream for debugging
 * \param *configuration for IsAngstroem
 * \param *cloud cluster of points
 */
void Tesselation::TesselateOnBoundary(ofstream *out, PointCloud *cloud)
{
  bool flag;
  PointMap::iterator winner;
  class BoundaryPointSet *peak = NULL;
  double SmallestAngle, TempAngle;
  Vector NormalVector, VirtualNormalVector, CenterVector, TempVector, helper, PropagationVector, *Center = NULL;
  LineMap::iterator LineChecker[2];

  Center = cloud->GetCenter(out);
  // create a first tesselation with the given BoundaryPoints
  do {
    flag = false;
    for (LineMap::iterator baseline = LinesOnBoundary.begin(); baseline != LinesOnBoundary.end(); baseline++)
      if (baseline->second->TrianglesCount == 1) {
        // 5a. go through each boundary point if not _both_ edges between either endpoint of the current line and this point exist (and belong to 2 triangles)
        SmallestAngle = M_PI;

        // get peak point with respect to this base line's only triangle
        BTS = baseline->second->triangles.begin()->second; // there is only one triangle so far
        *out << Verbose(2) << "Current baseline is between " << *(baseline->second) << "." << endl;
        for (int i = 0; i < 3; i++)
          if ((BTS->endpoints[i] != baseline->second->endpoints[0]) && (BTS->endpoints[i] != baseline->second->endpoints[1]))
            peak = BTS->endpoints[i];
        *out << Verbose(3) << " and has peak " << *peak << "." << endl;

        // prepare some auxiliary vectors
        Vector BaseLineCenter, BaseLine;
        BaseLineCenter.CopyVector(baseline->second->endpoints[0]->node->node);
        BaseLineCenter.AddVector(baseline->second->endpoints[1]->node->node);
        BaseLineCenter.Scale(1. / 2.); // points now to center of base line
        BaseLine.CopyVector(baseline->second->endpoints[0]->node->node);
        BaseLine.SubtractVector(baseline->second->endpoints[1]->node->node);

        // offset to center of triangle
        CenterVector.Zero();
        for (int i = 0; i < 3; i++)
          CenterVector.AddVector(BTS->endpoints[i]->node->node);
        CenterVector.Scale(1. / 3.);
        *out << Verbose(4) << "CenterVector of base triangle is " << CenterVector << endl;

        // normal vector of triangle
        NormalVector.CopyVector(Center);
        NormalVector.SubtractVector(&CenterVector);
        BTS->GetNormalVector(NormalVector);
        NormalVector.CopyVector(&BTS->NormalVector);
        *out << Verbose(4) << "NormalVector of base triangle is " << NormalVector << endl;

        // vector in propagation direction (out of triangle)
        // project center vector onto triangle plane (points from intersection plane-NormalVector to plane-CenterVector intersection)
        PropagationVector.MakeNormalVector(&BaseLine, &NormalVector);
        TempVector.CopyVector(&CenterVector);
        TempVector.SubtractVector(baseline->second->endpoints[0]->node->node); // TempVector is vector on triangle plane pointing from one baseline egde towards center!
        //*out << Verbose(2) << "Projection of propagation onto temp: " << PropagationVector.Projection(&TempVector) << "." << endl;
        if (PropagationVector.Projection(&TempVector) > 0) // make sure normal propagation vector points outward from baseline
          PropagationVector.Scale(-1.);
        *out << Verbose(4) << "PropagationVector of base triangle is " << PropagationVector << endl;
        winner = PointsOnBoundary.end();

        // loop over all points and calculate angle between normal vector of new and present triangle
        for (PointMap::iterator target = PointsOnBoundary.begin(); target != PointsOnBoundary.end(); target++) {
          if ((target->second != baseline->second->endpoints[0]) && (target->second != baseline->second->endpoints[1])) { // don't take the same endpoints
            *out << Verbose(3) << "Target point is " << *(target->second) << ":" << endl;

            // first check direction, so that triangles don't intersect
            VirtualNormalVector.CopyVector(target->second->node->node);
            VirtualNormalVector.SubtractVector(&BaseLineCenter); // points from center of base line to target
            VirtualNormalVector.ProjectOntoPlane(&NormalVector);
            TempAngle = VirtualNormalVector.Angle(&PropagationVector);
            *out << Verbose(4) << "VirtualNormalVector is " << VirtualNormalVector << " and PropagationVector is " << PropagationVector << "." << endl;
            if (TempAngle > (M_PI/2.)) { // no bends bigger than Pi/2 (90 degrees)
              *out << Verbose(4) << "Angle on triangle plane between propagation direction and base line to " << *(target->second) << " is " << TempAngle << ", bad direction!" << endl;
              continue;
            } else
              *out << Verbose(4) << "Angle on triangle plane between propagation direction and base line to " << *(target->second) << " is " << TempAngle << ", good direction!" << endl;

            // check first and second endpoint (if any connecting line goes to target has at least not more than 1 triangle)
            LineChecker[0] = baseline->second->endpoints[0]->lines.find(target->first);
            LineChecker[1] = baseline->second->endpoints[1]->lines.find(target->first);
            if (((LineChecker[0] != baseline->second->endpoints[0]->lines.end()) && (LineChecker[0]->second->TrianglesCount == 2))) {
              *out << Verbose(4) << *(baseline->second->endpoints[0]) << " has line " << *(LineChecker[0]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[0]->second->TrianglesCount << " triangles." << endl;
              continue;
            }
            if (((LineChecker[1] != baseline->second->endpoints[1]->lines.end()) && (LineChecker[1]->second->TrianglesCount == 2))) {
              *out << Verbose(4) << *(baseline->second->endpoints[1]) << " has line " << *(LineChecker[1]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[1]->second->TrianglesCount << " triangles." << endl;
              continue;
            }

            // check whether the envisaged triangle does not already exist (if both lines exist and have same endpoint)
            if ((((LineChecker[0] != baseline->second->endpoints[0]->lines.end()) && (LineChecker[1] != baseline->second->endpoints[1]->lines.end()) && (GetCommonEndpoint(LineChecker[0]->second, LineChecker[1]->second) == peak)))) {
              *out << Verbose(4) << "Current target is peak!" << endl;
              continue;
            }

            // check for linear dependence
            TempVector.CopyVector(baseline->second->endpoints[0]->node->node);
            TempVector.SubtractVector(target->second->node->node);
            helper.CopyVector(baseline->second->endpoints[1]->node->node);
            helper.SubtractVector(target->second->node->node);
            helper.ProjectOntoPlane(&TempVector);
            if (fabs(helper.NormSquared()) < MYEPSILON) {
              *out << Verbose(4) << "Chosen set of vectors is linear dependent." << endl;
              continue;
            }

            // in case NOT both were found, create virtually this triangle, get its normal vector, calculate angle
            flag = true;
            VirtualNormalVector.MakeNormalVector(baseline->second->endpoints[0]->node->node, baseline->second->endpoints[1]->node->node, target->second->node->node);
            TempVector.CopyVector(baseline->second->endpoints[0]->node->node);
            TempVector.AddVector(baseline->second->endpoints[1]->node->node);
            TempVector.AddVector(target->second->node->node);
            TempVector.Scale(1./3.);
            TempVector.SubtractVector(Center);
            // make it always point outward
            if (VirtualNormalVector.Projection(&TempVector) < 0)
              VirtualNormalVector.Scale(-1.);
            // calculate angle
            TempAngle = NormalVector.Angle(&VirtualNormalVector);
            *out << Verbose(4) << "NormalVector is " << VirtualNormalVector << " and the angle is " << TempAngle << "." << endl;
            if ((SmallestAngle - TempAngle) > MYEPSILON) { // set to new possible winner
              SmallestAngle = TempAngle;
              winner = target;
              *out << Verbose(4) << "New winner " << *winner->second->node << " due to smaller angle between normal vectors." << endl;
            } else if (fabs(SmallestAngle - TempAngle) < MYEPSILON) { // check the angle to propagation, both possible targets are in one plane! (their normals have same angle)
              // hence, check the angles to some normal direction from our base line but in this common plane of both targets...
              helper.CopyVector(target->second->node->node);
              helper.SubtractVector(&BaseLineCenter);
              helper.ProjectOntoPlane(&BaseLine);
              // ...the one with the smaller angle is the better candidate
              TempVector.CopyVector(target->second->node->node);
              TempVector.SubtractVector(&BaseLineCenter);
              TempVector.ProjectOntoPlane(&VirtualNormalVector);
              TempAngle = TempVector.Angle(&helper);
              TempVector.CopyVector(winner->second->node->node);
              TempVector.SubtractVector(&BaseLineCenter);
              TempVector.ProjectOntoPlane(&VirtualNormalVector);
              if (TempAngle < TempVector.Angle(&helper)) {
                TempAngle = NormalVector.Angle(&VirtualNormalVector);
                SmallestAngle = TempAngle;
                winner = target;
                *out << Verbose(4) << "New winner " << *winner->second->node << " due to smaller angle " << TempAngle << " to propagation direction." << endl;
              } else
                *out << Verbose(4) << "Keeping old winner " << *winner->second->node << " due to smaller angle to propagation direction." << endl;
            } else
              *out << Verbose(4) << "Keeping old winner " << *winner->second->node << " due to smaller angle between normal vectors." << endl;
          }
        } // end of loop over all boundary points

        // 5b. The point of the above whose triangle has the greatest angle with the triangle the current line belongs to (it only belongs to one, remember!): New triangle
        if (winner != PointsOnBoundary.end()) {
          *out << Verbose(2) << "Winning target point is " << *(winner->second) << " with angle " << SmallestAngle << "." << endl;
          // create the lins of not yet present
          BLS[0] = baseline->second;
          // 5c. add lines to the line set if those were new (not yet part of a triangle), delete lines that belong to two triangles)
          LineChecker[0] = baseline->second->endpoints[0]->lines.find(winner->first);
          LineChecker[1] = baseline->second->endpoints[1]->lines.find(winner->first);
          if (LineChecker[0] == baseline->second->endpoints[0]->lines.end()) { // create
            BPS[0] = baseline->second->endpoints[0];
            BPS[1] = winner->second;
            BLS[1] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
            LinesOnBoundary.insert(LinePair(LinesOnBoundaryCount, BLS[1]));
            LinesOnBoundaryCount++;
          } else
            BLS[1] = LineChecker[0]->second;
          if (LineChecker[1] == baseline->second->endpoints[1]->lines.end()) { // create
            BPS[0] = baseline->second->endpoints[1];
            BPS[1] = winner->second;
            BLS[2] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
            LinesOnBoundary.insert(LinePair(LinesOnBoundaryCount, BLS[2]));
            LinesOnBoundaryCount++;
          } else
            BLS[2] = LineChecker[1]->second;
          BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
          BTS->GetCenter(&helper);
          helper.SubtractVector(Center);
          helper.Scale(-1);
          BTS->GetNormalVector(helper);
          TrianglesOnBoundary.insert(TrianglePair(TrianglesOnBoundaryCount, BTS));
          TrianglesOnBoundaryCount++;
        } else {
          *out << Verbose(1) << "I could not determine a winner for this baseline " << *(baseline->second) << "." << endl;
        }

        // 5d. If the set of lines is not yet empty, go to 5. and continue
      } else
        *out << Verbose(2) << "Baseline candidate " << *(baseline->second) << " has a triangle count of " << baseline->second->TrianglesCount << "." << endl;
  } while (flag);

  // exit
  delete(Center);
};

/** Inserts all points outside of the tesselated surface into it by adding new triangles.
 * \param *out output stream for debugging
 * \param *cloud cluster of points
 * \param *LC LinkedCell structure to find nearest point quickly
 * \return true - all straddling points insert, false - something went wrong
 */
bool Tesselation::InsertStraddlingPoints(ofstream *out, PointCloud *cloud, LinkedCell *LC)
{
  Vector Intersection;
  TesselPoint *Walker = NULL;
  Vector *Center = cloud->GetCenter(out);
  list<BoundaryTriangleSet*> *triangles = NULL;

  *out << Verbose(1) << "Begin of InsertStraddlingPoints" << endl;

  cloud->GoToFirst();
  while (!cloud->IsEnd()) {  // we only have to go once through all points, as boundary can become only bigger
    Walker = cloud->GetPoint();
    *out << Verbose(2) << "Current point is " << *Walker << "." << endl;
    // get the next triangle
    triangles = FindClosestTrianglesToPoint(out, Walker->node, LC);
    if (triangles == NULL) {
      *out << Verbose(1) << "No triangles found, probably a tesselation point itself." << endl;
      cloud->GoToNext();
      continue;
    } else {
      BTS = triangles->front();
    }
    *out << Verbose(2) << "Closest triangle is " << BTS << "." << endl;
    // get the intersection point
    if (BTS->GetIntersectionInsideTriangle(out, Center, Walker->node, &Intersection)) {
      *out << Verbose(2) << "We have an intersection at " << Intersection << "." << endl;
      // we have the intersection, check whether in- or outside of boundary
      if ((Center->DistanceSquared(Walker->node) - Center->DistanceSquared(&Intersection)) < -MYEPSILON) {
        // inside, next!
        *out << Verbose(4) << Walker << " is inside wrt triangle " << BTS << "." << endl;
      } else {
        // outside!
        *out << Verbose(3) << Walker << " is outside wrt triangle " << BTS << "." << endl;
        class BoundaryLineSet *OldLines[3], *NewLines[3];
        class BoundaryPointSet *OldPoints[3], *NewPoint;
        // store the three old lines and old points
        for (int i=0;i<3;i++) {
          OldLines[i] = BTS->lines[i];
          OldPoints[i] = BTS->endpoints[i];
        }
        // add Walker to boundary points
        AddPoint(Walker);
        if (BPS[0] == NULL)
          NewPoint = BPS[0];
        else
          continue;
        // remove triangle
        TrianglesOnBoundary.erase(BTS->Nr);
        // create three new boundary lines
        for (int i=0;i<3;i++) {
          BPS[0] = NewPoint;
          BPS[1] = OldPoints[i];
          NewLines[i] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
          LinesOnBoundary.insert(LinePair(LinesOnBoundaryCount, NewLines[i])); // no need for check for unique insertion as BPS[0] is definitely a new one
          LinesOnBoundaryCount++;
        }
        // create three new triangle with new point
        for (int i=0;i<3;i++) { // find all baselines
          BLS[0] = OldLines[i];
          int n = 1;
          for (int j=0;j<3;j++) {
            if (NewLines[j]->IsConnectedTo(BLS[0])) {
              if (n>2) {
                *out << Verbose(1) << "ERROR: " << BLS[0] << " connects to all of the new lines?!" << endl;
                return false;
              } else
                BLS[n++] = NewLines[j];
            }
          }
          // create the triangle
          BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
          TrianglesOnBoundary.insert(TrianglePair(TrianglesOnBoundaryCount, BTS));
          TrianglesOnBoundaryCount++;
        }
      }
    } else { // something is wrong with FindClosestTriangleToPoint!
      *out << Verbose(1) << "ERROR: The closest triangle did not produce an intersection!" << endl;
      return false;
    }
    cloud->GoToNext();
  }

  // exit
  delete(Center);
  *out << Verbose(1) << "End of InsertStraddlingPoints" << endl;
  return true;
};

/** Adds an point to the tesselation::PointsOnBoundary list.
 * \param *Walker point to add
 */
void
Tesselation::AddPoint(TesselPoint *Walker)
{
  PointTestPair InsertUnique;
  BPS[0] = new class BoundaryPointSet(Walker);
  InsertUnique = PointsOnBoundary.insert(PointPair(Walker->nr, BPS[0]));
  if (InsertUnique.second) // if new point was not present before, increase counter
    PointsOnBoundaryCount++;
  else {
    delete(BPS[0]);
    BPS[0] = NULL;
  }
}
;

/** Adds point to Tesselation::PointsOnBoundary if not yet present.
 * Tesselation::TPS is set to either this new BoundaryPointSet or to the existing one of not unique.
 * @param Candidate point to add
 * @param n index for this point in Tesselation::TPS array
 */
void
Tesselation::AddTrianglePoint(TesselPoint* Candidate, int n)
{
  PointTestPair InsertUnique;
  TPS[n] = new class BoundaryPointSet(Candidate);
  InsertUnique = PointsOnBoundary.insert(PointPair(Candidate->nr, TPS[n]));
  if (InsertUnique.second) { // if new point was not present before, increase counter
    PointsOnBoundaryCount++;
  } else {
    delete TPS[n];
    cout << Verbose(3) << "Node " << *((InsertUnique.first)->second->node) << " is already present in PointsOnBoundary." << endl;
    TPS[n] = (InsertUnique.first)->second;
  }
}
;

/** Function tries to add line from current Points in BPS to BoundaryLineSet.
 * If successful it raises the line count and inserts the new line into the BLS,
 * if unsuccessful, it writes the line which had been present into the BLS, deleting the new constructed one.
 * @param *a first endpoint
 * @param *b second endpoint
 * @param n index of Tesselation::BLS giving the line with both endpoints
 */
void Tesselation::AddTriangleLine(class BoundaryPointSet *a, class BoundaryPointSet *b, int n) {
  bool insertNewLine = true;

  if (a->lines.find(b->node->nr) != a->lines.end()) {
    LineMap::iterator FindLine;
    pair<LineMap::iterator,LineMap::iterator> FindPair;
    FindPair = a->lines.equal_range(b->node->nr);

    for (FindLine = FindPair.first; FindLine != FindPair.second; ++FindLine) {
      // If there is a line with less than two attached triangles, we don't need a new line.
      if (FindLine->second->TrianglesCount < 2) {
        insertNewLine = false;
        cout << Verbose(3) << "Using existing line " << *FindLine->second << endl;

        BPS[0] = FindLine->second->endpoints[0];
        BPS[1] = FindLine->second->endpoints[1];
        BLS[n] = FindLine->second;

        break;
      }
    }
  }

  if (insertNewLine) {
    AlwaysAddTriangleLine(a, b, n);
  }
}
;

/**
 * Adds lines from each of the current points in the BPS to BoundaryLineSet.
 * Raises the line count and inserts the new line into the BLS.
 *
 * @param *a first endpoint
 * @param *b second endpoint
 * @param n index of Tesselation::BLS giving the line with both endpoints
 */
void Tesselation::AlwaysAddTriangleLine(class BoundaryPointSet *a, class BoundaryPointSet *b, int n)
{
  cout << Verbose(3) << "Adding line between " << *(a->node) << " and " << *(b->node) << "." << endl;
  BPS[0] = a;
  BPS[1] = b;
  BLS[n] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);  // this also adds the line to the local maps
  // add line to global map
  LinesOnBoundary.insert(LinePair(LinesOnBoundaryCount, BLS[n]));
  // increase counter
  LinesOnBoundaryCount++;
};

/** Function tries to add Triangle just created to Triangle and remarks if already existent (Failure of algorithm).
 * Furthermore it adds the triangle to all of its lines, in order to recognize those which are saturated later.
 */
void
Tesselation::AddTriangle()
{
  cout << Verbose(1) << "Adding triangle to global TrianglesOnBoundary map." << endl;

  // add triangle to global map
  TrianglesOnBoundary.insert(TrianglePair(TrianglesOnBoundaryCount, BTS));
  TrianglesOnBoundaryCount++;

  // NOTE: add triangle to local maps is done in constructor of BoundaryTriangleSet
}
;

/** Checks whether the triangle consisting of the three points is already present.
 * Searches for the points in Tesselation::PointsOnBoundary and checks their
 * lines. If any of the three edges already has two triangles attached, false is
 * returned.
 * \param *out output stream for debugging
 * \param *Candidates endpoints of the triangle candidate
 * \return integer 0 if no triangle exists, 1 if one triangle exists, 2 if two
 *                 triangles exist which is the maximum for three points
 */
int Tesselation::CheckPresenceOfTriangle(ofstream *out, TesselPoint *Candidates[3]) {
  int adjacentTriangleCount = 0;
  class BoundaryPointSet *Points[3];

  *out << Verbose(2) << "Begin of CheckPresenceOfTriangle" << endl;
  // builds a triangle point set (Points) of the end points
  for (int i = 0; i < 3; i++) {
    PointMap::iterator FindPoint = PointsOnBoundary.find(Candidates[i]->nr);
    if (FindPoint != PointsOnBoundary.end()) {
      Points[i] = FindPoint->second;
    } else {
      Points[i] = NULL;
    }
  }

  // checks lines between the points in the Points for their adjacent triangles
  for (int i = 0; i < 3; i++) {
    if (Points[i] != NULL) {
      for (int j = i; j < 3; j++) {
        if (Points[j] != NULL) {
          LineMap::iterator FindLine = Points[i]->lines.find(Points[j]->node->nr);
          for (; (FindLine != Points[i]->lines.end()) && (FindLine->first == Points[j]->node->nr); FindLine++) {
            TriangleMap *triangles = &FindLine->second->triangles;
            *out << Verbose(3) << "Current line is " << FindLine->first << ": " << *(FindLine->second) << " with triangles " << triangles << "." << endl;
            for (TriangleMap::iterator FindTriangle = triangles->begin(); FindTriangle != triangles->end(); FindTriangle++) {
              if (FindTriangle->second->IsPresentTupel(Points)) {
                adjacentTriangleCount++;
              }
            }
            *out << Verbose(3) << "end." << endl;
          }
          // Only one of the triangle lines must be considered for the triangle count.
          *out << Verbose(2) << "Found " << adjacentTriangleCount << " adjacent triangles for the point set." << endl;
          return adjacentTriangleCount;
        }
      }
    }
  }

  *out << Verbose(2) << "Found " << adjacentTriangleCount << " adjacent triangles for the point set." << endl;
  *out << Verbose(2) << "End of CheckPresenceOfTriangle" << endl;
  return adjacentTriangleCount;
};


/** Finds the starting triangle for find_non_convex_border().
 * Looks at the outermost point per axis, then Find_second_point_for_Tesselation()
 * for the second and Find_next_suitable_point_via_Angle_of_Sphere() for the third
 * point are called.
 * \param *out output stream for debugging
 * \param RADIUS radius of virtual rolling sphere
 * \param *LC LinkedCell structure with neighbouring TesselPoint's
 */
void Tesselation::Find_starting_triangle(ofstream *out, const double RADIUS, LinkedCell *LC)
{
  cout << Verbose(1) << "Begin of Find_starting_triangle\n";
  int i = 0;
  LinkedNodes *List = NULL;
  TesselPoint* FirstPoint = NULL;
  TesselPoint* SecondPoint = NULL;
  TesselPoint* MaxPoint[NDIM];
  double max_coordinate[NDIM];
  Vector Oben;
  Vector helper;
  Vector Chord;
  Vector SearchDirection;

  Oben.Zero();

  for (i = 0; i < 3; i++) {
    MaxPoint[i] = NULL;
    max_coordinate[i] = -1;
  }

  // 1. searching topmost point with respect to each axis
  for (int i=0;i<NDIM;i++) { // each axis
    LC->n[i] = LC->N[i]-1; // current axis is topmost cell
    for (LC->n[(i+1)%NDIM]=0;LC->n[(i+1)%NDIM]<LC->N[(i+1)%NDIM];LC->n[(i+1)%NDIM]++)
      for (LC->n[(i+2)%NDIM]=0;LC->n[(i+2)%NDIM]<LC->N[(i+2)%NDIM];LC->n[(i+2)%NDIM]++) {
        List = LC->GetCurrentCell();
        //cout << Verbose(2) << "Current cell is " << LC->n[0] << ", " << LC->n[1] << ", " << LC->n[2] << " with No. " << LC->index << "." << endl;
        if (List != NULL) {
          for (LinkedNodes::iterator Runner = List->begin();Runner != List->end();Runner++) {
            if ((*Runner)->node->x[i] > max_coordinate[i]) {
              cout << Verbose(2) << "New maximal for axis " << i << " node is " << *(*Runner) << " at " << *(*Runner)->node << "." << endl;
              max_coordinate[i] = (*Runner)->node->x[i];
              MaxPoint[i] = (*Runner);
            }
          }
        } else {
          cerr << "ERROR: The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2] << " is invalid!" << endl;
        }
      }
  }

  cout << Verbose(2) << "Found maximum coordinates: ";
  for (int i=0;i<NDIM;i++)
    cout << i << ": " << *MaxPoint[i] << "\t";
  cout << endl;

  BTS = NULL;
  CandidateList *Opt_Candidates = new CandidateList();
  for (int k=0;k<NDIM;k++) {
    Oben.x[k] = 1.;
    FirstPoint = MaxPoint[k];
    cout << Verbose(1) << "Coordinates of start node at " << *FirstPoint->node << "." << endl;

    double ShortestAngle;
    TesselPoint* Opt_Candidate = NULL;
    ShortestAngle = 999999.; // This will contain the angle, which will be always positive (when looking for second point), when looking for third point this will be the quadrant.

    Find_second_point_for_Tesselation(FirstPoint, NULL, Oben, Opt_Candidate, &ShortestAngle, RADIUS, LC); // we give same point as next candidate as its bonds are looked into in find_second_...
    SecondPoint = Opt_Candidate;
    if (SecondPoint == NULL)  // have we found a second point?
      continue;
    else
      cout << Verbose(1) << "Found second point is at " << *SecondPoint->node << ".\n";

    helper.CopyVector(FirstPoint->node);
    helper.SubtractVector(SecondPoint->node);
    helper.Normalize();
    Oben.ProjectOntoPlane(&helper);
    Oben.Normalize();
    helper.VectorProduct(&Oben);
    ShortestAngle = 2.*M_PI; // This will indicate the quadrant.

    Chord.CopyVector(FirstPoint->node); // bring into calling function
    Chord.SubtractVector(SecondPoint->node);
    double radius = Chord.ScalarProduct(&Chord);
    double CircleRadius = sqrt(RADIUS*RADIUS - radius/4.);
    helper.CopyVector(&Oben);
    helper.Scale(CircleRadius);
    // Now, oben and helper are two orthonormalized vectors in the plane defined by Chord (not normalized)

    // look in one direction of baseline for initial candidate
    SearchDirection.MakeNormalVector(&Chord, &Oben);  // whether we look "left" first or "right" first is not important ...

    // adding point 1 and point 2 and the line between them
    AddTrianglePoint(FirstPoint, 0);
    AddTrianglePoint(SecondPoint, 1);
    AddTriangleLine(TPS[0], TPS[1], 0);

    //cout << Verbose(2) << "INFO: OldSphereCenter is at " << helper << ".\n";
    Find_third_point_for_Tesselation(
      Oben, SearchDirection, helper, BLS[0], NULL, *&Opt_Candidates, &ShortestAngle, RADIUS, LC
    );
    cout << Verbose(1) << "List of third Points is ";
    for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
        cout << " " << *(*it)->point;
    }
    cout << endl;

    for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
      // add third triangle point
      AddTrianglePoint((*it)->point, 2);
      // add the second and third line
      AddTriangleLine(TPS[1], TPS[2], 1);
      AddTriangleLine(TPS[0], TPS[2], 2);
      // ... and triangles to the Maps of the Tesselation class
      BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
      AddTriangle();
      // ... and calculate its normal vector (with correct orientation)
      (*it)->OptCenter.Scale(-1.);
      cout << Verbose(2) << "Anti-Oben is currently " << (*it)->OptCenter << "." << endl;
      BTS->GetNormalVector((*it)->OptCenter);  // vector to compare with should point inwards
      cout << Verbose(0) << "==> Found starting triangle consists of " << *FirstPoint << ", " << *SecondPoint << " and "
      << *(*it)->point << " with normal vector " << BTS->NormalVector << ".\n";

      // if we do not reach the end with the next step of iteration, we need to setup a new first line
      if (it != Opt_Candidates->end()--) {
        FirstPoint = (*it)->BaseLine->endpoints[0]->node;
        SecondPoint = (*it)->point;
        // adding point 1 and point 2 and the line between them
        AddTrianglePoint(FirstPoint, 0);
        AddTrianglePoint(SecondPoint, 1);
        AddTriangleLine(TPS[0], TPS[1], 0);
      }
      cout << Verbose(2) << "Projection is " << BTS->NormalVector.Projection(&Oben) << "." << endl;
    }
    if (BTS != NULL) // we have created one starting triangle
      break;
    else {
      // remove all candidates from the list and then the list itself
      class CandidateForTesselation *remover = NULL;
      for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
        remover = *it;
        delete(remover);
      }
      Opt_Candidates->clear();
    }
  }

  // remove all candidates from the list and then the list itself
  class CandidateForTesselation *remover = NULL;
  for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
    remover = *it;
    delete(remover);
  }
  delete(Opt_Candidates);
  cout << Verbose(1) << "End of Find_starting_triangle\n";
};


/** This function finds a triangle to a line, adjacent to an existing one.
 * @param out output stream for debugging
 * @param Line current baseline to search from
 * @param T current triangle which \a Line is edge of
 * @param RADIUS radius of the rolling ball
 * @param N number of found triangles
 * @param *LC LinkedCell structure with neighbouring points
 */
bool Tesselation::Find_next_suitable_triangle(ofstream *out, BoundaryLineSet &Line, BoundaryTriangleSet &T, const double& RADIUS, int N, LinkedCell *LC)
{
  cout << Verbose(0) << "Begin of Find_next_suitable_triangle\n";
  bool result = true;
  CandidateList *Opt_Candidates = new CandidateList();

  Vector CircleCenter;
  Vector CirclePlaneNormal;
  Vector OldSphereCenter;
  Vector SearchDirection;
  Vector helper;
  TesselPoint *ThirdNode = NULL;
  LineMap::iterator testline;
  double ShortestAngle = 2.*M_PI; // This will indicate the quadrant.
  double radius, CircleRadius;

  cout << Verbose(1) << "Current baseline is " << Line << " of triangle " << T << "." << endl;
  for (int i=0;i<3;i++)
    if ((T.endpoints[i]->node != Line.endpoints[0]->node) && (T.endpoints[i]->node != Line.endpoints[1]->node))
      ThirdNode = T.endpoints[i]->node;

  // construct center of circle
  CircleCenter.CopyVector(Line.endpoints[0]->node->node);
  CircleCenter.AddVector(Line.endpoints[1]->node->node);
  CircleCenter.Scale(0.5);

  // construct normal vector of circle
  CirclePlaneNormal.CopyVector(Line.endpoints[0]->node->node);
  CirclePlaneNormal.SubtractVector(Line.endpoints[1]->node->node);

  // calculate squared radius of circle
  radius = CirclePlaneNormal.ScalarProduct(&CirclePlaneNormal);
  if (radius/4. < RADIUS*RADIUS) {
    CircleRadius = RADIUS*RADIUS - radius/4.;
    CirclePlaneNormal.Normalize();
    cout << Verbose(2) << "INFO: CircleCenter is at " << CircleCenter << ", CirclePlaneNormal is " << CirclePlaneNormal << " with circle radius " << sqrt(CircleRadius) << "." << endl;

    // construct old center
    GetCenterofCircumcircle(&OldSphereCenter, T.endpoints[0]->node->node, T.endpoints[1]->node->node, T.endpoints[2]->node->node);
    helper.CopyVector(&T.NormalVector);  // normal vector ensures that this is correct center of the two possible ones
    radius = Line.endpoints[0]->node->node->DistanceSquared(&OldSphereCenter);
    helper.Scale(sqrt(RADIUS*RADIUS - radius));
    OldSphereCenter.AddVector(&helper);
    OldSphereCenter.SubtractVector(&CircleCenter);
    //cout << Verbose(2) << "INFO: OldSphereCenter is at " << OldSphereCenter << "." << endl;

    // construct SearchDirection
    SearchDirection.MakeNormalVector(&T.NormalVector, &CirclePlaneNormal);
    helper.CopyVector(Line.endpoints[0]->node->node);
    helper.SubtractVector(ThirdNode->node);
    if (helper.ScalarProduct(&SearchDirection) < -HULLEPSILON)// ohoh, SearchDirection points inwards!
      SearchDirection.Scale(-1.);
    SearchDirection.ProjectOntoPlane(&OldSphereCenter);
    SearchDirection.Normalize();
    cout << Verbose(2) << "INFO: SearchDirection is " << SearchDirection << "." << endl;
    if (fabs(OldSphereCenter.ScalarProduct(&SearchDirection)) > HULLEPSILON) {
      // rotated the wrong way!
      cerr << "ERROR: SearchDirection and RelativeOldSphereCenter are still not orthogonal!" << endl;
    }

    // add third point
    Find_third_point_for_Tesselation(
      T.NormalVector, SearchDirection, OldSphereCenter, &Line, ThirdNode, Opt_Candidates,
      &ShortestAngle, RADIUS, LC
    );

  } else {
    cout << Verbose(1) << "Circumcircle for base line " << Line << " and base triangle " << T << " is too big!" << endl;
  }

  if (Opt_Candidates->begin() == Opt_Candidates->end()) {
    cerr << "WARNING: Could not find a suitable candidate." << endl;
    return false;
  }
  cout << Verbose(1) << "Third Points are ";
  for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
    cout << " " << *(*it)->point;
  }
  cout << endl;

  BoundaryLineSet *BaseRay = &Line;
  for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
    cout << Verbose(1) << " Third point candidate is " << *(*it)->point
    << " with circumsphere's center at " << (*it)->OptCenter << "." << endl;
    cout << Verbose(1) << " Baseline is " << *BaseRay << endl;

    // check whether all edges of the new triangle still have space for one more triangle (i.e. TriangleCount <2)
    TesselPoint *PointCandidates[3];
    PointCandidates[0] = (*it)->point;
    PointCandidates[1] = BaseRay->endpoints[0]->node;
    PointCandidates[2] = BaseRay->endpoints[1]->node;
    int existentTrianglesCount = CheckPresenceOfTriangle(out, PointCandidates);

    BTS = NULL;
    // If there is no triangle, add it regularly.
    if (existentTrianglesCount == 0) {
      AddTrianglePoint((*it)->point, 0);
      AddTrianglePoint(BaseRay->endpoints[0]->node, 1);
      AddTrianglePoint(BaseRay->endpoints[1]->node, 2);

      if (CheckLineCriteriaforDegeneratedTriangle(TPS)) {
        AddTriangleLine(TPS[0], TPS[1], 0);
        AddTriangleLine(TPS[0], TPS[2], 1);
        AddTriangleLine(TPS[1], TPS[2], 2);

        BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
        AddTriangle();
        (*it)->OptCenter.Scale(-1.);
        BTS->GetNormalVector((*it)->OptCenter);
        (*it)->OptCenter.Scale(-1.);

        cout << "--> New triangle with " << *BTS << " and normal vector " << BTS->NormalVector
          << " for this triangle ... " << endl;
      //cout << Verbose(1) << "We have "<< TrianglesOnBoundaryCount << " for line " << *BaseRay << "." << endl;
      } else {
        cout << Verbose(1) << "WARNING: This triangle consisting of ";
        cout << *(*it)->point << ", ";
        cout << *BaseRay->endpoints[0]->node << " and ";
        cout << *BaseRay->endpoints[1]->node << " ";
        cout << "exists and is not added, as it does not seem helpful!" << endl;
        result = false;
      }
    } else if (existentTrianglesCount == 1) { // If there is a planar region within the structure, we need this triangle a second time.
        AddTrianglePoint((*it)->point, 0);
        AddTrianglePoint(BaseRay->endpoints[0]->node, 1);
        AddTrianglePoint(BaseRay->endpoints[1]->node, 2);

        // We demand that at most one new degenerate line is created and that this line also already exists (which has to be the case due to existentTrianglesCount == 1)
        // i.e. at least one of the three lines must be present with TriangleCount <= 1
        if (CheckLineCriteriaforDegeneratedTriangle(TPS)) {
          AddTriangleLine(TPS[0], TPS[1], 0);
          AddTriangleLine(TPS[0], TPS[2], 1);
          AddTriangleLine(TPS[1], TPS[2], 2);

          BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
          AddTriangle();  // add to global map

          (*it)->OtherOptCenter.Scale(-1.);
          BTS->GetNormalVector((*it)->OtherOptCenter);
          (*it)->OtherOptCenter.Scale(-1.);

          cout << "--> WARNING: Special new triangle with " << *BTS << " and normal vector " << BTS->NormalVector
          << " for this triangle ... " << endl;
          cout << Verbose(1) << "We have "<< BaseRay->TrianglesCount << " for line " << BaseRay << "." << endl;
        } else {
          cout << Verbose(1) << "WARNING: This triangle consisting of ";
          cout << *(*it)->point << ", ";
          cout << *BaseRay->endpoints[0]->node << " and ";
          cout << *BaseRay->endpoints[1]->node << " ";
          cout << "exists and is not added, as it does not seem helpful!" << endl;
          result = false;
        }
    } else {
      cout << Verbose(1) << "This triangle consisting of ";
      cout << *(*it)->point << ", ";
      cout << *BaseRay->endpoints[0]->node << " and ";
      cout << *BaseRay->endpoints[1]->node << " ";
      cout << "is invalid!" << endl;
      result = false;
    }

    // set baseline to new ray from ref point (here endpoints[0]->node) to current candidate (here (*it)->point))
    BaseRay = BLS[0];
  }

  // remove all candidates from the list and then the list itself
  class CandidateForTesselation *remover = NULL;
  for (CandidateList::iterator it = Opt_Candidates->begin(); it != Opt_Candidates->end(); ++it) {
    remover = *it;
    delete(remover);
  }
  delete(Opt_Candidates);
  cout << Verbose(0) << "End of Find_next_suitable_triangle\n";
  return result;
};


/** Goes over all baselines and checks whether adjacent triangles and convex to each other.
 * \param *out output stream for debugging
 * \return true - all baselines were corrected, false - there are still concave pieces
 */
bool Tesselation::CorrectConcaveBaselines(ofstream *out)
{
  class BoundaryLineSet *OldLines[4], *NewLine;
  class BoundaryPointSet *OldPoints[2];
  Vector BaseLineNormal;
  class BoundaryLineSet *Base = NULL;
  int OldTriangles[2], OldBaseLine;
  int i,m;

  *out << Verbose(1) << "Begin of CorrectConcaveBaselines" << endl;

  for (LineMap::iterator baseline = LinesOnBoundary.begin(); baseline != LinesOnBoundary.end(); baseline++) {
    Base = baseline->second;
    *out << Verbose(2) << "Current baseline is " << *Base << " ... " << endl;
    // check convexity
    if (Base->CheckConvexityCriterion(out)) { // triangles are convex
      *out << Verbose(2) << "... has two convex triangles." << endl;
    } else { // not convex!
      *out << Verbose(2) << "... has two concave triangles!" << endl;
      // get the two triangles
      // gather four endpoints and four lines
      for (int j=0;j<4;j++)
        OldLines[j] = NULL;
      for (int j=0;j<2;j++)
        OldPoints[j] = NULL;
      i=0;
      m=0;
      *out << Verbose(3) << "The four old lines are: ";
      for(TriangleMap::iterator runner = Base->triangles.begin(); runner != Base->triangles.end(); runner++)
        for (int j=0;j<3;j++) // all of their endpoints and baselines
          if (runner->second->lines[j] != Base) { // pick not the central baseline
            OldLines[i++] = runner->second->lines[j];
            *out << *runner->second->lines[j] << "\t";
          }
      *out << endl;
      *out << Verbose(3) << "The two old points are: ";
      for(TriangleMap::iterator runner = Base->triangles.begin(); runner != Base->triangles.end(); runner++)
        for (int j=0;j<3;j++) // all of their endpoints and baselines
          if (!Base->ContainsBoundaryPoint(runner->second->endpoints[j])) { // and neither of its endpoints
            OldPoints[m++] = runner->second->endpoints[j];
            *out << *runner->second->endpoints[j] << "\t";
          }
      *out << endl;
      if (i<4) {
        *out << Verbose(1) << "ERROR: We have not gathered enough baselines!" << endl;
        return false;
      }
      for (int j=0;j<4;j++)
        if (OldLines[j] == NULL) {
          *out << Verbose(1) << "ERROR: We have not gathered enough baselines!" << endl;
          return false;
        }
      for (int j=0;j<2;j++)
        if (OldPoints[j] == NULL) {
          *out << Verbose(1) << "ERROR: We have not gathered enough endpoints!" << endl;
          return false;
        }

      // remove triangles and baseline removes itself
      m=0;
      OldBaseLine = Base->Nr;
      LinesOnBoundary.erase(OldBaseLine);
      for(TriangleMap::iterator runner = Base->triangles.begin(); runner != Base->triangles.end(); runner++) {
        TrianglesOnBoundary.erase(OldTriangles[m++] = runner->second->Nr);
        delete(runner->second);
      }

      // construct new baseline (with same number as old one)
      BPS[0] = OldPoints[0];
      BPS[1] = OldPoints[1];
      NewLine = new class BoundaryLineSet(BPS, OldBaseLine);
      LinesOnBoundary.insert(LinePair(OldBaseLine, NewLine)); // no need for check for unique insertion as NewLine is definitely a new one

      // construct new triangles with flipped baseline
      i=-1;
      if (OldLines[0]->IsConnectedTo(OldLines[2]))
        i=2;
      if (OldLines[0]->IsConnectedTo(OldLines[3]))
        i=3;
      if (i!=-1) {
        BLS[0] = OldLines[0];
        BLS[1] = OldLines[i];
        BLS[2] = NewLine;
        BTS = new class BoundaryTriangleSet(BLS, OldTriangles[0]);
        TrianglesOnBoundary.insert(TrianglePair(OldTriangles[0], BTS));

        BLS[0] = (i==2 ? OldLines[3] : OldLines[2]);
        BLS[1] = OldLines[1];
        BLS[2] = NewLine;
        BTS = new class BoundaryTriangleSet(BLS, OldTriangles[1]);
        TrianglesOnBoundary.insert(TrianglePair(OldTriangles[1], BTS));
      } else {
        *out << Verbose(1) << "The four old lines do not connect, something's utterly wrong here!" << endl;
        return false;
      }
    }
  }
  *out << Verbose(1) << "End of CorrectConcaveBaselines" << endl;
  return true;
};

/** Finds the second point of starting triangle.
 * \param *a first node
 * \param *Candidate pointer to candidate node on return
 * \param Oben vector indicating the outside
 * \param Opt_Candidate reference to recommended candidate on return
 * \param Storage[3] array storing angles and other candidate information
 * \param RADIUS radius of virtual sphere
 * \param *LC LinkedCell structure with neighbouring points
 */
void Tesselation::Find_second_point_for_Tesselation(TesselPoint* a, TesselPoint* Candidate, Vector Oben, TesselPoint*& Opt_Candidate, double Storage[3], double RADIUS, LinkedCell *LC)
{
  cout << Verbose(2) << "Begin of Find_second_point_for_Tesselation" << endl;
  Vector AngleCheck;
  double norm = -1., angle;
  LinkedNodes *List = NULL;
  int N[NDIM], Nlower[NDIM], Nupper[NDIM];

  if (LC->SetIndexToNode(a)) {  // get cell for the starting point
    for(int i=0;i<NDIM;i++) // store indices of this cell
      N[i] = LC->n[i];
  } else {
    cerr << "ERROR: Point " << *a << " is not found in cell " << LC->index << "." << endl;
    return;
  }
  // then go through the current and all neighbouring cells and check the contained points for possible candidates
  cout << Verbose(3) << "LC Intervals from [";
  for (int i=0;i<NDIM;i++) {
  cout << " " << N[i] << "<->" << LC->N[i];
  }
  cout << "] :";
  for (int i=0;i<NDIM;i++) {
    Nlower[i] = ((N[i]-1) >= 0) ? N[i]-1 : 0;
    Nupper[i] = ((N[i]+1) < LC->N[i]) ? N[i]+1 : LC->N[i]-1;
    cout << " [" << Nlower[i] << "," << Nupper[i] << "] ";
  }
  cout << endl;


  for (LC->n[0] = Nlower[0]; LC->n[0] <= Nupper[0]; LC->n[0]++)
    for (LC->n[1] = Nlower[1]; LC->n[1] <= Nupper[1]; LC->n[1]++)
      for (LC->n[2] = Nlower[2]; LC->n[2] <= Nupper[2]; LC->n[2]++) {
        List = LC->GetCurrentCell();
        //cout << Verbose(2) << "Current cell is " << LC->n[0] << ", " << LC->n[1] << ", " << LC->n[2] << " with No. " << LC->index << "." << endl;
        if (List != NULL) {
          for (LinkedNodes::iterator Runner = List->begin(); Runner != List->end(); Runner++) {
            Candidate = (*Runner);
            // check if we only have one unique point yet ...
            if (a != Candidate) {
              // Calculate center of the circle with radius RADIUS through points a and Candidate
              Vector OrthogonalizedOben, a_Candidate, Center;
              double distance, scaleFactor;

              OrthogonalizedOben.CopyVector(&Oben);
              a_Candidate.CopyVector(a->node);
              a_Candidate.SubtractVector(Candidate->node);
              OrthogonalizedOben.ProjectOntoPlane(&a_Candidate);
              OrthogonalizedOben.Normalize();
              distance = 0.5 * a_Candidate.Norm();
              scaleFactor = sqrt(((RADIUS * RADIUS) - (distance * distance)));
              OrthogonalizedOben.Scale(scaleFactor);

              Center.CopyVector(Candidate->node);
              Center.AddVector(a->node);
              Center.Scale(0.5);
              Center.AddVector(&OrthogonalizedOben);

              AngleCheck.CopyVector(&Center);
              AngleCheck.SubtractVector(a->node);
              norm = a_Candidate.Norm();
              // second point shall have smallest angle with respect to Oben vector
              if (norm < RADIUS*2.) {
                angle = AngleCheck.Angle(&Oben);
                if (angle < Storage[0]) {
                  //cout << Verbose(3) << "Old values of Storage: %lf %lf \n", Storage[0], Storage[1]);
                  cout << Verbose(3) << "Current candidate is " << *Candidate << ": Is a better candidate with distance " << norm << " and angle " << angle << " to oben " << Oben << ".\n";
                  Opt_Candidate = Candidate;
                  Storage[0] = angle;
                  //cout << Verbose(3) << "Changing something in Storage: %lf %lf. \n", Storage[0], Storage[2]);
                } else {
                  //cout << Verbose(3) << "Current candidate is " << *Candidate << ": Looses with angle " << angle << " to a better candidate " << *Opt_Candidate << endl;
                }
              } else {
                //cout << Verbose(3) << "Current candidate is " << *Candidate << ": Refused due to Radius " << norm << endl;
              }
            } else {
              //cout << Verbose(3) << "Current candidate is " << *Candidate << ": Candidate is equal to first endpoint." << *a << "." << endl;
            }
          }
        } else {
          cout << Verbose(3) << "Linked cell list is empty." << endl;
        }
      }
  cout << Verbose(2) << "End of Find_second_point_for_Tesselation" << endl;
};


/** This recursive function finds a third point, to form a triangle with two given ones.
 * Note that this function is for the starting triangle.
 * The idea is as follows: A sphere with fixed radius is (almost) uniquely defined in space by three points
 * that sit on its boundary. Hence, when two points are given and we look for the (next) third point, then
 * the center of the sphere is still fixed up to a single parameter. The band of possible values
 * describes a circle in 3D-space. The old center of the sphere for the current base triangle gives
 * us the "null" on this circle, the new center of the candidate point will be some way along this
 * circle. The shorter the way the better is the candidate. Note that the direction is clearly given
 * by the normal vector of the base triangle that always points outwards by construction.
 * Hence, we construct a Center of this circle which sits right in the middle of the current base line.
 * We construct the normal vector that defines the plane this circle lies in, it is just in the
 * direction of the baseline. And finally, we need the radius of the circle, which is given by the rest
 * with respect to the length of the baseline and the sphere's fixed \a RADIUS.
 * Note that there is one difficulty: The circumcircle is uniquely defined, but for the circumsphere's center
 * there are two possibilities which becomes clear from the construction as seen below. Hence, we must check
 * both.
 * Note also that the acos() function is not unique on [0, 2.*M_PI). Hence, we need an additional check
 * to decide for one of the two possible angles. Therefore we need a SearchDirection and to make this check
 * sensible we need OldSphereCenter to be orthogonal to it. Either we construct SearchDirection orthogonal
 * right away, or -- what we do here -- we rotate the relative sphere centers such that this orthogonality
 * holds. Then, the normalized projection onto the SearchDirection is either +1 or -1 and thus states whether
 * the angle is uniquely in either (0,M_PI] or [M_PI, 2.*M_PI).
 * @param NormalVector normal direction of the base triangle (here the unit axis vector, \sa Find_starting_triangle())
 * @param SearchDirection general direction where to search for the next point, relative to center of BaseLine
 * @param OldSphereCenter center of sphere for base triangle, relative to center of BaseLine, giving null angle for the parameter circle
 * @param BaseLine BoundaryLineSet with the current base line
 * @param ThirdNode third point to avoid in search
 * @param candidates list of equally good candidates to return
 * @param ShortestAngle the current path length on this circle band for the current Opt_Candidate
 * @param RADIUS radius of sphere
 * @param *LC LinkedCell structure with neighbouring points
 */
void Tesselation::Find_third_point_for_Tesselation(Vector NormalVector, Vector SearchDirection, Vector OldSphereCenter, class BoundaryLineSet *BaseLine, class TesselPoint  *ThirdNode, CandidateList* &candidates, double *ShortestAngle, const double RADIUS, LinkedCell *LC)
{
  Vector CircleCenter;  // center of the circle, i.e. of the band of sphere's centers
  Vector CirclePlaneNormal; // normal vector defining the plane this circle lives in
  Vector SphereCenter;
  Vector NewSphereCenter;   // center of the sphere defined by the two points of BaseLine and the one of Candidate, first possibility
  Vector OtherNewSphereCenter;   // center of the sphere defined by the two points of BaseLine and the one of Candidate, second possibility
  Vector NewNormalVector;   // normal vector of the Candidate's triangle
  Vector helper, OptCandidateCenter, OtherOptCandidateCenter;
  LinkedNodes *List = NULL;
  double CircleRadius; // radius of this circle
  double radius;
  double alpha, Otheralpha; // angles (i.e. parameter for the circle).
  int N[NDIM], Nlower[NDIM], Nupper[NDIM];
  TesselPoint *Candidate = NULL;
  CandidateForTesselation *optCandidate = NULL;

  cout << Verbose(1) << "Begin of Find_third_point_for_Tesselation" << endl;

  //cout << Verbose(2) << "INFO: NormalVector of BaseTriangle is " << NormalVector << "." << endl;

  // construct center of circle
  CircleCenter.CopyVector(BaseLine->endpoints[0]->node->node);
  CircleCenter.AddVector(BaseLine->endpoints[1]->node->node);
  CircleCenter.Scale(0.5);

  // construct normal vector of circle
  CirclePlaneNormal.CopyVector(BaseLine->endpoints[0]->node->node);
  CirclePlaneNormal.SubtractVector(BaseLine->endpoints[1]->node->node);

  // calculate squared radius TesselPoint *ThirdNode,f circle
  radius = CirclePlaneNormal.ScalarProduct(&CirclePlaneNormal);
  if (radius/4. < RADIUS*RADIUS) {
    CircleRadius = RADIUS*RADIUS - radius/4.;
    CirclePlaneNormal.Normalize();
    //cout << Verbose(2) << "INFO: CircleCenter is at " << CircleCenter << ", CirclePlaneNormal is " << CirclePlaneNormal << " with circle radius " << sqrt(CircleRadius) << "." << endl;

    // test whether old center is on the band's plane
    if (fabs(OldSphereCenter.ScalarProduct(&CirclePlaneNormal)) > HULLEPSILON) {
      cerr << "ERROR: Something's very wrong here: OldSphereCenter is not on the band's plane as desired by " << fabs(OldSphereCenter.ScalarProduct(&CirclePlaneNormal)) << "!" << endl;
      OldSphereCenter.ProjectOntoPlane(&CirclePlaneNormal);
    }
    radius = OldSphereCenter.ScalarProduct(&OldSphereCenter);
    if (fabs(radius - CircleRadius) < HULLEPSILON) {

      // check SearchDirection
      //cout << Verbose(2) << "INFO: SearchDirection is " << SearchDirection << "." << endl;
      if (fabs(OldSphereCenter.ScalarProduct(&SearchDirection)) > HULLEPSILON) {  // rotated the wrong way!
        cerr << "ERROR: SearchDirection and RelativeOldSphereCenter are not orthogonal!" << endl;
      }

      // get cell for the starting point
      if (LC->SetIndexToVector(&CircleCenter)) {
        for(int i=0;i<NDIM;i++) // store indices of this cell
        N[i] = LC->n[i];
        //cout << Verbose(2) << "INFO: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << "." << endl;
      } else {
        cerr << "ERROR: Vector " << CircleCenter << " is outside of LinkedCell's bounding box." << endl;
        return;
      }
      // then go through the current and all neighbouring cells and check the contained points for possible candidates
      //cout << Verbose(2) << "LC Intervals:";
      for (int i=0;i<NDIM;i++) {
        Nlower[i] = ((N[i]-1) >= 0) ? N[i]-1 : 0;
        Nupper[i] = ((N[i]+1) < LC->N[i]) ? N[i]+1 : LC->N[i]-1;
        //cout << " [" << Nlower[i] << "," << Nupper[i] << "] ";
      }
      //cout << endl;
      for (LC->n[0] = Nlower[0]; LC->n[0] <= Nupper[0]; LC->n[0]++)
        for (LC->n[1] = Nlower[1]; LC->n[1] <= Nupper[1]; LC->n[1]++)
          for (LC->n[2] = Nlower[2]; LC->n[2] <= Nupper[2]; LC->n[2]++) {
            List = LC->GetCurrentCell();
            //cout << Verbose(2) << "Current cell is " << LC->n[0] << ", " << LC->n[1] << ", " << LC->n[2] << " with No. " << LC->index << "." << endl;
            if (List != NULL) {
              for (LinkedNodes::iterator Runner = List->begin(); Runner != List->end(); Runner++) {
                Candidate = (*Runner);

                // check for three unique points
                //cout << Verbose(2) << "INFO: Current Candidate is " << *Candidate << " at " << Candidate->node << "." << endl;
                if ((Candidate != BaseLine->endpoints[0]->node) && (Candidate != BaseLine->endpoints[1]->node) ){

                  // construct both new centers
                  GetCenterofCircumcircle(&NewSphereCenter, BaseLine->endpoints[0]->node->node, BaseLine->endpoints[1]->node->node, Candidate->node);
                  OtherNewSphereCenter.CopyVector(&NewSphereCenter);

                  if ((NewNormalVector.MakeNormalVector(BaseLine->endpoints[0]->node->node, BaseLine->endpoints[1]->node->node, Candidate->node))
                  && (fabs(NewNormalVector.ScalarProduct(&NewNormalVector)) > HULLEPSILON)
                  ) {
                    helper.CopyVector(&NewNormalVector);
                    //cout << Verbose(2) << "INFO: NewNormalVector is " << NewNormalVector << "." << endl;
                    radius = BaseLine->endpoints[0]->node->node->DistanceSquared(&NewSphereCenter);
                    if (radius < RADIUS*RADIUS) {
                      helper.Scale(sqrt(RADIUS*RADIUS - radius));
                      //cout << Verbose(2) << "INFO: Distance of NewCircleCenter to NewSphereCenter is " << helper.Norm() << " with sphere radius " << RADIUS << "." << endl;
                      NewSphereCenter.AddVector(&helper);
                      NewSphereCenter.SubtractVector(&CircleCenter);
                      //cout << Verbose(2) << "INFO: NewSphereCenter is at " << NewSphereCenter << "." << endl;

                      // OtherNewSphereCenter is created by the same vector just in the other direction
                      helper.Scale(-1.);
                      OtherNewSphereCenter.AddVector(&helper);
                      OtherNewSphereCenter.SubtractVector(&CircleCenter);
                      //cout << Verbose(2) << "INFO: OtherNewSphereCenter is at " << OtherNewSphereCenter << "." << endl;

                      alpha = GetPathLengthonCircumCircle(CircleCenter, CirclePlaneNormal, CircleRadius, NewSphereCenter, OldSphereCenter, NormalVector, SearchDirection);
                      Otheralpha = GetPathLengthonCircumCircle(CircleCenter, CirclePlaneNormal, CircleRadius, OtherNewSphereCenter, OldSphereCenter, NormalVector, SearchDirection);
                      alpha = min(alpha, Otheralpha);
                      // if there is a better candidate, drop the current list and add the new candidate
                      // otherwise ignore the new candidate and keep the list
                      if (*ShortestAngle > (alpha - HULLEPSILON)) {
                        optCandidate = new CandidateForTesselation(Candidate, BaseLine, OptCandidateCenter, OtherOptCandidateCenter);
                        if (fabs(alpha - Otheralpha) > MYEPSILON) {
                          optCandidate->OptCenter.CopyVector(&NewSphereCenter);
                          optCandidate->OtherOptCenter.CopyVector(&OtherNewSphereCenter);
                        } else {
                          optCandidate->OptCenter.CopyVector(&OtherNewSphereCenter);
                          optCandidate->OtherOptCenter.CopyVector(&NewSphereCenter);
                        }
                        // if there is an equal candidate, add it to the list without clearing the list
                        if ((*ShortestAngle - HULLEPSILON) < alpha) {
                          candidates->push_back(optCandidate);
                          cout << Verbose(2) << "ACCEPT: We have found an equally good candidate: " << *(optCandidate->point) << " with "
                            << alpha << " and circumsphere's center at " << optCandidate->OptCenter << "." << endl;
                        } else {
                          // remove all candidates from the list and then the list itself
                          class CandidateForTesselation *remover = NULL;
                          for (CandidateList::iterator it = candidates->begin(); it != candidates->end(); ++it) {
                            remover = *it;
                            delete(remover);
                          }
                          candidates->clear();
                          candidates->push_back(optCandidate);
                          cout << Verbose(2) << "ACCEPT: We have found a better candidate: " << *(optCandidate->point) << " with "
                            << alpha << " and circumsphere's center at " << optCandidate->OptCenter << "." << endl;
                        }
                        *ShortestAngle = alpha;
                        //cout << Verbose(2) << "INFO: There are " << candidates->size() << " candidates in the list now." << endl;
                      } else {
                        if ((optCandidate != NULL) && (optCandidate->point != NULL)) {
                          //cout << Verbose(2) << "REJECT: Old candidate " << *(optCandidate->point) << " with " << *ShortestAngle << " is better than new one " << *Candidate << " with " << alpha << " ." << endl;
                        } else {
                          //cout << Verbose(2) << "REJECT: Candidate " << *Candidate << " with " << alpha << " was rejected." << endl;
                        }
                      }

                    } else {
                      //cout << Verbose(2) << "REJECT: NewSphereCenter " << NewSphereCenter << " for " << *Candidate << " is too far away: " << radius << "." << endl;
                    }
                  } else {
                    //cout << Verbose(2) << "REJECT: Three points from " << *BaseLine << " and Candidate " << *Candidate << " are linear-dependent." << endl;
                  }
                } else {
                  if (ThirdNode != NULL) {
                    //cout << Verbose(2) << "REJECT: Base triangle " << *BaseLine << " and " << *ThirdNode << " contains Candidate " << *Candidate << "." << endl;
                  } else {
                    //cout << Verbose(2) << "REJECT: Base triangle " << *BaseLine << " contains Candidate " << *Candidate << "." << endl;
                  }
                }
              }
            }
          }
    } else {
      cerr << Verbose(2) << "ERROR: The projected center of the old sphere has radius " << radius << " instead of " << CircleRadius << "." << endl;
    }
  } else {
    if (ThirdNode != NULL)
      cout << Verbose(2) << "Circumcircle for base line " << *BaseLine << " and third node " << *ThirdNode << " is too big!" << endl;
    else
      cout << Verbose(2) << "Circumcircle for base line " << *BaseLine << " is too big!" << endl;
  }

  //cout << Verbose(2) << "INFO: Sorting candidate list ..." << endl;
  if (candidates->size() > 1) {
    candidates->unique();
    candidates->sort(sortCandidates);
  }

  cout << Verbose(1) << "End of Find_third_point_for_Tesselation" << endl;
};

/** Finds the endpoint two lines are sharing.
 * \param *line1 first line
 * \param *line2 second line
 * \return point which is shared or NULL if none
 */
class BoundaryPointSet *Tesselation::GetCommonEndpoint(class BoundaryLineSet * line1, class BoundaryLineSet * line2)
{
  class BoundaryLineSet * lines[2] =
    { line1, line2 };
  class BoundaryPointSet *node = NULL;
  map<int, class BoundaryPointSet *> OrderMap;
  pair<map<int, class BoundaryPointSet *>::iterator, bool> OrderTest;
  for (int i = 0; i < 2; i++)
    // for both lines
    for (int j = 0; j < 2; j++)
      { // for both endpoints
        OrderTest = OrderMap.insert(pair<int, class BoundaryPointSet *> (
            lines[i]->endpoints[j]->Nr, lines[i]->endpoints[j]));
        if (!OrderTest.second)
          { // if insertion fails, we have common endpoint
            node = OrderTest.first->second;
            cout << Verbose(5) << "Common endpoint of lines " << *line1
                << " and " << *line2 << " is: " << *node << "." << endl;
            j = 2;
            i = 2;
            break;
          }
      }
  return node;
};

/** Finds the triangle that is closest to a given Vector \a *x.
 * \param *out output stream for debugging
 * \param *x Vector to look from
 * \return list of BoundaryTriangleSet of nearest triangles or NULL in degenerate case.
 */
list<BoundaryTriangleSet*> * Tesselation::FindClosestTrianglesToPoint(ofstream *out, Vector *x, LinkedCell* LC)
{
  class TesselPoint *trianglePoints[3];

  if (LinesOnBoundary.empty()) {
    *out << Verbose(0) << "Error: There is no tesselation structure to compare the point with, " << "please create one first.";
    return NULL;
  }

  trianglePoints[0] = findClosestPoint(x, LC);
  // check whether closest point is "too close" :), then it's inside
  if (trianglePoints[0]->node->DistanceSquared(x) < MYEPSILON) {
    *out << Verbose(1) << "Point is right on a tesselation point, no nearest triangle." << endl;
    return NULL;
  }
  list<TesselPoint*> *connectedClosestPoints = getCircleOfConnectedPoints(out, trianglePoints[0], x);
  trianglePoints[1] = connectedClosestPoints->front();
  trianglePoints[2] = connectedClosestPoints->back();
  for (int i=0;i<3;i++) {
    if (trianglePoints[i] == NULL) {
      *out << Verbose(1) << "IsInnerPoint encounters serious error, point " << i << " not found." << endl;
    }
    *out << Verbose(1) << "List of possible points:" << endl;
    *out << *trianglePoints[i] << endl;
  }
  delete(connectedClosestPoints);

  list<BoundaryTriangleSet*> *triangles = FindTriangles(trianglePoints);

  if (triangles->empty()) {
    *out << Verbose(0) << "Error: There is no nearest triangle. Please check the tesselation structure.";
    return NULL;
  } else
    return triangles;
};

/** Finds closest triangle to a point.
 * This basically just takes care of the degenerate case, which is not handled in FindClosestTrianglesToPoint().
 * \param *out output stream for debugging
 * \param *x Vector to look from
 * \return list of BoundaryTriangleSet of nearest triangles or NULL.
 */
class BoundaryTriangleSet * Tesselation::FindClosestTriangleToPoint(ofstream *out, Vector *x, LinkedCell* LC)
{
  class BoundaryTriangleSet *result = NULL;
  list<BoundaryTriangleSet*> *triangles = FindClosestTrianglesToPoint(out, x, LC);

  if (triangles == NULL)
    return NULL;

  if (x->ScalarProduct(&triangles->front()->NormalVector) < 0)
    result = triangles->back();
  else
    result = triangles->front();

  delete(triangles);
  return result;
};

/** Checks whether the provided Vector is within the tesselation structure.
 *
 * @param point of which to check the position
 * @param *LC LinkedCell structure
 *
 * @return true if the point is inside the tesselation structure, false otherwise
 */
bool Tesselation::IsInnerPoint(ofstream *out, Vector Point, LinkedCell* LC)
{
  class BoundaryTriangleSet *result = FindClosestTriangleToPoint(out, &Point, LC);
  if (result == NULL)
    return true;
  if (Point.ScalarProduct(&result->NormalVector) < 0)
    return true;
  else
    return false;
}

/** Checks whether the provided TesselPoint is within the tesselation structure.
 *
 * @param *Point of which to check the position
 * @param *LC Linked Cell structure
 *
 * @return true if the point is inside the tesselation structure, false otherwise
 */
bool Tesselation::IsInnerPoint(ofstream *out, TesselPoint *Point, LinkedCell* LC)
{
  class BoundaryTriangleSet *result = FindClosestTriangleToPoint(out, Point->node, LC);
  if (result == NULL)
    return true;
  if (Point->node->ScalarProduct(&result->NormalVector) < 0)
    return true;
  else
    return false;
}

/** Gets all points connected to the provided point by triangulation lines.
 * Maps them down onto the plane designated by the axis \a *Point and \a *Reference. The center of all points
 * connected in the tesselation to \a *Point is mapped to spherical coordinates with the zero angle being given
 * by the mapped down \a *Reference. Hence, the biggest and the smallest angles are those of the two shanks of the
 * triangle we are looking for.
 *
 * @param *Point of which get all connected points
 * @param *Reference Vector to be checked whether it is an inner point
 *
 * @return list of the two points linked to the provided one and closest to the point to be checked,
 */
list<TesselPoint*> * Tesselation::getCircleOfConnectedPoints(ofstream *out, TesselPoint* Point, Vector* Reference)
{
  list<TesselPoint*> connectedPoints;
  map<double, TesselPoint*> anglesOfPoints;
  map<double, TesselPoint*>::iterator runner;
  list<TesselPoint*>::iterator listRunner;
  Vector center, planeNorm, currentPoint, OrthogonalVector, helper;
  TesselPoint* current;
  bool takePoint = false;

  planeNorm.CopyVector(Point->node);
  planeNorm.SubtractVector(Reference);
  planeNorm.Normalize();

  LineMap::iterator findLines = LinesOnBoundary.begin();
  while (findLines != LinesOnBoundary.end()) {
    takePoint = false;

    if (findLines->second->endpoints[0]->Nr == Point->nr) {
      takePoint = true;
      current = findLines->second->endpoints[1]->node;
    } else if (findLines->second->endpoints[1]->Nr == Point->nr) {
      takePoint = true;
      current = findLines->second->endpoints[0]->node;
    }

    if (takePoint) {
      connectedPoints.push_back(current);
      currentPoint.CopyVector(current->node);
      currentPoint.ProjectOntoPlane(&planeNorm);
      center.AddVector(&currentPoint);
    }

    findLines++;
  }

  *out << "Summed vectors " << center << "; number of points " << connectedPoints.size()
    << "; scale factor " << 1.0/connectedPoints.size();

  center.Scale(1.0/connectedPoints.size());
  listRunner = connectedPoints.begin();

  *out << " calculated center " << center <<  endl;

  // construct one orthogonal vector
  helper.CopyVector(Reference);
  helper.ProjectOntoPlane(&planeNorm);
  OrthogonalVector.MakeNormalVector(&center, &helper, (*listRunner)->node);
  while (listRunner != connectedPoints.end()) {
    double angle = getAngle(*((*listRunner)->node), *(Reference), center, OrthogonalVector);
    *out << "Calculated angle " << angle << " for point " << **listRunner << endl;
    anglesOfPoints.insert(pair<double, TesselPoint*>(angle, (*listRunner)));
    listRunner++;
  }

  list<TesselPoint*> *result = new list<TesselPoint*>;
  runner = anglesOfPoints.begin();
  *out << "First value is " << *runner->second << endl;
  result->push_back(runner->second);
  runner = anglesOfPoints.end();
  runner--;
  *out << "Second value is " << *runner->second << endl;
  result->push_back(runner->second);

  *out << "List of closest points has " << result->size() << " elements, which are "
    << *(result->front()) << " and " << *(result->back()) << endl;

  return result;
}

/** Checks for a new special triangle whether one of its edges is already present with one one triangle connected.
 * This enforces that special triangles (i.e. degenerated ones) should at last close the open-edge frontier and not
 * make it bigger (i.e. closing one (the baseline) and opening two new ones).
 * \param TPS[3] nodes of the triangle
 * \return true - there is such a line (i.e. creation of degenerated triangle is valid), false - no such line (don't create)
 */
bool CheckLineCriteriaforDegeneratedTriangle(class BoundaryPointSet *nodes[3])
{
  bool result = false;
  int counter = 0;

  // check all three points
  for (int i=0;i<3;i++)
    for (int j=i+1; j<3; j++) {
      if (nodes[i]->lines.find(nodes[j]->node->nr) != nodes[i]->lines.end()) {  // there already is a line
        LineMap::iterator FindLine;
        pair<LineMap::iterator,LineMap::iterator> FindPair;
        FindPair = nodes[i]->lines.equal_range(nodes[j]->node->nr);
        for (FindLine = FindPair.first; FindLine != FindPair.second; ++FindLine) {
          // If there is a line with less than two attached triangles, we don't need a new line.
          if (FindLine->second->TrianglesCount < 2) {
            counter++;
            break;  // increase counter only once per edge
          }
        }
      } else { // no line
        cout << Verbose(1) << "ERROR: The line between " << nodes[i] << " and " << nodes[j] << " is not yet present, hence no need for a degenerate triangle!" << endl;
        result = true;
      }
    }
  if (counter > 1) {
    cout << Verbose(2) << "INFO: Degenerate triangle is ok, at least two, here " << counter << ", existing lines are used." << endl;
    result = true;
  }
  return result;
};


/** Sort function for the candidate list.
 */
bool sortCandidates(CandidateForTesselation* candidate1, CandidateForTesselation* candidate2) 
{
  Vector BaseLineVector, OrthogonalVector, helper;
  if (candidate1->BaseLine != candidate2->BaseLine) {  // sanity check
    cout << Verbose(0) << "ERROR: sortCandidates was called for two different baselines: " << candidate1->BaseLine << " and " << candidate2->BaseLine << "." << endl;
    //return false;
    exit(1);
  }
  // create baseline vector
  BaseLineVector.CopyVector(candidate1->BaseLine->endpoints[1]->node->node);
  BaseLineVector.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
  BaseLineVector.Normalize();

  // create normal in-plane vector to cope with acos() non-uniqueness on [0,2pi] (note that is pointing in the "right" direction already, hence ">0" test!)
  helper.CopyVector(candidate1->BaseLine->endpoints[0]->node->node);
  helper.SubtractVector(candidate1->point->node);
  OrthogonalVector.CopyVector(&helper);
  helper.VectorProduct(&BaseLineVector);
  OrthogonalVector.SubtractVector(&helper);
  OrthogonalVector.Normalize();

  // calculate both angles and correct with in-plane vector
  helper.CopyVector(candidate1->point->node);
  helper.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
  double phi = BaseLineVector.Angle(&helper);
  if (OrthogonalVector.ScalarProduct(&helper) > 0) {
    phi = 2.*M_PI - phi;
  }
  helper.CopyVector(candidate2->point->node);
  helper.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
  double psi = BaseLineVector.Angle(&helper);
  if (OrthogonalVector.ScalarProduct(&helper) > 0) {
    psi = 2.*M_PI - psi;
  }

  cout << Verbose(2) << *candidate1->point << " has angle " << phi << endl;
  cout << Verbose(2) << *candidate2->point << " has angle " << psi << endl;

  // return comparison
  return phi < psi;
};



/**
 * Finds the point which is closest to the provided one.
 *
 * @param Point to which to find the closest other point
 * @param linked cell structure
 *
 * @return point which is closest to the provided one
 */
TesselPoint* findClosestPoint(const Vector* Point, LinkedCell* LC)
{
  LinkedNodes *List = NULL;
  TesselPoint* closestPoint = NULL;
  double distance = 1e16;
  Vector helper;
  int N[NDIM], Nlower[NDIM], Nupper[NDIM];

  LC->SetIndexToVector(Point); // ignore status as we calculate bounds below sensibly
  for(int i=0;i<NDIM;i++) // store indices of this cell
    N[i] = LC->n[i];
  //cout << Verbose(2) << "INFO: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << "." << endl;

  LC->GetNeighbourBounds(Nlower, Nupper);
  //cout << endl;
  for (LC->n[0] = Nlower[0]; LC->n[0] <= Nupper[0]; LC->n[0]++)
    for (LC->n[1] = Nlower[1]; LC->n[1] <= Nupper[1]; LC->n[1]++)
      for (LC->n[2] = Nlower[2]; LC->n[2] <= Nupper[2]; LC->n[2]++) {
        List = LC->GetCurrentCell();
        //cout << "The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2] << endl;
        if (List != NULL) {
          for (LinkedNodes::iterator Runner = List->begin(); Runner != List->end(); Runner++) {
            helper.CopyVector(Point);
            helper.SubtractVector((*Runner)->node);
            double currentNorm = helper. Norm();
            if (currentNorm < distance) {
              distance = currentNorm;
              closestPoint = (*Runner);
            }
          }
        } else {
          cerr << "ERROR: The current cell " << LC->n[0] << "," << LC->n[1] << ","
            << LC->n[2] << " is invalid!" << endl;
        }
      }

  return closestPoint;
}


/**
 * Finds triangles belonging to the three provided points.
 *
 * @param *Points[3] list, is expected to contain three points
 *
 * @return triangles which belong to the provided points, will be empty if there are none,
 *         will usually be one, in case of degeneration, there will be two
 */
list<BoundaryTriangleSet*> *Tesselation::FindTriangles(TesselPoint* Points[3]) 
{
  list<BoundaryTriangleSet*> *result = new list<BoundaryTriangleSet*>;
  LineMap::iterator FindLine;
  PointMap::iterator FindPoint;
  TriangleMap::iterator FindTriangle;
  class BoundaryPointSet *TrianglePoints[3];

  for (int i = 0; i < 3; i++) {
    FindPoint = PointsOnBoundary.find(Points[i]->nr);
    if (FindPoint != PointsOnBoundary.end()) {
      TrianglePoints[i] = FindPoint->second;
    } else {
      TrianglePoints[i] = NULL;
    }
  }

  // checks lines between the points in the Points for their adjacent triangles
  for (int i = 0; i < 3; i++) {
    if (TrianglePoints[i] != NULL) {
      for (int j = i; j < 3; j++) {
        if (TrianglePoints[j] != NULL) {
          FindLine = TrianglePoints[i]->lines.find(TrianglePoints[j]->node->nr);
          if (FindLine != TrianglePoints[i]->lines.end()) {
            for (; FindLine->first == TrianglePoints[j]->node->nr; FindLine++) {
              FindTriangle = FindLine->second->triangles.begin();
              for (; FindTriangle != FindLine->second->triangles.end(); FindTriangle++) {
                if ((
                  (FindTriangle->second->endpoints[0] == TrianglePoints[0])
                    || (FindTriangle->second->endpoints[0] == TrianglePoints[1])
                    || (FindTriangle->second->endpoints[0] == TrianglePoints[2])
                  ) && (
                    (FindTriangle->second->endpoints[1] == TrianglePoints[0])
                    || (FindTriangle->second->endpoints[1] == TrianglePoints[1])
                    || (FindTriangle->second->endpoints[1] == TrianglePoints[2])
                  ) && (
                    (FindTriangle->second->endpoints[2] == TrianglePoints[0])
                    || (FindTriangle->second->endpoints[2] == TrianglePoints[1])
                    || (FindTriangle->second->endpoints[2] == TrianglePoints[2])
                  )
                ) {
                  result->push_back(FindTriangle->second);
                }
              }
            }
            // Is it sufficient to consider one of the triangle lines for this.
            return result;

          }
        }
      }
    }
  }

  return result;
}

/** Gets the angle between a point and a reference relative to the provided center.
 * We have two shanks (point, center) and (reference, center) between which the angle is calculated
 * and by scalar product with OrthogonalVector we decide the interval.
 * @param point to calculate the angle for
 * @param reference to which to calculate the angle
 * @param center for which to calculate the angle between the vectors
 * @param OrthogonalVector points in direction of [pi,2pi] interval
 *
 * @return angle between point and reference
 */
double getAngle(const Vector &point, const Vector &reference, const Vector &center, Vector OrthogonalVector)
{
  Vector ReferenceVector, helper;
  cout << Verbose(4) << center << " is our center " << endl;
  // create baseline vector
  ReferenceVector.CopyVector(&reference);
  ReferenceVector.SubtractVector(&center);
  OrthogonalVector.MakeNormalVector(&ReferenceVector);
  cout << Verbose(4) << ReferenceVector << " is our reference " << endl;
  if (ReferenceVector.IsNull())
    return M_PI;

  // calculate both angles and correct with in-plane vector
  helper.CopyVector(&point);
  helper.SubtractVector(&center);
  if (helper.IsNull())
    return M_PI;
  double phi = ReferenceVector.Angle(&helper);
  if (OrthogonalVector.ScalarProduct(&helper) > 0) {
    phi = 2.*M_PI - phi;
  }

  cout << Verbose(3) << helper << " has angle " << phi << " with respect to reference." << endl;

  return phi;
}