TriangleMesh.cpp

/*
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003-2009    (see file CONTACT for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS is distributed in the hope that it will be usefulen,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <Utils/TriangleMesh.hh>
#include <Base/Math/Vector.hh>
#include <Base/Math/Random.hh>
#include <Geometry/RMatrix.hh>
#include <MathAlgo/Lapack.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Filter/Gauss.hh>
#include <Base/Common/FileHandling.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Geometry/HomgPlane3D.hh>
#include <Filter/Rescale.hh>
#include <Base/Math/Quadric3.hh>

#define MESH_OPTIMZER
#define USE_OPTIMIZED
#define USE_CONTRACTION_TEST
#define USE_BOUNDARY_TEST

using namespace BIAS;
using namespace std;

TriangleMesh::TriangleMesh(const double& minCornerAngle, const double& maxAngle,
                           const double depthScale) {
  SetParams(minCornerAngle, maxAngle, depthScale);
}

int TriangleMesh::ChangeTexture(const BIAS::Image<unsigned char>& texture) {
  if (!texture.SamePixelAndChannelCount(texture_)) {
    BIASERR("invalid texture size, not changing existing texture");
    return -1;
  }
  texture_ = texture;
  return 0;
}

void TriangleMesh::SetParams(const double& minCornerAngle, const double& maxAngle,
                             const double depthScale) {
  minCornerAngle_ = minCornerAngle;
  maxAngle_ = maxAngle;
  depthScale_ = depthScale;
}

/* @return plane unit 1 normal
 assuming RHS order of p1,p2,p3, e.g.
 p1 p3
 p2
 @author Jan Woetzel 06/2003
 */
inline BIAS::Vector<double> GetPlaneNormalEuclidean(const BIAS::Vector3<double> & p1,
                                                    const BIAS::Vector3<double> & p2,
                                                    const BIAS::Vector3<double> & p3) {
  BIAS::Vector3<double> nvec; // normal unit 1 result vector
  nvec = (p2 - p1).CrossProduct(p3 - p1);
  if (Equal(nvec.NormL2(), 0.0)) {
    //BIASERR("normalvector could not be calculated");
  } else
    nvec.Normalize(); // to length 1;
  return nvec;
}

/* @return radian angle between the two direction vectors
 @author JW 06/2003
 */
inline double GetAngle(const BIAS::Vector3<double> & vec1, const BIAS::Vector3<double> & vec2) {
  double cosAlpha = (vec1.GetNormalized()).ScalarProduct(vec2.GetNormalized());
  // we want an angle between 0 and Pi/2 rad (0..90 deg):
  if (cosAlpha < 0) {
    return (M_PI - acos(cosAlpha));
  } else {
    return acos(cosAlpha);
  }

}

/** @return true if triangle(p1,p2,p3) corner angles are at least minPolyAngle
@author JW 06/2003
assume a RHS of p1,p2,p3, e.g.:
p1 p3
p2
@param minPolyAngle min. angle in rad of each polygion corner.
sorts out 'stretched' triangles with acute angles (spitze Winkel)
*/
inline bool AcceptPolyCornerAngles(const BIAS::Vector3<double> & p1,
                                   const BIAS::Vector3<double> & p2,
                                   const BIAS::Vector3<double> & p3, const double minPolyAngle = 5.0
                                   / 180.0 * M_PI) {
  // check min. poly angles of corner p1, p2, p3 in RHS order:
  if (GetAngle(p2 - p1, p3 - p1) < minPolyAngle)
    return false; // angle at p1 too small
  else if (GetAngle(p3 - p2, p1 - p2) < minPolyAngle)
    return false; // angle at p2 too small
  else if (GetAngle(p1 - p3, p2 - p3) < minPolyAngle)
    return false; // angle at p3 too small
  else
    return true;
}

/** @return true if the angel between plane view ray
and plane normal is not too big.
sorts out closing of object discontinuities/occlusions.
@param p1,p2,p3 triangle corners in RHS order
@param C virtual view camera center
@param maxAngle in rad between plane normal and ray of view, e.g. 89(deg).*(M_PI/180.0)
@author JW 06/2003 */
inline bool AcceptPolyPlanViewAngle(const BIAS::Vector3<double> & p1,
                                    const BIAS::Vector3<double> & p2,
                                    const BIAS::Vector3<double> & p3,
                                    const BIAS::Vector3<double> & C, const double maxAngle = 89.0
                                    / 180.0 * M_PI) {
  // check angle between plane normal and ray of view from C to nw:
  // view on p1 (may be any vertex)
  double angle = GetAngle(p1 - C, GetPlaneNormalEuclidean(p1, p2, p3));
  return (angle <= maxAngle);
}

int TriangleMesh::GenerateDenseMesh(const BIAS::Image<float>& DenseDepthMap,
                                    const BIAS::Image<unsigned char>& Texture) {
  #ifdef BIAS_DEBUG
  if( (DenseDepthMap.GetWidth()!=Texture.GetWidth()) ||
      (DenseDepthMap.GetHeight()!=Texture.GetHeight()) ) {
    BIASERR("Texturesize != DepthMapsize \n");
    return -1;
  }
  #endif
  //init datastructs
  unsigned int w = DenseDepthMap.GetWidth();
  unsigned int h = DenseDepthMap.GetHeight();
  vector<int> pointIndices;
  pointIndices.resize(w * h);

  meshVertices_.clear();
  vertexNormals_.clear();
  textureCoords_.clear();
  triangleIndices_.clear();

  meshVertices_.reserve(w * h);
  vertexNormals_.reserve(w * h);
  textureCoords_.reserve(w * h);
  triangleIndices_.reserve(w * h * 2);

  texture_ = Texture;

  //build list of indices and list of Vertices
  float texCoordOffsetX = 1.0f / float(w - 1);
  float texCoordOffsetY = 1.0f / float(h - 1);
  Vector2<float> texCoord;
  texCoord.Set(0.0, 1.0);

  Vector3<double> vertex;
  HomgPoint3D vertexHom;

  const float** depthValues = DenseDepthMap.GetImageDataArray();
  unsigned int currentIndex = 0;
  unsigned int x, y;
  for (y = 0; y < h; y++) {
    for (x = 0; x < w; x++) {
      if (depthValues[y][x] <= 0.0) {
        pointIndices[y * w + x] = -1;
      } else {
        pointIndices[y * w + x] = currentIndex;
        vertexHom = HomgPoint3D((double) x, (double) y, depthScale_ * (double) depthValues[y][x]);
        vertexHom.GetEuclidean(vertex);
        meshVertices_.push_back(vertex);
        textureCoords_.push_back(texCoord);
        currentIndex++;
      }
      texCoord[0] += texCoordOffsetX;
    }
    texCoord[0] = 0.0;
    texCoord[1] -= texCoordOffsetY;
  }

  //meshing
  Vector3<int> triangle;
  int index0, index1, index2;
  for (y = 0; y < h; y++) {
    for (x = 0; x < w - 1; x++) {
      /*         v1
       *      i0---i1
       *       |   /
       *       |  / v0
       *       i2
       */
      if (y < h - 1) {
        index0 = pointIndices[y * w + x];
        index1 = pointIndices[y * w + x + 1];
        index2 = pointIndices[(y + 1) * w + x];
        if (index0 > (-1) && index1 > (-1) && index2 > (-1)) {
          triangle.Set(index2, index1, index0);
          if (AcceptPolyCornerAngles(meshVertices_[index0], meshVertices_[index1],
                                     meshVertices_[index2], minCornerAngle_))
            triangleIndices_.push_back(triangle);
        }
      }
      /*
       *         i0
       *        / |
       *       /  |
       *      i2--i1
       */
      if (y > 0) {
        index2 = pointIndices[y * w + x];
        index1 = pointIndices[y * w + x + 1];
        index0 = pointIndices[(y - 1) * w + x + 1];
        if (index0 > (-1) && index1 > (-1) && index2 > (-1)) {
          triangle.Set(index2, index1, index0);
          if (AcceptPolyCornerAngles(meshVertices_[index0], meshVertices_[index1],
                                     meshVertices_[index2], minCornerAngle_))
            triangleIndices_.push_back(triangle);
        }
      }
    }

    //   cout<<"vertices "<<meshVertices_.size()<<endl;
    //   cout<<"texCoords "<<textureCoords_.size()<<endl;
    //   cout<<"triangles "<<triangleIndices_.size()<<endl;

  }

  if (triangleIndices_.empty())
    return -1;
  return 0;

}

int TriangleMesh::GenerateDenseMesh(const BIAS::Image<float>& DenseDepthMap,
                                    BIAS::Projection& P,
                                    const BIAS::Image<unsigned char>& Texture,
                                    BIAS::Projection& TexP,
                                    bool reduce,
                                    double reduceValue){

  GenerateDenseMesh(DenseDepthMap,P,Texture,P);
  if(reduce){
#ifdef MESH_OPTIMZER
    TriangleMesh::SimplifyMeshSurface_(Texture, TexP, reduceValue);
#endif
  }
  return 0;
}

int TriangleMesh::GenerateDenseMesh(const BIAS::Image<float>& DenseDepthMap,
                                    BIAS::PMatrix& P,
                                    const BIAS::Image<unsigned char>& Texture) {
  ProjectionParametersPerspective ppp;
  ppp.SetImageSize(Texture.GetWidth(), Texture.GetHeight());
  ppp.SetP(P);
  Projection pP(ppp);
  return GenerateDenseMesh(DenseDepthMap, pP, Texture, pP);
}

int TriangleMesh::GenerateDenseMesh(const BIAS::Image<float>& DenseDepthMap,
                                    BIAS::Projection& P,
                                    const BIAS::Image<unsigned char>& Texture,
                                    BIAS::Projection& TexP,
                                    const unsigned int distBetweenPoints) {
  unsigned int w = DenseDepthMap.GetWidth();
  unsigned int h = DenseDepthMap.GetHeight();
  return GenerateDenseMesh(DenseDepthMap, P, Texture, TexP, 0, 0, w, h, distBetweenPoints);
}

Vector2<float> ImageToTexCoordinates(float x0, float y0, unsigned int w, unsigned int h) {
  float texCoordOffsetX = 1.0f / float(w - 1);
  float texCoordOffsetY = 1.0f / float(h - 1);
  Vector2<float> texCoord;
  texCoord.Set(x0 * texCoordOffsetX, 1.0f - y0 * texCoordOffsetY);
  return texCoord;
}

int TriangleMesh::GenerateDenseMesh(const BIAS::Image<float>& DenseDepthMap,
                                    BIAS::Projection& P,
                                    const BIAS::Image<unsigned char>& Texture,
                                    BIAS::Projection& TexP, const unsigned int x0,
                                    const unsigned int y0, const unsigned int width,
                                    const unsigned int height,
                                    const unsigned int distBetweenPoints) {
  Vector3<double> C = P.GetC();

  if (width <= 0 || height <= 0) {
    BIASERR("tile width is too small!");
    return -1;
  }

  //init datastructs
  //unsigned int dw=DenseDepthMap.GetWidth();
  //unsigned int dh=DenseDepthMap.GetHeight();
  unsigned int tw = Texture.GetWidth();
  unsigned int th = Texture.GetHeight();
  vector<int> pointIndices;
  pointIndices.resize(width * height);

  meshVertices_.clear();
  vertexNormals_.clear();
  textureCoords_.clear();
  triangleIndices_.clear();

  meshVertices_.reserve(width * height);
  vertexNormals_.reserve(width * height);
  textureCoords_.reserve(width * height);
  triangleIndices_.reserve(width * height * 2);

  texture_ = Texture;

  //build list of indices and list of Vertices
  //    float texCoordOffsetX = 1.0f / float(tw-1);
  //    float texCoordOffsetY = 1.0f / float(th-1);
  Vector2<float> texCoord;
  //    texCoord.Set(x0 * texCoordOffsetX, 1.0f - y0 * texCoordOffsetY);

  Vector3<double> vertex;

  const float** depthValues = DenseDepthMap.GetImageDataArray();
  unsigned int currentIndex = 0;
  unsigned int x, y;

  for (y = 0; y < height; y++) {
    for (x = 0; x < width; x++) {
      if (depthValues[y0 + y][x0 + x] < 1e-5) {
        pointIndices[y * width + x] = -1;
      } else {
        //          BIASASSERT(depthValues[y][x]>1e-5);
        vertex = P.UnProjectToPoint(HomgPoint2D(x + x0, y + y0), depthScale_
                                    * depthValues[y + y0][x0 + x]);
        //          BIASASSERT((vertex-P.GetC()).NormL2()>1e-10)
        if (vertex.NormL2() > 1e-10) {
          //generate texture coordinates
          HomgPoint2D uv;
          HomgPoint3D p3dh(vertex);
          if (TexP.DoesPointProjectIntoImage(p3dh, uv)) {
            double u = uv[0] / float(tw - 1);
            double v = 1 - uv[1] / float(th - 1);
            texCoord.Set(float(u), float(v));
          } //... or else?

          meshVertices_.push_back(vertex);
          textureCoords_.push_back(texCoord);
          pointIndices[y * width + x] = currentIndex;
          currentIndex++;
        } else {
          pointIndices[y * width + x] = -1;
          //cout<<"***vertex invalid***"<<currentIndex<<" "<<endl;
        }
      }
      BIASASSERT(meshVertices_.size()==currentIndex);
      //   texCoord[0] += texCoordOffsetX;
      if (!meshVertices_.empty()) {
        BIASASSERT(meshVertices_.back().NormL2()>1e-10);
      }

    }
  }
  //cout<<"size of vertex array is "<<meshVertices_.size()<<endl;
  //meshing
  Vector3<int> triangle;
  int index0, index1, index2;
  int rejected = 0;
  for (y = 0; y < height; y += distBetweenPoints) {
    for (x = 0; x < width - 1; x += distBetweenPoints) {
      /*         v1
       *      i0---i1
       *       |   /
       *       |  / v0
       *       i2
       */
      if (y < height - 1) {
        index0 = pointIndices[y * width + x];
        index1 = pointIndices[y * width + x + 1];
        index2 = pointIndices[(y + 1) * width + x];
        if (index0 > (-1) && index1 > (-1) && index2 > (-1)) {

          //BIASASSERT(index1<meshVertices_.size());
          //BIASASSERT(index2<meshVertices_.size());
          //BIASASSERT(index0<meshVertices_.size());

          //BIASASSERT((meshVertices_[index0]-meshVertices_[index1]).NormL2()>1e-10);
          //BIASASSERT((meshVertices_[index2]-meshVertices_[index1]).NormL2()>1e-10);
          //BIASASSERT((meshVertices_[index0]-meshVertices_[index2]).NormL2()>1e-10);
          //BIASASSERT(meshVertices_[index0].NormL2()>1e-10);
          //BIASASSERT(meshVertices_[index1].NormL2()>1e-10);
          //BIASASSERT(meshVertices_[index2].NormL2()>1e-10);

          triangle.Set(index2, index1, index0);
          //      triangleIndices_.push_back(Vector<unsigned int>(triangle));
          if (AcceptPolyCornerAngles(meshVertices_[index0], meshVertices_[index1],
                                     meshVertices_[index2], minCornerAngle_)
              && AcceptPolyPlanViewAngle(meshVertices_[index0], meshVertices_[index1],
                                         meshVertices_[index2], C, maxAngle_)) {
            triangleIndices_.push_back(triangle);
          } else {
            rejected++;
          }
        }
      }
      /*
       *         i0
       *        / |
       *       /  |
       *      i2--i1
       */
      if (y > 0) {
        index2 = pointIndices[y * width + x];
        index1 = pointIndices[y * width + x + 1];
        index0 = pointIndices[(y - 1) * width + x + 1];
        if (index0 > (-1) && index1 > (-1) && index2 > (-1)) {
          triangle.Set(index2, index1, index0);
          //      triangleIndices_.push_back(Vector<unsigned int>(triangle));
          if (AcceptPolyCornerAngles(meshVertices_[index0], meshVertices_[index1],
                                     meshVertices_[index2], minCornerAngle_)
              && AcceptPolyPlanViewAngle(meshVertices_[index0], meshVertices_[index1],
                                         meshVertices_[index2], C, maxAngle_)) {
            triangleIndices_.push_back(triangle);
          } else {
            rejected++;
          }
        }
      }
    }
  }

  //      cout<<"vertices "<<meshVertices_.size()<<endl;
  //      cout<<"texCoords "<<textureCoords_.size()<<endl;
  //      cout<<"triangles "<<triangleIndices_.size()<<endl;
  //      cout<<"rejected "<<rejected<<" triangles due to invalid angles\n";
  if (triangleIndices_.empty()) {
    //BIASERR("no triangles generated");
    return -1;
  }

  return 0;
}


int TriangleMesh::GenerateDenseMesh(const Image<float>& DenseDepthMapC,
                                    PMatrix& P,
                                    const Image<unsigned char>& TextureC,
                                    const ROI& CutOut) {
  // dirty de-const cast to conveniently access ROI, which is restored later
  Image<float>& DenseDepthMap(const_cast<Image<float>&> (DenseDepthMapC));
  Image<unsigned char>& Texture(const_cast<Image<unsigned char>&> (TextureC));

  int UpperLeftX, UpperLeftY, LowerRightX, LowerRightY;
  CutOut.GetCorners(UpperLeftX, UpperLeftY, LowerRightX, LowerRightY);

  // adapt pmatrix principal point
  PMatrix PCutOut(P);
  if (PCutOut.IsMetric()) {
    //  more exact to use saved K,R,C and recompose than multiplying
    KMatrix K = PCutOut.GetK();
    K[0][2] -= double(UpperLeftX);
    K[1][2] -= double(UpperLeftY);
    RMatrix R = PCutOut.GetR();
    Vector3<double> C(PCutOut.GetC());
    PCutOut.Compose(K, R, C);
  } else {
    // non-metrix case, just multiply with correction matrix
    KMatrix K(MatrixIdentity);
    K[0][2] -= double(UpperLeftX);
    K[1][2] -= double(UpperLeftY);
    PCutOut = K * PCutOut;

    // tell PMatrix that a basic matrix operation has been invoked
    // and chache is invalid
    PCutOut.InvalidateDecomposition();
  }
  // PCutout is now the new projection matrix for the cutout

  // get CutOut from texture
  ROI backupROI = *Texture.GetROI();
  Texture.SetROICorners(UpperLeftX, UpperLeftY, LowerRightX, LowerRightY);
  Image<unsigned char> CutOutTexture;
  Texture.GetCopyOfROI(CutOutTexture);
  *Texture.GetROI() = backupROI;

  // get CutOut from depth map
  backupROI = *DenseDepthMap.GetROI();
  DenseDepthMap.SetROICorners(UpperLeftX, UpperLeftY, LowerRightX, LowerRightY);
  Image<float> CutOutDenseDepthMap;
  DenseDepthMap.GetCopyOfROI(CutOutDenseDepthMap);
  *DenseDepthMap.GetROI() = backupROI;

  // call generic interface
  return GenerateDenseMesh(CutOutDenseDepthMap, PCutOut, CutOutTexture);
}

int TriangleMesh::GenerateImagePlane(const BIAS::Projection& P,
                                     const BIAS::Image<unsigned char>& Texture,
                                     const double& w) {

  if (typeid(*P.GetParameters()) != typeid(ProjectionParametersPerspective)) {
    BIASWARN("Interpolation of texture coordinates not yet implemented !");
  }
  HomgPoint2D x(0.0, 0.0);
  Vector2<float> texcoord(0.0, 1.0);
  for (unsigned int i = 0; i < 4; i++) {
    switch (i) {
      case 1: {
        x = HomgPoint2D(Texture.GetWidth() - 1, 0);
        texcoord = Vector2<float> (1.0, 1.0);
        break;
      }
      case 2: {
        x = HomgPoint2D(0, Texture.GetHeight() - 1);
        texcoord = Vector2<float> (0.0, 0.0);
        break;
      }
      case 3: {
        x = HomgPoint2D(Texture.GetWidth() - 1, Texture.GetHeight() - 1);
        texcoord = Vector2<float> (1.0, 0.0);
        break;
      }
    }
    Vector3<double> eucl;
    P.GetParameters()->UnProjectToImagePlane(x, w).GetEuclidean(eucl);
    meshVertices_.push_back(eucl);
    textureCoords_.push_back(texcoord);
  }
  Vector3<int> triangle;
  // two triangles
  triangle.Set(0, 1, 2);
  triangleIndices_.push_back(triangle);
  triangle.Set(1, 2, 3);
  triangleIndices_.push_back(triangle);
  texture_ = Texture;

  return 0;
}


void TriangleMesh::GenerateTexturedCamera(const ProjectionParametersBase* rectPPB,
                                          Image<unsigned char>& rectB,
                                          const double& scale,
                                          const double& opacity,
                                          const double& resolution) {

  ProjectionParametersBase* newparams = rectPPB->Clone();
  if (resolution != 1.0)
    newparams->Rescale(float(resolution));

  BIASASSERT(opacity<=1.0)
  unsigned int width = 0, height = 0;
  newparams->GetImageSize(width, height);
  Image<float> depthmap(width, height, 1);
  depthmap.SetZero();
  for (unsigned int y = 0; y < height; y++) {
    for (unsigned int x = 0; x < width; x++) {
      HomgPoint2D p2(x, y);
      HomgPoint3D p3 = newparams->UnProjectToImagePlane(p2, scale);
      if (p3.NormL2() < 0.1)
        continue;
      depthmap.GetImageDataArray()[y][x] = (float) (p3 - HomgPoint3D(rectPPB->GetC())).NormL2();

    }
  }

  BIASASSERT(rectB.GetChannelCount()==3);
  Rescale<unsigned char, unsigned char> R;
  Image<unsigned char> rect(width, height, rectB.GetChannelCount());
  R.SetFactor(resolution);
  R.Filter(rectB, rect);

  Projection Proj(*newparams);

  Image<unsigned char> rectAlpha(rect.GetWidth(), rect.GetHeight(), 4, true);

  rectAlpha.SetColorModel(ImageBase::CM_RGBA);
  for (unsigned int y = 0; y < rectAlpha.GetHeight(); y++) {
    for (unsigned int x = 0; x < rectAlpha.GetWidth(); x++) {
      unsigned char *pD = &rect.GetImageDataArray()[y][3 * x];
      unsigned char *pDA = &rectAlpha.GetImageDataArray()[y][4 * x];
      *pDA++ = *pD++;
      *pDA++ = *pD++;
      *pDA++ = *pD++;
      *pDA++ = (unsigned char) (255.0 * opacity);
    }
  }

  GenerateDenseMesh(depthmap, Proj, rectAlpha, Proj);
  delete newparams;
}

/* take 5 points: 4 corners + center, calculate normals for 4 triangles,
     compare normals and if one angle is above threshold, refine recursivly.
     If depth values of one point is missing, refine, too
 */
bool TriangleMesh::MakeTriangles_(const Projection& P, const Projection& TexP,
                                  const BIAS::Image<unsigned char>& Texture,
                                  std::vector<Image<float> > & depthmaps, float x, float y,
                                  float offset, float minoffset, double minCosAngle,
                                  double maxDiagRatio, bool simulateOnly) {

  const int maxlayer = depthmaps.size() - 1;
  int step = int(rint(log(offset) / log(2.0))) - 2;
  if (step > maxlayer)
    step = maxlayer;
  if (step < 0)
    step = 0;

  Image<float> &depthmap = depthmaps[step];
  HomgPoint2D S[5];
  Vector3<double> qp[5];
  Vector3<double> normal[4];
  bool vetoed = true;
  unsigned int FoundDepthValues = 0;
  unsigned int c;
  float** depthValues = depthmap.GetImageDataArray();
  for (c = 0; c < 5; c++) {
    S[c][2] = 1.0;
    switch (c) {
      case 0:
        S[c][0] = x;
        S[c][1] = y;
        break;
      case 1:
        S[c][0] = x - offset;
        S[c][1] = y - offset;
        break;
      case 2:
        S[c][0] = x - offset;
        S[c][1] = y + offset;
        break;
      case 3:
        S[c][0] = x + offset;
        S[c][1] = y + offset;
        break;
      case 4:
        S[c][0] = x + offset;
        S[c][1] = y - offset;
        break;
    }
    double depth = depthValues[int(S[c][1])][int(S[c][0])];
    if (depth < 1e-10) {
      // invalid depth
      break;
    }

    HomgPoint3D backProjected;
    backProjected = P.GetParameters()->UnProjectToPoint(S[c], depthScale_
                                                        * depthValues[int(S[c][1])][int(S[c][0])]);
    if (backProjected.NormL2() < 1e-10) {
      // point is not backprojectable, e.g. out of sphere circle
      break;
      //cout<<"***vertex invalid***"<<currentIndex<<" "<<endl;
    }
    backProjected.GetEuclidean(qp[c]);
    FoundDepthValues++;
  } // for c=1 to 5

  // check if loop terminated normaly or break was called
  double cosang[6];
  double diag1;
  double diag2, diagratio;

  bool passedMinAngleTest = true;
  
  Vector3<double> meannormal(0, 0, 0);
  Vector3<double> LoS;
  // add all single-pixel quads
  if (FoundDepthValues == 5) {
    vetoed = false;
    // calculate 4 normals
    (qp[1] - qp[0]).CrossProduct((qp[2] - qp[0]), normal[0]);
    (qp[2] - qp[0]).CrossProduct((qp[3] - qp[0]), normal[1]);
    (qp[3] - qp[0]).CrossProduct((qp[4] - qp[0]), normal[2]);
    (qp[4] - qp[0]).CrossProduct((qp[1] - qp[0]), normal[3]);
    for (unsigned int i = 0; i < 4; i++) {
      double mynorm = normal[i].NormL2();
      if (mynorm < 1e-4)
        mynorm = 1e-4;
      normal[i] /= mynorm;
      meannormal += normal[i];
    }
    meannormal /= 4.0;

    // compare normals

    normal[0].ScalarProduct(normal[1], cosang[0]);
    normal[0].ScalarProduct(normal[2], cosang[1]);
    normal[0].ScalarProduct(normal[3], cosang[2]);
    normal[1].ScalarProduct(normal[2], cosang[3]);
    normal[1].ScalarProduct(normal[3], cosang[4]);
    normal[2].ScalarProduct(normal[3], cosang[5]);
    for (unsigned int c = 0; c < 6; c++) {
      if (cosang[c] < minCosAngle)
        vetoed = true;
    }
    // 25 deg: 0.9, 18 deg: 0.95, 8 deg: 0.96 5 deg: 0.996
    diag1 = (qp[1] - qp[3]).NormL2();
    diag2 = (qp[2] - qp[4]).NormL2();
    diagratio = diag1 > diag2 ? diag1 / diag2 : diag2 / diag1;
    if (diagratio > maxDiagRatio) {
      vetoed = true;
    }
    LoS = qp[0] - P.GetC();
    LoS.Normalize();
    if (GetAngle(meannormal, LoS) > maxAngle_) {
      // too bad viewing angle
      FoundDepthValues = 0; // reject even if smallest size !
    }
    if((!AcceptPolyCornerAngles(qp[0], qp[1], qp[2], minCornerAngle_)) ||
       (!AcceptPolyCornerAngles(qp[0], qp[2], qp[3], minCornerAngle_)) || 
       (!AcceptPolyCornerAngles(qp[0], qp[3], qp[4], minCornerAngle_)) || 
       (!AcceptPolyCornerAngles(qp[0], qp[4], qp[1], minCornerAngle_))){
       passedMinAngleTest = false;
    }
  }
  if(passedMinAngleTest){
  if ((FoundDepthValues == 5) && ((!vetoed) || offset <= minoffset)) {

    float newx, newy, newoff;
    newoff = offset / 2;
    newx = x - newoff;
    newy = y - newoff;

    if ((newoff <= minoffset) && simulateOnly)
      return true;

    bool result = true;

    MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset, minCosAngle,
                   maxDiagRatio, true);
    newy = y + newoff;
    result &= MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset,
                             minCosAngle, maxDiagRatio, true);
    newx = x + newoff;
    result &= MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset,
                             minCosAngle, maxDiagRatio, true);
    newy = y - newoff;
    result &= MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset,
                             minCosAngle, maxDiagRatio, true);

    if (simulateOnly)
      return result;

    if (result) {

      //cout<<"allow at "<<newoff<<endl;
    }

    if (result || (newoff <= minoffset)) {
      // we are allowed to place the quad !
      // add this quad to vertarray
      vector<int> indices;
      for (unsigned int c = 1; c < 5; c++) {
        int found = -1;
        //         Vector2<float> thecoords(ImageToTexCoordinates(float(S[c][0]),
        //                                                        float(S[c][1]),
        //                                                        depthmap.GetWidth(),
        //                                                        depthmap.GetHeight()));

        //generate texture coordinates
        HomgPoint2D uv;
        HomgPoint3D p3dh(qp[c]);
        Vector2<float> thecoords;
        unsigned int tw = Texture.GetWidth();
        unsigned int th = Texture.GetHeight();
        if (TexP.DoesPointProjectIntoImage(p3dh, uv)) {
          thecoords[0] = float(uv[0]) / float(tw - 1);
          thecoords[1] = 1.0f - float(uv[1]) / float(th - 1);
        } else {
          BIASWARNONCE("point does not project into texture!!!!");
        }

        //create texture coordinates
        for (unsigned int f = 0; f < textureCoords_.size(); f++) {
          if (textureCoords_[f][0] == thecoords[0] && textureCoords_[f][1] == thecoords[1]) {
            found = f;
            break;
          }
        }
        if (found > -1) {
          // this vertex already exists ...
          indices.push_back(found);
        } else {
          meshVertices_.push_back(qp[c]);
          textureCoords_.push_back(thecoords);
          indices.push_back(currentIndex_++);
        }
      }

      Vector3<int> triangle;
      //triangle.Set(currentIndex_-4, currentIndex_-3, currentIndex_-2);
      triangle.Set(indices[0], indices[1], indices[2]);
      triangleIndices_.push_back(triangle);
      //triangle.Set(currentIndex_-2, currentIndex_-1, currentIndex_-4);
      triangle.Set(indices[2], indices[3], indices[0]);
      triangleIndices_.push_back(triangle);

      HomgPlane3D thePlaneA;
      thePlaneA.SetFromPoints(HomgPoint3D(qp[1]), HomgPoint3D(qp[2]), HomgPoint3D(qp[3]));
      int x = int(S[0][0] - offset);
      int y = 0;
      Vector3<double> p, dir;
      for (y = int(S[0][1] - offset); y <= int(S[0][1] + offset); y++) {
        P.UnProjectToRay(HomgPoint2D(x, y), p, dir);
        HomgPoint3D Ray(dir);
        Ray[3] = 0;
        HomgPoint3D Intersection(0, 0, 0, 0);
        thePlaneA.GetLineIntersection(HomgPoint3D(P.GetC()), Ray, Intersection);
        if (depthmaps[0].IsPositionInImage(x, y))
          for (unsigned int i = 0; i < depthmaps.size(); i++) {
            depthmaps[i].GetImageDataArray()[y][x] = float((HomgPoint3D(P.GetC())
                - Intersection).NormL2());
          }
      }
      y--;
      for (x = int(S[0][0] - offset); x <= int(S[0][0] + offset); x++) {
        P.UnProjectToRay(HomgPoint2D(x, y), p, dir);
        HomgPoint3D Ray(dir);
        Ray[3] = 0;
        HomgPoint3D Intersection(0, 0, 0, 0);
        thePlaneA.GetLineIntersection(HomgPoint3D(P.GetC()), Ray, Intersection);
        if (depthmaps[0].IsPositionInImage(x, y))
          for (unsigned int i = 0; i < depthmaps.size(); i++) {
            depthmaps[i].GetImageDataArray()[y][x] = float((HomgPoint3D(P.GetC())
                - Intersection).NormL2());
          }
      }
      x--;
      thePlaneA.SetFromPoints(HomgPoint3D(qp[3]), HomgPoint3D(qp[4]), HomgPoint3D(qp[1]));
      for (; y >= int(S[0][1] - offset); y--) {
        P.UnProjectToRay(HomgPoint2D(x, y), p, dir);
        HomgPoint3D Ray(dir);
        Ray[3] = 0;
        HomgPoint3D Intersection(0, 0, 0, 0);
        thePlaneA.GetLineIntersection(HomgPoint3D(P.GetC()), Ray, Intersection);
        if (depthmaps[0].IsPositionInImage(x, y))
          for (unsigned int i = 0; i < depthmaps.size(); i++) {
            depthmaps[i].GetImageDataArray()[y][x] = float((HomgPoint3D(P.GetC())
                - Intersection).NormL2());
          }
      }
      y++;
      for (; x >= int(S[0][0] - offset); x--) {
        P.UnProjectToRay(HomgPoint2D(x, y), p, dir);
        HomgPoint3D Ray(dir);
        Ray[3] = 0;
        HomgPoint3D Intersection(0, 0, 0, 0);
        thePlaneA.GetLineIntersection(HomgPoint3D(P.GetC()), Ray, Intersection);
        if (depthmaps[0].IsPositionInImage(x, y))
          for (unsigned int i = 0; i < depthmaps.size(); i++) {
            depthmaps[i].GetImageDataArray()[y][x] = float((HomgPoint3D(P.GetC())
                - Intersection).NormL2());
          }
      }
      return true;
    }
  }
  if (offset > (minoffset)) {
    if (simulateOnly)
      return false;
    float newx, newy, newoff;
    newoff = offset / 2;
    newx = x - newoff;
    newy = y - newoff;

    MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset, minCosAngle,
                   maxDiagRatio, false);
    newy = y + newoff;
    MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset, minCosAngle,
                   maxDiagRatio, false);
    newx = x + newoff;
    MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset, minCosAngle,
                   maxDiagRatio, false);
    newy = y - newoff;
    MakeTriangles_(P, TexP, Texture, depthmaps, newx, newy, newoff, minoffset, minCosAngle,
                   maxDiagRatio, false);

    return false;
  }
  }
  return true;
}


int TriangleMesh::
GenerateSimplifiedMesh(const BIAS::Image<float>& DenseDepthMap,
                       const BIAS::Projection& P,
                       const BIAS::Image<unsigned char>& Texture,
                       const Projection& TexP,
                       const unsigned int x0, const unsigned int y0,
                       const unsigned int width, const unsigned int height,
                       double minCosAngle,
                       double maxDiagRatio)
{
  // some sanity checks
  unsigned mwidth, mheight;
  BIASASSERT(P.GetParameters());
  P.GetParameters()->GetImageSize(mwidth, mheight);
  BIASASSERT(DenseDepthMap.GetWidth()==mwidth &&
             DenseDepthMap.GetHeight()==mheight);
  BIASASSERT(width==mwidth && height==mheight);
  BIASASSERT(TexP.GetParameters());
  TexP.GetParameters()->GetImageSize(mwidth, mheight);
  BIASASSERT(Texture.GetWidth()==mwidth &&
             Texture.GetHeight()==mheight);

  // check whether straight lines in space map to straight lines in the image
  // if not, we may get holes
  const ProjectionParametersPerspective *ppp =
    dynamic_cast<const ProjectionParametersPerspective *> (P.GetParameters());

  // from all our cam models, only the perspectives preserve straight lines
  bool straightlinesPreserved = (ppp != NULL);
  if (ppp != NULL) {
    // and only if they dont contain distortion
    straightlinesPreserved &= (!ppp->IsDistorted());
  }
  if (!straightlinesPreserved) {
    BIASWARN("Your projection does not relate straight lines in the image to "
             "straight lines in space, which leads to the problem that "
             "the 3D triangle tesselation may contain holes, since it is done "
             "in the image. ");
    // TODO !!!
    // here it would be best to cube-map the depth (in case of wide-angle
    // fisheye) or if fov<120 to map the depth onto an ideal perspective

    // for (cubeface=1 to 5 do)
    //   P.GetIdealK();
    //   ProjectionMapping.map(origdepth, idealdepth);

    // then, in the code below, simply make distinction between texture and
    // depth when computing texture coordinates and generate trianglemesh
    // for each cubeface

  }

  currentIndex_ = 0;
  Vector3<double> C = P.GetC();
  //   if( ( (DenseDepthMap.GetWidth()!=Texture.GetWidth()) ||
  //         (DenseDepthMap.GetHeight()!=Texture.GetHeight()) )
  //      && ( !(Texture.IsEmpty()) )
  //     ) {
  //     BIASWARN("Texturesize != DepthMapsize \n");
  //   }
  if (width <= 0 || height <= 0) {
    BIASERR("tile width is too small!");
    return -1;
  }

  vector<Image<float> > depthimages;

  int maxlevel = int(log(double(DenseDepthMap.GetWidth())) / log(2.0) - 3);
  int spacing = int(pow(2.0, maxlevel));
  cout << "maximum level is " << maxlevel << " spacing is " << spacing << endl;
  depthimages.reserve(maxlevel + 1);
  depthimages.push_back(DenseDepthMap);

  Gauss<float, float> DepthSmooth;
  bool smoothOriginal = true;
  if (smoothOriginal) {
    DepthSmooth.SetSigma(1.0);
    depthimages.back().SetZero();
    DepthSmooth.Filter7x7GreyIgnoreBelowThreshold(DenseDepthMap, depthimages.back(), 0.0f);
  }

  double curSigma = 0.5;
  for (int curLevel = 0; curLevel < maxlevel; curLevel++) {
    double neededSigma = sqrt(pow(2.0, double(2 * curLevel)) - curSigma * curSigma);
    cout << "Filtering with sigma " << neededSigma << endl;
    curSigma = pow(2.0, double(curLevel));
    DepthSmooth.SetSigma(neededSigma);
    depthimages.push_back(depthimages.back());
    depthimages.back().SetZero();
    DepthSmooth.Filter7x7GreyIgnoreBelowThreshold(*(depthimages.rbegin() + 1),
                                                    depthimages.back(), 0.0f);
  }
  // for (int curLevel = 0; curLevel<maxlevel; curLevel++) {
  //   ImageIO::Write("depth"+toString(curLevel),depthimages[curLevel]);
  // }

  meshVertices_.clear();
  vertexNormals_.clear();
  textureCoords_.clear();
  triangleIndices_.clear();

  meshVertices_.reserve(width * height);
  vertexNormals_.reserve(width * height);
  textureCoords_.reserve(width * height);
  triangleIndices_.reserve(width * height * 2);

  texture_ = Texture;

  unsigned int x, y;
  int minoffset = 2;
  for (y = spacing + 1; y < height - spacing; y += spacing) {
    for (x = spacing + 1; x < width - spacing; x += spacing) {
      MakeTriangles_(P, TexP, Texture, depthimages, float(x), float(y), float(spacing),
                     float(minoffset), minCosAngle, maxDiagRatio, false);
    }
  }

  bool multiPass = true;
  if (multiPass) {
    for (int curLevel = 2; curLevel < maxlevel; curLevel++) {
      cout << "pass " << curLevel << " of " << maxlevel - 1 << " ..." << flush << endl;
      meshVertices_.clear();
      vertexNormals_.clear();
      textureCoords_.clear();
      triangleIndices_.clear();
      currentIndex_ = 0;
      // second path with consistent depth maps
      for (y = spacing + 1; y < height - spacing; y += spacing) {
        for (x = spacing + 1; x < width - spacing; x += spacing) {
          MakeTriangles_(P, TexP, Texture, depthimages, float(x), float(y), float(spacing),
                         float(minoffset), minCosAngle, maxDiagRatio, false);

        }
      }
    }
  }

  //      cout<<"vertices "<<meshVertices_.size()<<endl;
  //      cout<<"texCoords "<<textureCoords_.size()<<endl;
  //      cout<<"triangles "<<triangleIndices_.size()<<endl;
  //      cout<<"rejected "<<rejected<<" triangles due to invalid angles\n";
  if (triangleIndices_.empty()) {
    BIASERR("no triangles generated");
    return -1;
  }
  return 0;
}


int TriangleMesh::
GenerateTexturedQuad(const Image<unsigned char>& Texture,
                     const Vector3<double>& UL, const Vector3<double>& UR,
                     const Vector3<double>& LL, const Vector3<double>& LR,
                     const Vector2<double>& ul, const Vector2<double>& ur,
                     const Vector2<double>& ll, const Vector2<double>& lr,
                     const Vector3<double>& normal)
{
  const bool doTexture = Texture.GetWidth() > 0;
  const float w = Texture.GetWidth() - 1.0f;
  const float h = Texture.GetHeight() - 1.0f;

  meshVertices_.clear();
  vertexNormals_.clear();
  textureCoords_.clear();
  triangleIndices_.clear();

  meshVertices_.push_back(UL);
  meshVertices_.push_back(UR);
  meshVertices_.push_back(LL);
  meshVertices_.push_back(LR);

  const double normalLength = normal.NormL2();
  if (BIAS::Equal(normalLength, 1.0)) {
    for (unsigned int i = 0; i < meshVertices_.size(); i++)
      vertexNormals_.push_back(normal);
  }

  if (doTexture)
  {
    textureCoords_.push_back(Vector2<float> ((float) ul[0] / w, 1.0f - (float) ul[1] / h));
    textureCoords_.push_back(Vector2<float> ((float) ur[0] / w, 1.0f - (float) ur[1] / h));
    textureCoords_.push_back(Vector2<float> ((float) ll[0] / w, 1.0f - (float) ll[1] / h));
    textureCoords_.push_back(Vector2<float> ((float) lr[0] / w, 1.0f - (float) lr[1] / h));
    texture_ = Texture;
  }

  Vector<int> triangle(3);
  triangle[0] = 0;
  triangle[1] = 1;
  triangle[2] = 2;
  triangleIndices_.push_back(triangle);
  triangle[0] = 1;
  triangle[1] = 3;
  triangle[2] = 2;
  triangleIndices_.push_back(triangle);

  return 0;
}


int TriangleMesh::
GenerateTexturedQuad(PMatrix P, const Image<unsigned char>& Texture,
                     const Vector3<double>& UL, const Vector3<double>& UR,
                     const Vector3<double>& LL, const Vector3<double>& LR,
                     const Vector3<double>& normal)
{
  const float w = Texture.GetWidth() - 1.0f;
  const float h = Texture.GetHeight() - 1.0f;

  HomgPoint2D ul(P * HomgPoint3D(UL));
  HomgPoint2D ur(P * HomgPoint3D(UR));
  HomgPoint2D ll(P * HomgPoint3D(LL));
  HomgPoint2D lr(P * HomgPoint3D(LR));

  if (P.NormFrobenius() < 1e-9) {
    ul.Set(0, 0);
    ur.Set(w, 0);
    ll.Set(0, h);
    lr.Set(w, h);
  } else {
    ul.Set(P * HomgPoint3D(UL));
    ur.Set(P * HomgPoint3D(UR));
    ll.Set(P * HomgPoint3D(LL));
    lr.Set(P * HomgPoint3D(LR));
    ul.Homogenize();
    ur.Homogenize();
    ll.Homogenize();
    lr.Homogenize();
  }

  return GenerateTexturedQuad(Texture, UL, UR, LL, LR,
                              ul.GetEuclidean(), ur.GetEuclidean(),
                              ll.GetEuclidean(), lr.GetEuclidean(), normal);
}


int TriangleMesh::
GenerateTexturedQuadStrip(const Image<unsigned char>& Texture,
                          const std::vector< Vector3<double> > &X,
                          const std::vector< Vector2<double> > &x)
{
  const bool doTexture = Texture.GetWidth() > 0;
  const float w = Texture.GetWidth() - 1.0f;
  const float h = Texture.GetHeight() - 1.0f;

  // clear mesh data
  meshVertices_.clear();
  vertexNormals_.clear();
  textureCoords_.clear();
  triangleIndices_.clear();

  // add vertices
  std::vector< Vector3<double> >::const_iterator x3d;
  for (x3d = X.begin(); x3d != X.end(); x3d++) {
    meshVertices_.push_back(*x3d);
  }

  // add texture coordinates
  if (doTexture && !x.empty())
  {
    BIASASSERT(X.size() == x.size());
    texture_ = Texture;
    std::vector< Vector2<double> >::const_iterator x2d;
    for (x2d = x.begin(); x2d != x.end(); x2d++) {
      textureCoords_.push_back(
        Vector2<float>((float)((*x2d)[0] / w), 1.0f - (float)((*x2d)[1] / h)));
    }
  }

  // add triangles
  const unsigned int num = X.size();
  Vector<int> triangle(3);
  for (unsigned int idx = 0; idx+3 < num; idx += 2) {
    triangle[0] = idx;
    triangle[1] = idx + 1;
    triangle[2] = idx + 2;
    triangleIndices_.push_back(triangle);
    triangle[0] = idx + 1;
    triangle[1] = idx + 3;
    triangle[2] = idx + 2;
    triangleIndices_.push_back(triangle);
  }

  return 0;
}


int TriangleMesh::
GenerateTexturedQuadStrip(const Projection& P,
                          const Image<unsigned char>& Texture,
                          const std::vector< Vector3<double> > &X)
{
  std::vector< Vector2<double> > x;
  std::vector< Vector3<double> >::const_iterator x3d;
  x.reserve(X.size());
  HomgPoint2D x2d;
  for (x3d = X.begin(); x3d != X.end(); x3d++) {
    x2d = P.Project(HomgPoint3D(*x3d));
    if (!Equal(x2d[2], 0.0)) {
      // 3d point is not at infinity and projects into texture image
      x2d.Homogenize();
      x.push_back(x2d.GetEuclidean());
    } else {
      // 3d point is at infinity, add dummy texture coordinates
      x.push_back(Vector2<double>(0.0, 0.0));
    }
  }

  return GenerateTexturedQuadStrip(Texture, X, x);
}


/**
 * @brief Simplifies a TriangleMesh to a desired percentage of triangles of the original mesh.
 * The method uses the algorithm described by Garland and Heckbert in "Surface Simplification Using Quadric Error Metrics"
 *
 * @author bangerer, 09/2008
 * @param Texture the texture of the mesh
 * @param TexP the
 * @param reduce reduces the triangles in the generated mesh to "reduce" percent of the original mesh
 * @return true if mesh could be reduced to the given reduce value
 */
bool TriangleMesh::SimplifyMeshSurface_(const BIAS::Image<unsigned char>& Texture,
                                        BIAS::Projection& TexP, double reduce) {

  vector<int> deletedVertices_;
  Vector2<float> texCoord;

  // Initialize Quadrics.

  cout << "Mesh has " << meshVertices_.size() << " vertices and " << triangleIndices_.size()
  << " triangles" << endl;

  std::vector<Quadric3> quadrics;
  std::vector<int> triangleIndicesRemove;

  // initialize datastructures with values
  for (unsigned int i = 0; i < meshVertices_.size(); i++) {
    Quadric3 quad;
    quadrics.push_back(quad);
    set<int> neighbours;
    neighboursOfVertex_.push_back(neighbours);
    set<int> tris;
    vertexIsInTriangles_.push_back(tris);
    set<int> pair;
    vertexIsInThisPairs_.push_back(pair);
  }

  // calculate the inital Quadrics for each vertex
  for (unsigned int i = 0; i < triangleIndices_.size(); i++) {
    // make vectors for fast indexing

    triangleIndicesRemove.push_back(0);

    //neighbours of vector 1
    neighboursOfVertex_[triangleIndices_[i][0]].insert(triangleIndices_[i][1]);
    neighboursOfVertex_[triangleIndices_[i][0]].insert(triangleIndices_[i][2]);

    // neighbours of vector 2
    neighboursOfVertex_[triangleIndices_[i][1]].insert(triangleIndices_[i][0]);
    neighboursOfVertex_[triangleIndices_[i][1]].insert(triangleIndices_[i][2]);

    // neighbours of vector 3
    neighboursOfVertex_[triangleIndices_[i][2]].insert(triangleIndices_[i][0]);
    neighboursOfVertex_[triangleIndices_[i][2]].insert(triangleIndices_[i][1]);

    // triangle, vector is in
    vertexIsInTriangles_[triangleIndices_[i][0]].insert(i);
    vertexIsInTriangles_[triangleIndices_[i][1]].insert(i);
    vertexIsInTriangles_[triangleIndices_[i][2]].insert(i);

    // get corners of triangle
    Vector3<double> v1 = meshVertices_[triangleIndices_[i][0]];
    Vector3<double> v2 = meshVertices_[triangleIndices_[i][1]];
    Vector3<double> v3 = meshVertices_[triangleIndices_[i][2]];

    Vector4<double> test;
    double area = 0.0;
    area = ThreePointsToPlane_(v1, v2, v3, test); // calculate plane and area for triangle

    Quadric3 Q(test[0], test[1], test[2], test[3], area);
    Q *= Q.GetArea();
    // add quadric to every vertex
    quadrics[triangleIndices_[i][0]] += Q;
    quadrics[triangleIndices_[i][1]] += Q;
    quadrics[triangleIndices_[i][2]] += Q;
  }

  // add all valid edges to a set
  set<int>::iterator externIter, internIter;
  for (unsigned int i = 0; i < neighboursOfVertex_.size(); i++) {
    for (externIter = neighboursOfVertex_[i].begin(); externIter
    != neighboursOfVertex_[i].end(); ++externIter) {
      //cout << "Kante von " << i << " nach " << *externIter << endl;
      if ((unsigned int)*externIter > i) {
        Edge e;
        e.IDv1 = i;
        e.IDv2 = *externIter;

        validPairs_.push_back(e);
        vertexIsInThisPairs_[e.IDv1].insert(validPairs_.size() - 1);
        vertexIsInThisPairs_[e.IDv2].insert(validPairs_.size() - 1);

        // we have to test if one of the edges is a boundary edge. if it is, then give it
        // a penalty value because it should not be contracted.
#ifdef USE_BOUNDARY_TEST

        // get the triangles v1 is in
            int countTriangles = 0;
            int triangleID = 0;
            for (internIter = vertexIsInTriangles_[e.IDv1].begin(); internIter
                != vertexIsInTriangles_[e.IDv1].end(); ++internIter) {
              if ((triangleIndices_[*internIter][0] == e.IDv2) || (triangleIndices_[*internIter][1]
                  == e.IDv2) || (triangleIndices_[*internIter][2] == e.IDv2)) {
                countTriangles++;
                triangleID = *internIter;
              }
            }


            // construct plane through e1 and plane
        if (countTriangles == 1) {

          Vector4<double> test;
          ThreePointsToPlane_(meshVertices_[triangleIndices_[triangleID][0]],
                              meshVertices_[triangleIndices_[triangleID][1]],
                              meshVertices_[triangleIndices_[triangleID][2]], test);
          Vector4<double> senkrechteEbene;
          Vector3<double> normale(test[0], test[1], test[2]);
          normale.AddIP(meshVertices_[e.IDv1]);
          double area2 = 0.0;
          area2 = ThreePointsToPlane_(meshVertices_[e.IDv1], meshVertices_[e.IDv2], normale,
                                      senkrechteEbene);

          // calc quadric of orthogonal plane and add to v1 and v2

          Quadric3 Q2(senkrechteEbene[0], senkrechteEbene[1], senkrechteEbene[2],
                      senkrechteEbene[3], area2);
          Q2 *= Q2.GetArea();
          Q2 *= 1000; // penalty
          quadrics[e.IDv1] += Q2;
          quadrics[e.IDv2] += Q2;
        }
#endif
      }
    }
  }

 // double T = 0.05; // distance between two non connected points


//  if (T > 0) {
//    for (unsigned int i = 0; i < meshVertices_.size(); i++) {
//      for (unsigned int j = i + 1; j < meshVertices_.size(); j++) {
//       // cout << (meshVertices_[i].Normalize() - meshVertices_[j].Normalize()).NormL2() << endl;
//        Vector3<double> vt1;
//        Vector3<double> vt2;
//        vt1 = meshVertices_[i];
//        vt1.Normalize();
//        vt2 = meshVertices_[j];
//        vt2.Normalize();
//        if ((vt1 - vt2).NormL2() < T) {
//          Edge e;
//          e.IDv1 = i;
//          e.IDv2 = j;
//          validPairs_.push_back(e);
//          vertexIsInThisPairs_[e.IDv1].insert(validPairs_.size() - 1);
//          vertexIsInThisPairs_[e.IDv2].insert(validPairs_.size() - 1);
//          cout << "Found additional valid pair" << endl;
//        }
//      }
//    }
//  }

  // now compute the contraction for every pair/edge with the costs and put in a sorted list
  for (unsigned int i = 0; i < validPairs_.size(); i++) {
    Quadric3 Q;
    Q = quadrics[validPairs_[i].IDv1];
    Q += quadrics[validPairs_[i].IDv2];


#ifdef USE_OPTIMIZED
    // compute optimized point
    if (Q.Optimize(validPairs_[i].newV)) {
      validPairs_[i].err = Q.Evaluate(validPairs_[i].newV[0], validPairs_[i].newV[1],
                                      validPairs_[i].newV[2]);
      //cout << "found optimal new point" << endl;
    } else {
#endif
      // move vertex v2 at position of vertex v1
      Vector3<double> vi = meshVertices_[validPairs_[i].IDv1];
      Vector3<double> vj = meshVertices_[validPairs_[i].IDv2];

      double ei = Q.Evaluate(vi[0], vi[1], vi[2]);
      double ej = Q.Evaluate(vj[0], vj[1], vj[2]);

      if (ei < ej) {
        validPairs_[i].err = ei;
        validPairs_[i].newV = vi;
      } else {
        validPairs_[i].err = ej;
        validPairs_[i].newV = vj;
      }
#ifdef USE_OPTIMIZED
    }
#endif

    // test if one of the triangles after contraction changes direction. if yes, then the points
    // v1 and v2 should not be contracted
#ifdef USE_CONTRACTION_TEST
    TestContraction(validPairs_[i].IDv1, validPairs_[i].IDv2, i, true);
#endif

    // add to sorted set
    sorted.insert(validPairs_[i]);
  }

  // give me the sorted edges
  std::multiset<Edge,Edge>::iterator iter;

  // now we can start with the decimation of the vertices
  int valid_triangles = triangleIndices_.size();
  int target = int(valid_triangles * (reduce/100.0));   // percentage of original mesh
  Edge ed;
  // compute contraction
  Vector3<double> dv1, dv2;

  std::vector<int> dead_triangles; // here we put triangles that degenerate
  std::vector<int> moved_triangles; // here we put triangle indices of triangles that reshape
  std::vector<BIAS::Vector<int> > trianglesToKill;
  std::vector<int> IntegerToKill;

  int moved_pivot;

  cout << "starting mesh reduction... " << endl;

  int difference = valid_triangles - target;
  int progress=0;
  int lastPerCent = 0;
  while (valid_triangles > target && sorted.size() > 0) {
        // calculate progress and print status
        progress = (int)rint((double) 1 - ((double) (valid_triangles - target)
            / (double) difference)) * 100;

        if (progress % 10 == 0) {
          if (progress != lastPerCent) {
            cout << progress << " %" << endl;
            lastPerCent = progress;
          }
        }

        // fetch the first element out of the sorted set an delete it from the set.
        iter = sorted.begin();
        ed = *iter;
        sorted.erase(ed);

    // clean vectors
    dead_triangles.clear();
    moved_triangles.clear();

    // the two vectors that should be contracted
    Vector3<double> v1 = meshVertices_[ed.IDv1];
    Vector3<double> v2 = meshVertices_[ed.IDv2];

    ed.newV.Sub(v1, dv1);
    ed.newV.Sub(v2, dv2);

    // get the triangles that degenerate and the triangles that are only moved
    GetNeighbours_(ed.IDv1, ed.IDv2, dead_triangles, moved_triangles, moved_pivot);

    // accumulate quadrics of the two vertices
    quadrics[ed.IDv1] += quadrics[ed.IDv2];

    vector<Edge> maybeDoubleEdges;

    // concat the edges with v2 to v1

    //first search for potential double edges and delete the actually used edge from pairs
    IntegerToKill.clear();
    set<int>::iterator pairIter;
    for (pairIter = vertexIsInThisPairs_[ed.IDv1].begin(); pairIter
    != vertexIsInThisPairs_[ed.IDv1].end(); ++pairIter) {

      sorted.erase(validPairs_[*pairIter]);

      if (validPairs_[*pairIter].IDv1 == ed.IDv2 || validPairs_[*pairIter].IDv2 == ed.IDv2) {
        // delete edge from pairs
        IntegerToKill.push_back(*pairIter);
      } else {
        // add double edge
        maybeDoubleEdges.push_back(validPairs_[*pairIter]);
      }
    }
    // delete the selected pairs
    for(unsigned int j = 0; j < IntegerToKill.size(); j++){
              vertexIsInThisPairs_[ed.IDv1].erase(IntegerToKill[j]);
              vertexIsInThisPairs_[ed.IDv2].erase(IntegerToKill[j]);
    }
    IntegerToKill.clear();

    // relink pairs with v2 into pairs with v1 and check for double entrys
    for (pairIter = vertexIsInThisPairs_[ed.IDv2].begin(); pairIter
    != vertexIsInThisPairs_[ed.IDv2].end(); ++pairIter) {

      Edge edg = validPairs_[*pairIter];
      bool relink = true;
      if (validPairs_[*pairIter].IDv1 == ed.IDv2) {
        sorted.erase(validPairs_[*pairIter]);
        for (unsigned int j = 0; j < maybeDoubleEdges.size(); j++) {

          if(maybeDoubleEdges[j].IDv1 == validPairs_[*pairIter].IDv2)
            relink = false;
          else if(maybeDoubleEdges[j].IDv2 == validPairs_[*pairIter].IDv2)
            relink = false;
          else if(validPairs_[*pairIter].IDv2 == ed.IDv1)
            relink = false;

        }
        if (relink) {

          if(validPairs_[*pairIter].IDv2 == ed.IDv1){
            IntegerToKill.push_back(*pairIter);
          }else{
          validPairs_[*pairIter].IDv1 = ed.IDv1;
          vertexIsInThisPairs_[ed.IDv1].insert(*pairIter);
          }
        }else{
          IntegerToKill.push_back(*pairIter);
        }

      }

      if (validPairs_[*pairIter].IDv2 == ed.IDv2) {
        sorted.erase(validPairs_[*pairIter]);
        for (unsigned int j = 0; j < maybeDoubleEdges.size(); j++) {

          if(maybeDoubleEdges[j].IDv1 == validPairs_[*pairIter].IDv1)
                      relink = false;
                    else if(maybeDoubleEdges[j].IDv2 == validPairs_[*pairIter].IDv1)
                      relink = false;
                    else if(validPairs_[*pairIter].IDv1 == ed.IDv1)
                      relink = false;

        }
        if (relink) {
          if(validPairs_[*pairIter].IDv1 == ed.IDv1){
                     IntegerToKill.push_back(*pairIter);
          }else{
          validPairs_[*pairIter].IDv2 = ed.IDv1;
          vertexIsInThisPairs_[ed.IDv1].insert(*pairIter);
          }
        }else{
          IntegerToKill.push_back(*pairIter);
        }
      }

    }

    for(unsigned int j = 0; j < IntegerToKill.size(); j++){
              vertexIsInThisPairs_[validPairs_[IntegerToKill[j]].IDv1].erase(IntegerToKill[j]);
              vertexIsInThisPairs_[validPairs_[IntegerToKill[j]].IDv2].erase(IntegerToKill[j]);
    }


    // move v1 to vNew
    meshVertices_[ed.IDv1] += dv1;
    //    meshVertices_[ed.IDv2] +=  dv2;

    // calc new texture
    //generate texture coordinates
    HomgPoint2D uv;
    HomgPoint3D p3dh(meshVertices_[ed.IDv1]);

    unsigned int tw = Texture.GetWidth();
    unsigned int th = Texture.GetHeight();

    if (TexP.DoesPointProjectIntoImage(p3dh, uv)) {
      double u = uv[0] / float(tw - 1);
      double v = 1 - uv[1] / float(th - 1);
      texCoord.Set(float(u), float(v));

      textureCoords_[ed.IDv1] = texCoord;
    }


    // all v2 become v1
    set<int>::iterator listIter;

    for (unsigned int i = 0; i < moved_triangles.size(); i++) {
      if (triangleIndices_[moved_triangles[i]][0] == ed.IDv2) {
        triangleIndices_[moved_triangles[i]][0] = ed.IDv1;
        vertexIsInTriangles_[ed.IDv1].insert(moved_triangles[i]);
      }

      if (triangleIndices_[moved_triangles[i]][1] == ed.IDv2) {
        triangleIndices_[moved_triangles[i]][1] = ed.IDv1;
        vertexIsInTriangles_[ed.IDv1].insert(moved_triangles[i]);
      }
      if (triangleIndices_[moved_triangles[i]][2] == ed.IDv2) {
        triangleIndices_[moved_triangles[i]][2] = ed.IDv1;
        vertexIsInTriangles_[ed.IDv1].insert(moved_triangles[i]);
      }
    }

    // remove degenerated triangles from vertices
    for (unsigned int i = 0; i < dead_triangles.size(); i++) {

      vertexIsInTriangles_[triangleIndices_[dead_triangles[i]][0]].erase(dead_triangles[i]);
      vertexIsInTriangles_[triangleIndices_[dead_triangles[i]][1]].erase(dead_triangles[i]);
      vertexIsInTriangles_[triangleIndices_[dead_triangles[i]][2]].erase(dead_triangles[i]);

      triangleIndicesRemove[dead_triangles[i]] = 1;
    }

    // delete all of v2
    vertexIsInTriangles_[ed.IDv2].clear();
    vertexIsInThisPairs_[ed.IDv2].clear();

    // update number of valid triangles
    valid_triangles -= dead_triangles.size();

    // update list with error quadrics
    for (pairIter = vertexIsInThisPairs_[ed.IDv1].begin(); pairIter
    != vertexIsInThisPairs_[ed.IDv1].end(); ++pairIter) {

      sorted.erase(validPairs_[*pairIter]);

      int doubleV1p = validPairs_[*pairIter].IDv1;
      int doubleV2p = validPairs_[*pairIter].IDv2;

      if(doubleV1p == doubleV2p)
        cout << "SOMETHING WENT WRONG" << endl;

      Quadric3 Q;
      Q = quadrics[validPairs_[*pairIter].IDv1];
      Q += quadrics[validPairs_[*pairIter].IDv2];
#ifdef USE_OPTIMIZED
      // compute optimized point
      if (Q.Optimize(validPairs_[*pairIter].newV)) {
        validPairs_[*pairIter].err = Q.Evaluate(validPairs_[*pairIter].newV[0],
                                                validPairs_[*pairIter].newV[1],
                                                validPairs_[*pairIter].newV[2]);
      } else {
#endif
        Vector3<double> vi = meshVertices_[validPairs_[*pairIter].IDv1];
        Vector3<double> vj = meshVertices_[validPairs_[*pairIter].IDv2];

        double ei = Q.Evaluate(vi[0], vi[1], vi[2]);
        double ej = Q.Evaluate(vj[0], vj[1], vj[2]);

        if (ei < ej) {
          validPairs_[*pairIter].err = ei;
          validPairs_[*pairIter].newV = vi;
        } else {
          validPairs_[*pairIter].err = ej;
          validPairs_[*pairIter].newV = vj;
        }
#ifdef USE_OPTIMIZED
      }

#endif
      // evaluate contraction
#ifdef USE_CONTRACTION_TEST
      TestContraction(ed.IDv1, ed.IDv2, *pairIter, true);
#endif

      sorted.insert(validPairs_[*pairIter]);
    }
  }

  cout << "100 %" << endl;

  // remove the degenerated triangles from the original mesh
  std::vector<BIAS::Vector<int> > tmp;

  for(unsigned int i = 0; i < triangleIndices_.size(); i++){
    if(triangleIndicesRemove[i] == 0){
      tmp.push_back(triangleIndices_[i]);
    }
  }

  triangleIndices_.clear();
  triangleIndices_ = tmp;

  cout << "Triangles after mesh reduction: " << triangleIndices_.size() << endl;

  return true;
}

/**
 * @brief  calculates a plan of three points and returns it  area
 *
 * @param v1 first vector of plane
 * @param v2 second vector of plane
 * @param v3 third vector of plane
 * @param erg vector in which the plane is stored
 * @return the area of the plan
 */
double TriangleMesh::ThreePointsToPlane_(Vector3<double> v1, Vector3<double> v2,
                                         Vector3<double> v3, Vector4<double>& erg) {
  // plane in parameterform
  // E = A + s* AB + t* AC
  Vector3<double> AB;
  Vector3<double> AC;

  AB = v2 - v1;
  AC = v3 - v1;

  // calc normal vector N
  Vector3<double> N;
  AB.CrossProduct(AC, N);

  // generate cooordinate form
  // ax + by + cz + d = 0;
  N.Normalize();

  erg[0] = N[0]; // a
  erg[1] = N[1]; // b
  erg[2] = N[2]; // c
  erg[3] = -(int) (N.ScalarProduct(v1)); // d

  // return the area of the triangle
  return (N.NormL2() * 0.5);
}

/**
 * @brief get the neighbours of the contracted points and returns two list that contains
 * the triangles to remove from the model and triangles that have to be moved to one of the vertices
 */
void TriangleMesh::GetNeighbours_(int v1, int v2, std::vector<int>& dead_triangles,
                                  std::vector<int>& moved_triangles, int &moved_pivot) {
  // first check for triangles that contain one or two of the vectors.
  multiset<int> neighbours;
  set<int>::iterator iter;

  // get neighbours
  for (iter = vertexIsInTriangles_[v1].begin(); iter != vertexIsInTriangles_[v1].end(); ++iter) {
    neighbours.insert(*iter);
    //cout << "Triangle i= " << vertexIsInTriangles_[v1][i] <<" enthaelt " << neighbours.count(vertexIsInTriangles_[v1][i])<< endl;
  }

  // if theres the neighbour only once, then move the triangle with the one neighbour, else kill the triangle
  for (iter = vertexIsInTriangles_[v2].begin(); iter != vertexIsInTriangles_[v2].end(); ++iter) {
    neighbours.insert(*iter);
    // cout << "Count: " << neighbours.count(*iter) << endl;
    if (neighbours.count(*iter) == 1) {
      moved_triangles.push_back(*iter);
    } else {
      // cout << "Triangle to kill: " << *iter << endl;
      dead_triangles.push_back(*iter);
    }
  }
  ////////////////////////////////
  //if(dead_triangles.size() == 0 || dead_triangles.size() > 2){
  //cout << "Number of triangles to kill: " << dead_triangles.size() << endl;
  //cout << "V1: " << v1 << " V2: " << v2 << endl;
  //}
}

/**
 * @brief Tests if a triangle that is involved in the contraction changes
 *        direction or overlapps
 */
void TriangleMesh::TestContraction(int v1, int v2, int edge, bool init) {
  // test for planes connected to v1
  Vector<double> normalOld;
  Vector<double> normalNew;
  normalNew.SetZero();

  if (init) {
    set<int>::iterator iter;

    for (iter = vertexIsInTriangles_[v1].begin();
         iter != vertexIsInTriangles_[v1].end();
         ++iter) {
      //cout << *iter << endl;
      if (triangleIndices_[*iter][0] != v2 && triangleIndices_[*iter][1] != v2
          && triangleIndices_[*iter][2] != v2) {
        // compute normale before contraction
        // cout << "V1: " << v1 << " V2: " << v2 << endl;

        normalOld = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                            meshVertices_[triangleIndices_[*iter][1]],
                                            meshVertices_[triangleIndices_[*iter][2]]);
        // compute normale after contraction
        if (triangleIndices_[*iter][0] == v1)
          normalNew = GetPlaneNormalEuclidean(validPairs_[edge].newV,
                                              meshVertices_[triangleIndices_[*iter][1]],
                                              meshVertices_[triangleIndices_[*iter][2]]);
        if (triangleIndices_[*iter][1] == v1)
          normalNew = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                              validPairs_[edge].newV,
                                              meshVertices_[triangleIndices_[*iter][2]]);
        if (triangleIndices_[*iter][2] == v1)
          normalNew = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                              meshVertices_[triangleIndices_[*iter][1]],
                                              validPairs_[edge].newV);

        if (!normalNew.IsZero() && !normalOld.IsZero()) {
          if (normalNew.NormL2() > std::numeric_limits<double>::epsilon()
              && normalOld.NormL2() > std::numeric_limits<double>::epsilon()) {
            const double angle = GetAngle(normalOld, normalNew);
            if (angle > 0.15 || angle < -0.15) {
              validPairs_[edge].err *= 1000;
              return;
            }
          }
        }
      }
    }

    for (iter = vertexIsInTriangles_[v2].begin();
         iter != vertexIsInTriangles_[v2].end();
         ++iter) {
      //cout << *iter << endl;
      if (triangleIndices_[*iter][0] != v1 && triangleIndices_[*iter][1] != v1
          && triangleIndices_[*iter][2] != v1) {
        // compute normale before contraction
        // cout << "V1: " << v1 << " V2: " << v2 << endl;

        normalOld = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                            meshVertices_[triangleIndices_[*iter][1]],
                                            meshVertices_[triangleIndices_[*iter][2]]);
        // compute normale after contraction
        if (triangleIndices_[*iter][0] == v2)
          normalNew = GetPlaneNormalEuclidean(validPairs_[edge].newV,
                                              meshVertices_[triangleIndices_[*iter][1]],
                                              meshVertices_[triangleIndices_[*iter][2]]);
        if (triangleIndices_[*iter][1] == v2)
          normalNew = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                              validPairs_[edge].newV,
                                              meshVertices_[triangleIndices_[*iter][2]]);
        if (triangleIndices_[*iter][2] == v2)
          normalNew = GetPlaneNormalEuclidean(meshVertices_[triangleIndices_[*iter][0]],
                                              meshVertices_[triangleIndices_[*iter][1]],
                                              validPairs_[edge].newV);

        if (!normalNew.IsZero() && !normalOld.IsZero()) {
          if (normalNew.NormL2() > std::numeric_limits<double>::epsilon()
              && normalOld.NormL2() > std::numeric_limits<double>::epsilon()) {
            const double angle = GetAngle(normalOld, normalNew);

            if (angle > 0.15 || angle < -0.15) {
              validPairs_[edge].err *= 1000;
              return;
            }
          }
        }
      }
    }
  }
}

void TriangleMesh::GetBiggestConnectedSegment(TriangleMesh& biggestSegment) const
{
  /////
  // to speed up searching, create a list of triangles for each 3D point that
  // touch it
  /////
  vector<vector<size_t> > pointsToTris(meshVertices_.size());

  for (size_t triCnt = 0; triCnt < triangleIndices_.size(); ++triCnt) {
    for (size_t cornerCnt = 0;
         cornerCnt < size_t(triangleIndices_[triCnt].size());
         ++cornerCnt) {
      pointsToTris[triangleIndices_[triCnt][cornerCnt]].push_back(triCnt);
    }
  }

  /////
  // find conntected segments
  /////
  vector<vector<size_t> > segments(triangleIndices_.size()); // indices of triangles belonging to a certain segment
  size_t emptySegmentIndex = 0;                              // always points to an empty segment
  vector<int> trisToSegments(triangleIndices_.size(), -1);   // for each tri store corresponding segment

  // iterate over the previously created list of points and add all triangles
  // touching the same point to the same segment
  for (size_t pointCnt = 0; pointCnt < pointsToTris.size(); ++pointCnt) {
    // skip if no triangles touch this point
    if (pointsToTris[pointCnt].empty()) {
      BIASWARN("mesh contains a vertex that is not used by any triangle")
      continue;
    }

    // add all triangles (and their corresponding segments) to the segment of
    // the first point in the list

    // get segment of first triangle for current point or create new one for it
    const size_t firstTriIndex = pointsToTris[pointCnt][0];
    int segmentForCurPoint = trisToSegments[firstTriIndex];

    if (segmentForCurPoint < 0) { // create new segment
      segments[emptySegmentIndex].push_back(firstTriIndex);
      trisToSegments[firstTriIndex] = emptySegmentIndex;
      segmentForCurPoint = (int)emptySegmentIndex;
      ++emptySegmentIndex;
    }

    // now add remaining triangles (and their segments) meeting in cur point
    for (size_t triCnt = 1; triCnt < pointsToTris[pointCnt].size(); ++triCnt) {
      const size_t curTriIndex = pointsToTris[pointCnt][triCnt];
      const int curTriSegment = trisToSegments[curTriIndex];

      // check if current tri already belongs to a segment
      if (curTriSegment < 0) { // only need to add this
        segments[segmentForCurPoint].push_back(curTriIndex);
        trisToSegments[curTriIndex] = segmentForCurPoint;
      }
      else if (curTriSegment == segmentForCurPoint) { // already in the segment
        // nothing to do
      }
      else { // triangle belongs to a segment -> add all triangles from segment
        for (size_t triSegmCnt = 0;
             triSegmCnt < segments[size_t(curTriSegment)].size();
             ++triSegmCnt) {
          segments[segmentForCurPoint].push_back(segments[size_t(curTriSegment)][triSegmCnt]);
          trisToSegments[segments[size_t(curTriSegment)][triSegmCnt]]
              = segmentForCurPoint;
        }

        segments[size_t(curTriSegment)].clear();
      }
    }
  }

  /////
  // search for biggest segment
  /////
  size_t maxNumTris = 0;
  int biggestSegmentIndex = -1;

  for (size_t segmCnt = 0; segmCnt < segments.size(); ++segmCnt) {
    if (segments[segmCnt].size() > maxNumTris) {
      maxNumTris = segments[segmCnt].size();
      biggestSegmentIndex = int(segmCnt);
    }
  }

  if (biggestSegmentIndex == -1) {
    BIASWARN("did not find any connected segments at all")
    return;
  }

  /////
  // remove unused vertices and produce final TriangleMesh
  /////
  vector<Vector3<double> > vertices;
  vector<Vector3<double> > normals;
  vector<Vector4<unsigned char> > colors;
  vector<int> newVertexIndices(meshVertices_.size(), -1); // new indices for orig vertices

  for (size_t triCnt = 0;
       triCnt < segments[biggestSegmentIndex].size();
       ++triCnt) {
    const size_t curTriIndex = segments[biggestSegmentIndex][triCnt];

    for (size_t pointCnt = 0;
         pointCnt < size_t(triangleIndices_[curTriIndex].size());
         ++pointCnt) {
      const size_t curPointIndex = triangleIndices_[curTriIndex][pointCnt];

      if (newVertexIndices[curPointIndex] == -1) {
        newVertexIndices[curPointIndex] = vertices.size();
        vertices.push_back(meshVertices_[curPointIndex]);
        if (!vertexNormals_.empty()) {
          normals.push_back(vertexNormals_[curPointIndex]);
        }
        colors.push_back(vertexColors_[curPointIndex]);
      }
    }
  }

  vector<Vector<int> > triangleIndices;
  triangleIndices.reserve(segments[biggestSegmentIndex].size());

  for (size_t triCnt = 0;
       triCnt < segments[biggestSegmentIndex].size();
       ++triCnt) {
    const Vector<int>& origTri
        = triangleIndices_[segments[biggestSegmentIndex][triCnt]];
    Vector<int> newTri(triangleIndices_[segments[biggestSegmentIndex][triCnt]].size());

    for (size_t pointCnt = 0; pointCnt < size_t(origTri.size()); ++pointCnt) {
      const size_t origIndex = origTri[pointCnt];
      const size_t newIndex = newVertexIndices[origIndex];
      newTri[pointCnt] = newIndex;
    }

    triangleIndices.push_back(newTri);
  }

  biggestSegment.SetMesh(vertices, normals, colors, triangleIndices);
}

void TriangleMesh::GetBoundingBox(Vector3<double>& minPoint,
                                  Vector3<double>& maxPoint) const
{
  // if this mesh is empty, set both points to zero
  if (meshVertices_.empty()) {
    minPoint.Set(0.0, 0.0, 0.0);
    maxPoint.Set(0.0, 0.0, 0.0);
    return;
  }

  // at this point we have vertices in the mesh, so search for minimal and
  // maximal components
  minPoint.Set(numeric_limits<double>::max(),
               numeric_limits<double>::max(),
               numeric_limits<double>::max());
  maxPoint.Set(-numeric_limits<double>::max(),
               -numeric_limits<double>::max(),
               -numeric_limits<double>::max());

  for (size_t vertexCnt = 0; vertexCnt < meshVertices_.size(); ++vertexCnt) {
    const Vector3<double>& curVertex = meshVertices_[vertexCnt]; // readability

    if (curVertex[0] < minPoint[0]) minPoint[0] = curVertex[0];
    if (curVertex[1] < minPoint[1]) minPoint[1] = curVertex[1];
    if (curVertex[2] < minPoint[2]) minPoint[2] = curVertex[2];

    if (curVertex[0] > maxPoint[0]) maxPoint[0] = curVertex[0];
    if (curVertex[1] > maxPoint[1]) maxPoint[1] = curVertex[1];
    if (curVertex[2] > maxPoint[2]) maxPoint[2] = curVertex[2];
  }
}

void TriangleMesh::GenerateVertexNormals(bool leftHanded,
                                         Vector3<double> normalForUnusedVertices)
{
  BIASASSERT(normalForUnusedVertices.NormL2() == 1.0)

  // prepare normals vector
  vertexNormals_.clear();
  vertexNormals_.resize(meshVertices_.size(), normalForUnusedVertices);

  // compute normal for each triangle
  vector<Vector3<double> > triNormals;
  triNormals.reserve(triangleIndices_.size());

  for (size_t triCnt = 0; triCnt < triangleIndices_.size(); ++triCnt) {
    // get corner points and compute difference vectors
    const Vector3<double>& p1 = meshVertices_[triangleIndices_[triCnt][0]];
    const Vector3<double>& p2 = meshVertices_[triangleIndices_[triCnt][1]];
    const Vector3<double>& p3 = meshVertices_[triangleIndices_[triCnt][2]];

    const Vector3<double>& diffP1P2 = p1 - p2;
    const Vector3<double>& diffP1P3 = p1 - p3;

    if (leftHanded) {
      // cross product for perpendicular vector
      Vector3<double> triNormal = diffP1P2.CrossProduct(diffP1P3);

      // normalise and save
      triNormal.Normalize();
      triNormals.push_back(triNormal);
    }
    else { // left-handed
      // cross product for perpendicular vector
      Vector3<double> triNormal = diffP1P3.CrossProduct(diffP1P2);

      // normalise and save
      triNormal.Normalize();
      triNormals.push_back(triNormal);
    }
  }

  // for each vertice determine which triangles it touches
  // vertices not used in any triangle get index -1
  vector<vector<int> > trianglesForVertices(meshVertices_.size());

  for (size_t triCnt = 0; triCnt < triangleIndices_.size(); ++triCnt) {
    trianglesForVertices[triangleIndices_[triCnt][0]].push_back(triCnt);
    trianglesForVertices[triangleIndices_[triCnt][1]].push_back(triCnt);
    trianglesForVertices[triangleIndices_[triCnt][2]].push_back(triCnt);
  }

  // compute normal for each vertex by averaging the normals of the surrounding
  // triangles
  for (size_t vertexCnt = 0;
       vertexCnt < trianglesForVertices.size();
       ++vertexCnt) {
    if (!trianglesForVertices[vertexCnt].empty()) {
      vertexNormals_[vertexCnt].Set(0.0, 0.0, 0.0);

      for (size_t triCnt = 0;
           triCnt < trianglesForVertices[vertexCnt].size();
           ++triCnt) {
        vertexNormals_[vertexCnt]
            += triNormals[trianglesForVertices[vertexCnt][triCnt]];
      }

      vertexNormals_[vertexCnt]
          /= double(trianglesForVertices[vertexCnt].size());
      vertexNormals_[vertexCnt].Normalize();
    }
  }
}