cornelius.cpp

Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file cornelius.cpp

#include <iostream>
#include <fstream>
#include <math.h>
#include <stdlib.h>

using namespace std;

class GeneralElement
{
  protected:
    static const int DIM = 4;
    double *centroid;
    double *normal;
    int normal_calculated;
    int centroid_calculated;
    virtual void calculate_centroid() {};
    virtual void calculate_normal() {};
    void check_normal_direction(double *normal, double *out);
  public:
    GeneralElement();
    ~GeneralElement();
    double *get_centroid();
    double *get_normal();
};

GeneralElement::GeneralElement()
{
  normal_calculated = 0;
  centroid_calculated = 0;
  centroid = new double[DIM];
  normal = new double[DIM];
}

GeneralElement::~GeneralElement()
{
  delete[] centroid;
  delete[] normal;
}

void GeneralElement::check_normal_direction(double *normal, double *out)
{
  //We calculate the dot product, if less than 4 dimensions the elements,
  //which are not used, are just zero and do not effect this
  double dot_product = 0;
  for (int i=0; i < DIM; i++) {
    dot_product += out[i]*normal[i];
  }
  //If the dot product is negative, the normal is pointing in the
  //wrong direction and we have to flip it's direction
  if ( dot_product < 0 ) {
    for (int i=0; i < DIM; i++) {
      normal[i] = -normal[i];
    }
  }
}

double *GeneralElement::get_centroid()
{
  if ( !centroid_calculated )
    calculate_centroid();
  return centroid;
}

double *GeneralElement::get_normal()
{
  if ( !normal_calculated )
    calculate_normal();
  return normal;
}


class Line : public GeneralElement
{
  private:
    static const int LINE_CORNERS = 2;
    static const int LINE_DIM = 2;
    int x1,x2;
    int start_point;
    int end_point;
    double **corners;
    double *out;
    int *const_i;
    void calculate_centroid();
    void calculate_normal();
  public:
    Line();
    ~Line();
    void init(double**,double*,int*);
    void flip_start_end();
    double *get_start();
    double *get_end();
    double *get_out();
};

Line::Line()
{
  GeneralElement();
  corners = new double*[LINE_CORNERS];
  for (int i=0; i < LINE_CORNERS; i++) {
    corners[i] = new double[DIM];
  }
  out = new double[DIM];
  const_i = new int[DIM-LINE_DIM]; 
}

Line::~Line()
{
  for (int i=0; i < LINE_CORNERS; i++) {
    delete[] corners[i];
  }
  delete[] corners;
  delete[] out;
  delete[] const_i;
}

void Line::init(double **p, double *o, int *c)
{
  //We copy the values of the end points and the outside point
  for (int i=0; i < LINE_CORNERS; i++) {
    for (int j=0; j < DIM; j++) {
      corners[i][j] = p[i][j];
      if ( i==0 ) {
        out[j] = o[j];
      }
    }
  }
  //Let's set indices for start and end points, so we can conveniently
  //flip the start and end point without changing the actual values
  start_point = 0;
  end_point = 1;
  //Here we copy the indices which are constant at this line
  for (int i=0; i < DIM-LINE_DIM; i++) {
    const_i[i] = c[i];
  }
  //Here we fix the non-zero indices in such a way that x1 is
  //always smaller
  if ( c[0] == 0 ) {
    if ( c[1] == 1 ) {
      x1 = 2; x2 = 3;
    } else if ( c[1] == 2 ) {
      x1 = 1; x2 = 3;
    } else {
      x1 = 1; x2 = 2;
    }
  } else if ( c[0] == 1 ) {
    if ( c[1] == 2 ) {
      x1 = 0; x2 = 3;
    } else {
      x1 = 0; x2 = 2;
    }
  } else {
    x1 = 0; x2 = 1;
  }
  //We need to set these to zero so when this function is called
  //again we know that normal and centroid are not yet calculated
  //with new values
  normal_calculated = 0;
  centroid_calculated = 0;
}

void Line::calculate_centroid()
{
  //Centroid is just the average of the points
  for (int i=0; i < DIM; i++) {
    centroid[i] = (corners[0][i] + corners[1][i])/2.0;
  }
  centroid_calculated = 1;
}

void Line::calculate_normal()
{
  //Centroid must be calculated before we can calculate normal
  if ( !centroid_calculated )
    calculate_centroid();
  //Normal is just (-dy,dx)
  normal[x1] = -(corners[1][x2] - corners[0][x2]);
  normal[x2] = corners[1][x1] - corners[0][x1];
  normal[const_i[0]] = 0;
  normal[const_i[1]] = 0;
  //Now we check if the normal is in the correct direction
  double *Vout = new double[DIM];
  for (int j=0; j < DIM; j++) {
    Vout[j] = out[j] - centroid[j];
  }
  check_normal_direction(normal,Vout);
  delete[] Vout;
  normal_calculated = 1;
}

void Line::flip_start_end()
{
  int temp = start_point;
  start_point = end_point;
  end_point = temp;
}

double *Line::get_start()
{
  return corners[start_point];
}

double *Line::get_end()
{
  return corners[end_point];
}

double *Line::get_out()
{
  return out;
}

class Polygon : public GeneralElement
{
  private:
    static const int MAX_LINES = 24;
    static const int POLYGON_DIM = 3;
    Line **lines;
    int Nlines;
    int x1,x2,x3;
    int const_i;
    void calculate_centroid();
    void calculate_normal();
  public:
    Polygon();
    ~Polygon();
    void init(int);
    bool add_line(Line*,int);
    int get_Nlines();
    Line** get_lines();
    void print(ofstream &file,double*);
};

Polygon::Polygon()
{
  GeneralElement();
  lines = new Line*[MAX_LINES];
}

Polygon::~Polygon()
{
  delete[] lines;
}

void Polygon::init(int c)
{
  //Let's copy the constant index
  const_i = c;
  //Here we fix the indices which are not constant
  x1 = -1;
  x2 = -1;
  x3 = -1;
  for (int i=0; i < DIM; i++) {
    if ( i != c ) {
      if ( x1 < 0 ) {
        x1 = i;
      } else if ( x2 < 0 ) {
        x2 = i;
      } else {
        x3 = i;
      }
    }
  }
  //We need to set these to zero so when this function is called
  //again we know that polygon has no lines added and normal and
  //centroid are not yet calculated with new values
  Nlines = 0;
  normal_calculated = 0;
  centroid_calculated = 0;
}

bool Polygon::add_line(Line *l, int donotcheck)
{
  double eps = 1e-10;
  //If this is the first line or we do not want to check, line is added
  //automatically
  if ( donotcheck || Nlines == 0 ) {
    lines[Nlines] = l;
    Nlines++;
    return true; 
  } else {
    //Here we check if the new line is connected with the last line in
    //polygon. This since lines are ordered.
    double *p1 =  l->get_start();
    double *p2 =  l->get_end();
    double *p3 =  lines[Nlines-1]->get_end();
    double diff1 = 0;
    double diff2 = 0;
    for (int j=0; j < DIM; j++) {
      diff1 += fabs(p1[j]-p3[j]);
      diff2 += fabs(p2[j]-p3[j]);
    }
    //If start or end point is the same as the end point of previous line, line
    //is connected to polygon and it is added
    if ( diff1 < eps || diff2 < eps ) {
      //We always want that start point is connected to end so we flip the
      //start and end in the new line if needed
      if ( diff2 < eps )
        l->flip_start_end();
      lines[Nlines] = l;
      Nlines++;
      return true;
    }
    //If we are here, the line was not connected to the current polygon
    return false;
  }
}

void Polygon::calculate_centroid()
{
  //We need a vector for the mean of the corners.
  double *mean = new double[DIM];
  for (int i=0; i < DIM; i++ ) {
    mean[i] = 0;
  }
  //First we determine the mean of the corner points. Every point
  //is here twice since lines are not ordered
  for (int i=0; i < Nlines; i++ ) {
    double *p1 = lines[i]->get_start(); 
    double *p2 = lines[i]->get_end(); 
    for (int j=0; j < DIM; j++ ) {
      mean[j] += p1[j] + p2[j];
    }
  }
  for (int j=0; j < DIM; j++ ) {
    mean[j] = mean[j]/double(2.0*Nlines);
  }
  //If only 3 lines, i.e. 3 corners, the polygon is always on a plane
  //and centroid is the mean
  if ( Nlines == 3 ) {
    for (int i=0; i < DIM; i++) {
      centroid[i] = mean[i];
    }
    centroid_calculated = 1;
    delete[] mean;
    return;
  }
  //If more than 3 corners, calculation of the centroid is more
  //complicated
  //Here we from triangles from the lines and the mean point
  double *sum_up = new double[DIM]; //areas of the single triangles 
  double sum_down = 0; //area of all the triangles
  for (int i=0; i < DIM; i++) {
    sum_up[i] = 0;
  }
  //a and b are vectors which from the triangle
  double *a = new double[DIM];
  double *b = new double[DIM];
  //centroid of the triangle (this is always on a plane)
  double *cm_i = new double[DIM];
  for (int i=0; i < Nlines; i++) {
    double *p1 = lines[i]->get_start();
    double *p2 = lines[i]->get_end();
    for (int j=0; j < DIM; j++) {
      cm_i[j] = (p1[j] + p2[j] + mean[j])/3.0;
    } 
    //Triangle is defined by these two vectors
    for (int j=0; j < DIM; j++) {
      a[j] = p1[j] - mean[j];
      b[j] = p2[j] - mean[j];
    }
    //Area is calculated from cross product of these vectors
    double A_i = 0.5*sqrt( pow(a[x2]*b[x3]-a[x3]*b[x2],2.0) + pow(a[x1]*b[x3]-a[x3]*b[x1],2.0) + pow(a[x2]*b[x1]-a[x1]*b[x2],2.0) );
    //Then we store the area and update the total area
    for (int j=0; j < DIM; j++) {
      sum_up[j] += cm_i[j]*A_i;
    }
    sum_down += A_i;
  }
  //Now we can determine centroid as a weighted average of the centroids
  //of the triangles
  for (int i=0; i < DIM; i++) {
    centroid[i] = sum_up[i]/sum_down;
  }
  //centroid is now calculated and memory is freed
  centroid_calculated = 1;
  delete[] sum_up;
  delete[] mean;
  delete[] a;
  delete[] b;
  delete[] cm_i;
}

void Polygon::calculate_normal()
{
  //centroid must be calculated before normal can be determined
  if ( !centroid_calculated )
    calculate_centroid();
  //First we find the normal for each triangle formed from
  //one edge and centroid
  double **normals = new double*[Nlines]; //these are the normals
  double *Vout = new double[DIM]; //the point which is always outside
  for (int i=0; i < Nlines; i++) {
    normals[i] = new double[DIM];
    for (int j=0; j < DIM; j++) {
      normals[i][j] = 0;
    }
  }
  //Normal is defined by these two vectors
  double *a = new double[DIM];
  double *b = new double[DIM];
  //Loop over all triangles
  for (int i=0; i < Nlines; i++) {
    //First we calculate the vectors which form the triangle
    double *p1 = lines[i]->get_start();
    double *p2 = lines[i]->get_end();
    for (int j=0; j < DIM; j++) {
      a[j] = p1[j] - centroid[j];
      b[j] = p2[j] - centroid[j];
    }
    //Normal is calculated as a cross product of these vectors
    normals[i][x1] =  0.5*(a[x2]*b[x3]-a[x3]*b[x2]);
    normals[i][x2] = -0.5*(a[x1]*b[x3]-a[x3]*b[x1]);
    normals[i][x3] =  0.5*(a[x1]*b[x2]-a[x2]*b[x1]);
    normals[i][const_i] = 0;
    //Then we construct a vector which points out
    double *o = lines[i]->get_out();
    for (int j=0; j < DIM; j++) {
      Vout[j] = o[j] - centroid[j];
    }
    //then we check that normal is point in the correct direction
    check_normal_direction(normals[i],Vout);
  }
  //Finally the normal is a sum of the normals of the triangles
  for (int i=0; i < DIM; i++) {
    normal[i] = 0;
  }
  for (int i=0; i < Nlines; i++) {
    for (int j=0; j < DIM; j++) {
      normal[j] += normals[i][j];
    }
  }
  //Normal is now calculated and memory can be freed
  normal_calculated = 1;
  delete[] a;
  delete[] b;
  for (int i=0; i < Nlines; i++) {
    delete[] normals[i];
  }
  delete[] normals;
  delete[] Vout;
}

int Polygon::get_Nlines()
{
  return Nlines;
}

Line** Polygon::get_lines()
{
  return lines;
}

void Polygon::print(ofstream &file, double *pos)
{
  for (int i=0; i < Nlines; i++) {
    double *p1 = lines[i]->get_start();
    double *p2 = lines[i]->get_end();
    file << pos[x1] + p1[x1] << " " << pos[x2] + p1[x2]  << " " << pos[x3] + p1[x3];
    file << " " << pos[x1] + p2[x1] << " " << pos[x2] + p2[x2]  << " " << pos[x3] + p2[x3];
    file << " " << pos[x1] + centroid[x1] << " " << pos[x2] + centroid[x2]  << " " << pos[x3] + centroid[x3] << endl;
  }
}

class Polyhedron : public GeneralElement
{
  private:
    static const int MAX_POLYGONS = 24;
    Polygon **polygons;
    int Npolygons;
    int Ntetrahedra;
    int x1,x2,x3,x4;
    bool lines_equal(Line*,Line*);
    void tetravolume(double*,double*,double*,double*);
    void calculate_centroid();
    void calculate_normal();
  public:
    Polyhedron();
    ~Polyhedron();
    void init();
    bool add_polygon(Polygon*,int);
};

Polyhedron::Polyhedron()
{
  GeneralElement();
  polygons = new Polygon*[MAX_POLYGONS];
}

Polyhedron::~Polyhedron()
{
  delete[] polygons;
}

void Polyhedron::init()
{
  //Here we fix the non-constant indices
  x1 = 0;
  x2 = 1;
  x3 = 2;
  x4 = 3;
  //We need to set these to zero so when this function is called
  //again we know that polyhedron has no polyfons added and normal
  //and centroid are not yet calculated with new values
  Npolygons = 0;
  Ntetrahedra = 0;
  normal_calculated = 0;
  centroid_calculated = 0;
}

bool Polyhedron::add_polygon(Polygon *p, int donocheck)
{
  //If this is the first polygon or we want to add it in any case, we
  //just add it automatically
  if ( donocheck || Npolygons == 0 ) {
    polygons[Npolygons] = p;
    Npolygons++;
    Ntetrahedra += p->get_Nlines();
    return true; 
  } else {
    //Here we check if the new polygon is connected with the last polygon in
    //polyhedron
    for (int i=0; i < Npolygons; i++) {
      int Nlines1 = p->get_Nlines();
      int Nlines2 = polygons[i]->get_Nlines();
      Line **lines1 = p->get_lines();
      Line **lines2 = polygons[i]->get_lines();
      for (int j=0; j < Nlines1; j++) {
        for (int k=0; k < Nlines2; k++) {
          if ( lines_equal(lines1[j],lines2[k]) ) {
            polygons[Npolygons] = p;
            Npolygons++;
            Ntetrahedra += p->get_Nlines();
            return true; 
          }
        }
      }
    }
    //If we are here, the polygon was not connected to polyhedron
    return false;
  }
}

bool Polyhedron::lines_equal(Line *l1, Line *l2)
{
  double eps = 1e-10;
  double *p11 = l1->get_start();
  double *p12 = l1->get_end();
  double *p21 = l2->get_start();
  double diff1 = 0;
  double diff2 = 0;
  for (int i=0; i < DIM; i++) {
    diff1 += fabs(p11[i]-p21[i]);
    diff2 += fabs(p12[i]-p21[i]);
    if ( diff1 > eps && diff2 > eps )
      return false;
  }
  //If we are here, the lines are connected
  return true;
}

void Polyhedron::tetravolume(double *a, double *b, double *c, double *n)
{
  double bc01 = b[0]*c[1]-b[1]*c[0];
  double bc02 = b[0]*c[2]-b[2]*c[0];
  double bc03 = b[0]*c[3]-b[3]*c[0];
  double bc12 = b[1]*c[2]-b[2]*c[1];
  double bc13 = b[1]*c[3]-b[3]*c[1];
  double bc23 = b[2]*c[3]-b[3]*c[2];
  n[0] =  1.0/6.0*(a[1]*bc23 - a[2]*bc13 + a[3]*bc12);
  n[1] = -1.0/6.0*(a[0]*bc23 - a[2]*bc03 + a[3]*bc02);
  n[2] =  1.0/6.0*(a[0]*bc13 - a[1]*bc03 + a[3]*bc01);
  n[3] = -1.0/6.0*(a[0]*bc12 - a[1]*bc02 + a[2]*bc01);
  /*n[0] =  1.0/6.0*(a[1]*(b[2]*c[3]-b[3]*c[2]) - a[2]*(b[1]*c[3]-b[3]*c[1])
                  +a[3]*(b[1]*c[2]-b[2]*c[1]));
  n[1] = -1.0/6.0*(a[0]*(b[2]*c[3]-b[3]*c[2]) - a[2]*(b[0]*c[3]-b[3]*c[0])
                  +a[3]*(b[0]*c[2]-b[2]*c[0]));
  n[2] =  1.0/6.0*(a[0]*(b[1]*c[3]-b[3]*c[1]) - a[1]*(b[0]*c[3]-b[3]*c[0])
                  +a[3]*(b[0]*c[1]-b[1]*c[0]));
  n[3] = -1.0/6.0*(a[0]*(b[1]*c[2]-b[2]*c[1]) - a[1]*(b[0]*c[2]-b[2]*c[0])
                  +a[2]*(b[0]*c[1]-b[1]*c[0]));*/
}

void Polyhedron::calculate_centroid()
{
  double *mean = new double[DIM];
  for (int i=0; i < DIM; i++ ) {
    mean[i] = 0;
  }
  //First we determine the mean of the corners
  for (int i=0; i < Npolygons; i++ ) {
    int Nlines = polygons[i]->get_Nlines();
    Line **lines = polygons[i]->get_lines();
    for (int j=0; j < Nlines; j++ ) {
      double *p1 = lines[j]->get_start();
      double *p2 = lines[j]->get_end();
      for (int k=0; k < DIM; k++ ) {
        mean[k] += p1[k] + p2[k];
      }
    }
  }
  for (int j=0; j < DIM; j++ ) {
    mean[j] = mean[j]/double(2.0*Ntetrahedra);
  }
  //Some memory allocated for temporary variables
  double *a = new double[DIM];
  double *b = new double[DIM];
  double *c = new double[DIM];
  double *n = new double[DIM];
  double *cm_i = new double[DIM];
  double *sum_up = new double[DIM];
  double sum_down = 0;
  for (int i=0; i < DIM; i++) {
    sum_up[i] = 0;
  }
  for (int i=0; i < Npolygons; i++ ) {
    int Nlines = polygons[i]->get_Nlines();
    Line **lines = polygons[i]->get_lines();
    double *cent = polygons[i]->get_centroid();
    for (int j=0; j < Nlines; j++) {
      double *p1 = lines[j]->get_start();
      double *p2 = lines[j]->get_end();
      //CM of the tetrahedra
      for (int k=0; k < DIM; k++) {
        cm_i[k] = (p1[k] + p2[k] + cent[k] + mean[k])/4.0;
      } 
      //Volume of a tetrahedron is determined next
      //Tetrahedron is defined by these three vectors
      for (int k=0; k < DIM; k++) {
        a[k] = p1[k] - mean[k];
        b[k] = p2[k] - mean[k];
        c[k] = cent[k] - mean[k];
      }
      //Then we calculate the volume from the normal n 
      tetravolume(a,b,c,n);
      double V_i = 0; 
      for (int k=0; k < DIM; k++) {
        V_i += n[k]*n[k];
      }
      V_i = sqrt(V_i);
      //Then we add contributions to sums
      for (int k=0; k < DIM; k++) {
        sum_up[k] += cm_i[k]*V_i;
      }
      sum_down += V_i;
    }
  }
  //Now the centroid is the volume weighted average of individual
  //tetrahedra
  for (int i=0; i < DIM; i++) {
    centroid[i] = sum_up[i]/sum_down;
  }
  //Centroid is now calculated and we can free memory
  centroid_calculated = 1;
  delete[] a;
  delete[] b;
  delete[] c;
  delete[] n;
  delete[] cm_i;
  delete[] sum_up;
  delete[] mean;
}

void Polyhedron::calculate_normal()
{
  //We need centroid in the following calculation and thus we
  //need to check that it is calculated
  if ( !centroid_calculated )
    calculate_centroid();
  //First we allocate memory for temporary variables
  double *Vout = new double[DIM];
  double *a = new double[DIM];
  double *b = new double[DIM];
  double *c = new double[DIM];
  double **normals = new double*[Ntetrahedra];
  for (int i=0; i < Ntetrahedra; i++) {
    normals[i] = new double[DIM];
  }
  int Ntetra = 0; //Index of tetrahedron we are dealing with
  for (int i=0; i < Npolygons; i++ ) {
    int Nlines = polygons[i]->get_Nlines();
    Line **lines = polygons[i]->get_lines();
    double *cent = polygons[i]->get_centroid();
    for (int j=0; j < Nlines; j++) {
      double *p1 = lines[j]->get_start();
      double *p2 = lines[j]->get_end();
      //Tetrahedron is defined by these three vectors
      for (int k=0; k < DIM; k++) {
        a[k] = p1[k] - centroid[k];
        b[k] = p2[k] - centroid[k];
        c[k] = cent[k] - centroid[k];
      }
      //Normal is calculated with the same function as volume
      tetravolume(a,b,c,normals[Ntetra]);
      //Then we determine the direction towards lower energy
      double *o = lines[j]->get_out();
      for (int k=0; k < DIM; k++) {
        Vout[k] = o[k] - centroid[k];
      }
      check_normal_direction(normals[Ntetra],Vout);
      Ntetra++;
    }
  }
  //Finally the element normal is a sum of the calculated normals
  for (int i=0; i < DIM; i++) {
    normal[i] = 0;
  }
  for (int i=0; i < Ntetrahedra; i++) {
    for (int j=0; j < DIM; j++) {
      normal[j] += normals[i][j];
    }
  }
  //Normal is now determined and we can free memory
  normal_calculated = 1;
  for (int i=0; i < Ntetrahedra; i++) {
    delete[] normals[i];
  }
  delete[] a;
  delete[] b;
  delete[] c;
  delete[] normals;
  delete[] Vout;
}

class Square
{
  private:
    static const int DIM = 4;
    static const int SQUARE_DIM = 2;
    static const int MAX_POINTS = 4;
    static const int MAX_LINES = 2;
    double **points;
    double **cuts;
    double **out;
    double **points_temp;
    double *out_temp;
    int *const_i;
    double *const_value;
    int x1, x2;
    double *dx;
    int Ncuts;
    int Nlines;
    Line *lines;
    int ambiguous;
    void ends_of_edge(double);
    void find_outside(double);
  public:
    Square();
    ~Square();
    void init(double**,int*,double*,double*);
    void construct_lines(double);
    int is_ambiguous();
    int get_Nlines();
    Line* get_lines();
};

Square::Square()
{
  cuts = new double*[MAX_POINTS];
  out = new double*[MAX_POINTS];
  points = new double*[SQUARE_DIM];
  const_i = new int[DIM-SQUARE_DIM];
  const_value = new double[DIM-SQUARE_DIM];
  lines = new Line[MAX_LINES];
  for (int i=0; i < SQUARE_DIM; i++) {
    points[i] = new double[SQUARE_DIM];
  }
  for (int i=0; i < MAX_POINTS; i++) {
    cuts[i] = new double[SQUARE_DIM];
    out[i] = new double[SQUARE_DIM];
  }
  int Npoints = 2;
  points_temp = new double*[SQUARE_DIM];
  out_temp = new double[DIM];
  for (int j=0; j < Npoints; j++) {
    points_temp[j] = new double[DIM];
  }
  ambiguous = 0;
}

Square::~Square()
{
  for (int i=0; i < DIM-SQUARE_DIM; i++) {
    delete[] points[i];
  }
  delete[] points;
  for (int i=0; i < MAX_POINTS; i++) {
    delete[] cuts[i];
    delete[] out[i];
  }
  delete[] cuts;
  delete[] out;
  delete[] const_i;
  delete[] const_value;
  delete[] lines;
  for (int i=0; i < SQUARE_DIM; i++) {
    delete[] points_temp[i];
  }
  delete[] points_temp;
  delete[] out_temp;
}

void Square::init(double **sq, int *c_i, double *c_v, double *dex)
{
  for (int i=0; i < SQUARE_DIM; i++) {
    for (int j=0; j < SQUARE_DIM; j++) {
      points[i][j] = sq[i][j];
    }
  }
  for (int i=0; i < DIM-SQUARE_DIM; i++) {
    const_i[i] = c_i[i];
    const_value[i] = c_v[i];
  }
  dx = dex;
  x1 = -1;
  x2 = -1;
  for (int i=0; i < DIM; i++) {
    if ( i != const_i[0] && i != const_i[1] ) {
      if ( x1 < 0 ) {
        x1 = i;
      } else {
        x2 = i;
      }
    }
  }
  //We need to set these to zero so when this function is called
  //again we know that nothing has been done for this square
  Ncuts = 0;
  Nlines = 0;
  ambiguous = 0;
}

void Square::construct_lines(double E0)
{
  //First we check the corner points and see if there are any lines
  int above=0;
  for (int i=0; i < DIM-SQUARE_DIM; i++) {
    for (int j=0; j < DIM-SQUARE_DIM; j++) {
      if ( points[i][j] >= E0 )
        above++; 
    }
  }
  //If all corners are above or below the surface value, no lines in this
  //square
  if ( above == 0 || above == 4 ) {
    Nlines = 0;
    return;
  }
  //First we find the cut points and the points which are always outside of the
  //surface. Also find_outside() arranges cuts so that first two cuts form a line
  //as defined in the algorithm (in case there are 4 cuts)
  ends_of_edge(E0);
  if ( Ncuts > 0 )
    find_outside(E0);
  //Then we go through the cut points and form the line elements 
  for (int i=0; i < Ncuts; i++) {
    points_temp[i%2][x1] = cuts[i][0];
    points_temp[i%2][x2] = cuts[i][1];
    points_temp[i%2][const_i[0]] = const_value[0];
    points_temp[i%2][const_i[1]] = const_value[1];
    //When we have inserted both endpoints we insert outside point
    //and we are ready to create line element.
    if ( i%2 == 1 ) {
      out_temp[x1] = out[i/2][0];
      out_temp[x2] = out[i/2][1];
      out_temp[const_i[0]] = const_value[0];
      out_temp[const_i[1]] = const_value[1];
      lines[i/2].init(points_temp,out_temp,const_i);
      Nlines++;
    }
  }
}

void Square::ends_of_edge(double E0)
{
  //Edge 1
  if ( (points[0][0]-E0)*(points[1][0]-E0) < 0 ) {
    cuts[Ncuts][0] = (points[0][0]-E0)/(points[0][0]-points[1][0])*dx[x1];
    cuts[Ncuts][1] = 0;
    Ncuts++;
  } else if ( points[0][0] == E0 && points[1][0] < E0 ) {
    cuts[Ncuts][0] = 1e-9*dx[x1];
    cuts[Ncuts][1] = 0;
    Ncuts++;
  } else if ( points[1][0] == E0 && points[0][0] < E0 ) {
    cuts[Ncuts][0] = (1.0-1e-9)*dx[x1];
    cuts[Ncuts][1] = 0;
    Ncuts++;
  }
  //Edge 2
  if ( (points[0][0]-E0)*(points[0][1]-E0) < 0 ) {
    cuts[Ncuts][0] = 0;
    cuts[Ncuts][1] = (points[0][0]-E0)/(points[0][0]-points[0][1])*dx[x2];
    Ncuts++;
  } else if ( points[0][0] == E0 && points[0][1] < E0 ) {
    cuts[Ncuts][0] = 0;
    cuts[Ncuts][1] = 1e-9*dx[x2];
    Ncuts++;
  } else if ( points[0][1] == E0 && points[0][0] < E0 ) {
    cuts[Ncuts][0] = 0;
    cuts[Ncuts][1] = (1.0-1e-9)*dx[x2];
    Ncuts++;
  }
  //Edge 3
  if ( (points[1][0]-E0)*(points[1][1]-E0) < 0 ) {
    cuts[Ncuts][0] = dx[x1];
    cuts[Ncuts][1] = (points[1][0]-E0)/(points[1][0]-points[1][1])*dx[x2];
    Ncuts++;
  } else if ( points[1][0] == E0 && points[1][1] < E0 ) {
    cuts[Ncuts][0] = dx[x1];
    cuts[Ncuts][1] = 1e-9*dx[x2];
    Ncuts++;
  } else if ( points[1][1] == E0 && points[1][0] < E0 ) {
    cuts[Ncuts][0] = dx[x1];
    cuts[Ncuts][1] = (1.0-1e-9)*dx[x2];
    Ncuts++;
  }
  //Edge 4
  if ( (points[0][1]-E0)*(points[1][1]-E0) < 0 ) {
    cuts[Ncuts][0] = (points[0][1]-E0)/(points[0][1]-points[1][1])*dx[x1];
    cuts[Ncuts][1] = dx[x2];
    Ncuts++;
  } else if ( points[0][1] == E0 && points[1][1] < E0 ) {
    cuts[Ncuts][0] = 1e-9*dx[x1];
    cuts[Ncuts][1] = dx[x2];
    Ncuts++;
  } else if ( points[1][1] == E0 && points[0][1] < E0 ) {
    cuts[Ncuts][0] = (1.0-1e-9)*dx[x1];
    cuts[Ncuts][1] = dx[x2];
    Ncuts++;
  }
  if ( Ncuts != 0 && Ncuts != 2 && Ncuts != 4 ) {
    cout << "Error in EndsOfEdge: Ncuts " << Ncuts << endl;
    exit(1);
  }
}

void Square::find_outside(double E0)
{
  if ( Ncuts == 4 ) { //Here the surface is ambiguous
    ambiguous = 1;
    //Let's calculate the value in the middle of the square
    double Emid = 0;
    for (int i=0; i < 2; i++) {
      for (int j=0; j < 2; j++) {
        Emid += 0.25*points[i][j];
      }
    }
    //The default is that cuts are connected as \\ here
    //If both Emid and (0,0) are above or below the criterion
    //the cuts should be like // and we have to switch order in cuts
    if ( (points[0][0] < E0 && Emid < E0) || (points[0][0] > E0 && Emid > E0) ) {
      for (int i=0; i < 2; i++) {
        double temp = cuts[1][i];
        cuts[1][i] = cuts[2][i];
        cuts[2][i] = temp;
      }
    }
    //The center is below, so the middle point is always outside
    //the surface
    if ( (Emid-E0) < 0 ) {
      for (int i=0; i < 2; i++) {
        for (int j=0; j < 2; j++) {
          if ( j == 0 )
            out[i][j] = 0.5*dx[x1];
          else
            out[i][j] = 0.5*dx[x2];
        }
      }
    } else { // The center is above
       // Cuts are \\ here so bl and tr corners are outside
      if ( (points[0][0]-E0) < 0 ) {
        for (int i=0; i < 2; i++) {
          out[0][i] = 0;
          if ( i == 0 )
            out[1][i] = dx[x1];
          else
            out[1][i] = dx[x2];
        }
      // Cuts are // here so br and tl corners are outside
      } else {
        out[0][0] = dx[x1];
        out[0][1] = 0;
        out[1][0] = 0;
        out[1][1] = dx[x2];
      }
    }
  } else { //This is the normal case (not ambiguous)
    for (int i=0; i < 2; i++) {
      for (int j=0; j < 2; j++) {
        out[i][j] = 0;
      }
    }
    int Nout = 0;
    for (int i=0; i < 2; i++) {
      for (int j=0; j < 2; j++) {
        if ( points[i][j] < E0 ) {
          out[0][0] += i*dx[x1];
          out[0][1] += j*dx[x2];
          Nout++;
        }
      }
    }
    if ( Nout > 0 ) {
      for (int i=0; i < 2; i++) {
        for (int j=0; j < 2; j++) {
          out[i][j] = out[i][j]/double(Nout);
        }
      }
    }
  }
}

int Square::is_ambiguous()
{
  return ambiguous;
}

int Square::get_Nlines()
{
  return Nlines;
}

Line* Square::get_lines()
{
  return lines;
}


class Cube
{
  private:
    static const int DIM = 4;
    static const int CUBE_DIM = 4;
    static const int MAX_POLY = 8;
    static const int NSQUARES = 6;
    static const int STEPS = 2;
    double ***cube;
    Line **lines;
    Polygon *polygons;
    Square *squares;
    int Nlines;
    int Npolygons;
    int ambiguous;
    int const_i;
    double const_value;
    int x1,x2,x3;
    double *dx;
    void split_to_squares();
    void check_ambiguous(int);
  public:
    Cube();
    ~Cube();
    void init(double***&,int,double,double*&);
    void construct_polygons(double);
    int get_Nlines();
    int get_Npolygons();
    int is_ambiguous();
    Polygon* get_polygons();
};

Cube::Cube()
{
  cube = new double**[STEPS];
  for (int i=0; i < STEPS; i++) {
    cube[i] = new double*[STEPS];
    for (int j=0; j < STEPS; j++) {
      cube[i][j] = new double[STEPS];
    }
  }
  lines = new Line*[NSQUARES*2]; //Each square may have max. 2 lines
  polygons = new Polygon[MAX_POLY];
  squares = new Square[NSQUARES];
}

Cube::~Cube()
{
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) {
      delete[] cube[i][j];
    }
    delete[] cube[i];
  }
  delete[] cube;
  delete[] lines;
  delete[] polygons;
  delete[] squares;
}

void Cube::init(double ***&c, int c_i, double c_v, double *&dex)
{
  const_i = c_i;
  const_value = c_v;
  dx = dex;
  //Here we fix the non-zero indices
  x1 = -1;
  x2 = -1;
  x3 = -1;
  for (int i=0; i < DIM; i++) {
    if ( i != c_i ) {
      if ( x1 < 0 ) {
        x1 = i;
      } else if ( x2 < 0 ) {
        x2 = i;
      } else {
        x3 = i;
      }
    }
  }
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) {
      for (int k=0; k < STEPS; k++) {
        cube[i][j][k] = c[i][j][k];
      }
    }
  }
  //We need to set these to zero so when this function is called
  //again we know that nothing has been done for this cube
  Nlines = 0;
  Npolygons = 0;
  ambiguous = 0;
}

void Cube::split_to_squares()
{
  double **sq = new double*[STEPS];
  int *c_i = new int[STEPS];
  double *c_v = new double[STEPS];
  for (int i=0; i < STEPS; i++) {
    sq[i] = new double[STEPS]; 
  }
  int Nsquares = 0;
  for (int i=0; i < DIM; i++) {
    //i is the index which is kept constant, thus we ignore the index which
    //is constant in this cube
    if ( i != const_i ) {
      c_i[0] = const_i;
      c_i[1] = i;
      for (int j=0; j < STEPS; j++) {
        c_v[0] = const_value;
        c_v[1] = j*dx[i];
        for (int ci1=0; ci1 < STEPS; ci1++) {
          for (int ci2=0; ci2 < STEPS; ci2++) {
            if ( i == x1 ) {
              sq[ci1][ci2] = cube[j][ci1][ci2];
            } else if ( i == x2 ) {
              sq[ci1][ci2] = cube[ci1][j][ci2];
            } else {
              sq[ci1][ci2] = cube[ci1][ci2][j];
            }
          }
        }
        squares[Nsquares].init(sq,c_i,c_v,dx);
        Nsquares++;
      }
    }
  }
  for (int i=0; i < STEPS; i++) {
    delete[] sq[i];
  }
  delete[] sq; 
  delete[] c_i;
  delete[] c_v;
}

void Cube::construct_polygons(double value0)
{
  //Let's start by splitting the cube to squares and finding
  //the lines from them
  split_to_squares();
  for (int i=0; i < NSQUARES; i++) {
    squares[i].construct_lines(value0);
  } 
  //Then we make a table which contains pointers to the lines
  Nlines = 0;
  for (int i=0; i < NSQUARES; i++) {
    int Nline = squares[i].get_Nlines();
    Line *l = squares[i].get_lines();
    for (int j=0; j < Nline; j++) {
      lines[Nlines] = &l[j];
      double *p = lines[Nlines]->get_start();
      p = lines[Nlines]->get_end();
      Nlines++; 
    }
    l = NULL;
  }
  //If no lines were found we may exit. This can happen only in 4D case
  if ( Nlines == 0 ) {
    return;
  }
  //Then we check if the surface is ambiguous and continue accordingly
  check_ambiguous(Nlines);
  if ( ambiguous > 0 ) {
    //Surface is ambiguous, so let's connect the lines to polygons and see how
    //many polygons we have
    int *not_used = new int[Nlines];
    for (int i=0; i < Nlines; i++) {
      not_used[i] = 1;
    }
    int used = 0; //this keeps track how many lines we have used
    //We may found several polygons with this
    do {
      if ( Nlines - used < 3 ) {
        cout << "Error: cannot construct polygon from " << Nlines -used << " lines" << endl;
        exit(1);
      }
      //First we initialize polygon
      polygons[Npolygons].init(const_i);
      //Then we go through all lines and try to add them to polygon
      for (int i=0; i < Nlines; i++) {
        if ( not_used[i] ) {
          //add_line returns true if line is succesfully added
          if ( polygons[Npolygons].add_line(lines[i],0) ) {
            not_used[i]=0;
            used++;
            //if line is succesfully added we start the loop from the
            //beginning
            i=0;
          }
        }
      }
      //When we have reached this point one complete polygon is formed
      Npolygons++;
    } while ( used < Nlines );
    delete[] not_used;
  } else {
    //Surface is not ambiguous, so we have only one polygons and all lines
    //can be added to it without ordering them
    polygons[Npolygons].init(const_i);
    for (int i=0; i < Nlines; i++) {
      polygons[Npolygons].add_line(lines[i],1);
    }
    Npolygons++;
  }
}

void Cube::check_ambiguous(int Nlines)
{
  //First we check if any squares may have ambiguous elements
  for (int i=0; i < NSQUARES; i++) {
    if ( squares[i].is_ambiguous() ) {
      ambiguous++;
    }
  }
  //If the surface is not ambigous already, it is still possible to
  //have a ambiguous case if we have exatcly 6 lines, i.e. the surface
  //elements are at the opposite corners
  if ( ambiguous == 0 && Nlines == 6 ) {
    ambiguous++;
  }
}

int Cube::is_ambiguous()
{
  return ambiguous;
}

int Cube::get_Nlines()
{
  return Nlines;
}

int Cube::get_Npolygons()
{
  return Npolygons;
}

Polygon* Cube::get_polygons()
{
  return polygons;
}

class Hypercube
{
  private:
    static const int DIM = 4;
    static const int MAX_POLY = 10;
    static const int NCUBES = 8;
    static const int STEPS = 2;
    double ****hcube;
    Polyhedron *polyhedrons;
    Polygon **polygons;
    Cube *cubes;
    int Npolyhedrons;
    int ambiguous;
    int x1,x2,x3,x4;
    double *dx;
    void split_to_cubes();
    void check_ambiguous(double);
  public:
    Hypercube();
    ~Hypercube();
    void init(double****,double*);
    void construct_polyhedrons(double);
    int get_Npolyhedrons();
    Polyhedron* get_polyhedrons();
};

Hypercube::Hypercube()
{
  hcube = new double***[STEPS];
  for (int i=0; i < STEPS; i++) {
    hcube[i] = new double**[STEPS];
    for (int j=0; j < STEPS; j++) {
      hcube[i][j] = new double*[STEPS];
      for (int k=0; k < STEPS; k++) {
        hcube[i][j][k] = new double[STEPS];
      }
    }
  }
  polyhedrons = new Polyhedron[MAX_POLY];
  polygons = new Polygon*[NCUBES*10];
  cubes = new Cube[NCUBES];
}

Hypercube::~Hypercube()
{
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) {
      for (int k=0; k < STEPS; k++) {
        delete[] hcube[i][j][k];
      }
      delete[] hcube[i][j];
    }
    delete[] hcube[i];
  }
  delete[] hcube;
  delete[] polyhedrons;
  delete[] polygons;
  delete[] cubes;
}

void Hypercube::init(double ****c, double *dex)
{
  dx = dex;
  //Here we fix the non-zero indices
  x1 = 0;
  x2 = 1;
  x3 = 2;
  x4 = 3;
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) {
      for (int k=0; k < STEPS; k++) {
        for (int l=0; l < STEPS; l++) {
          hcube[i][j][k][l] = c[i][j][k][l];
        }
      }
    }
  }
  //We need to set these to zero so we can use this function to initialize
  //many hypercubes
  Npolyhedrons = 0;
  ambiguous = 0;
}

void Hypercube::split_to_cubes()
{
  double ***cu = new double**[STEPS];
  for (int i=0; i < STEPS; i++) {
    cu[i] = new double*[STEPS]; 
    for (int j=0; j < STEPS; j++) {
      cu[i][j] = new double[STEPS]; 
    }
  }
  int Ncubes = 0;
  for (int i=0; i < DIM; i++) {
    //i is the index which is kept constant, thus we ignore the index which
    //is constant in this cube
    int c_i = i;
    for (int j=0; j < STEPS; j++) {
      double c_v = j*dx[i];
      for (int ci1=0; ci1 < STEPS; ci1++) {
        for (int ci2=0; ci2 < STEPS; ci2++) {
          for (int ci3=0; ci3 < STEPS; ci3++) {
            if ( i == x1 ) {
              cu[ci1][ci2][ci3] = hcube[j][ci1][ci2][ci3];
            } else if ( i == x2 ) {
              cu[ci1][ci2][ci3] = hcube[ci1][j][ci2][ci3];
            } else if ( i == x3 ) {
              cu[ci1][ci2][ci3] = hcube[ci1][ci2][j][ci3];
            } else {
              cu[ci1][ci2][ci3] = hcube[ci1][ci2][ci3][j];
            }
          }
        }
      }
      cubes[Ncubes].init(cu,c_i,c_v,dx);
      Ncubes++;
    }
  }
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) {
      delete[] cu[i][j];
    }
    delete[] cu[i];
  }
  delete[] cu;
}

void Hypercube::construct_polyhedrons(double value0)
{
  split_to_cubes();
  for (int i=0; i < NCUBES; i++) {
    cubes[i].construct_polygons(value0);
  }
  int Npolygons = 0;
  //then polygons are loaded to table
  for (int i=0; i < NCUBES; i++) {
    int Npoly = cubes[i].get_Npolygons();
    Polygon *p = cubes[i].get_polygons();
    for (int j=0; j < Npoly; j++) {
      polygons[Npolygons] = &p[j];
      Npolygons++; 
    }
  }
  check_ambiguous(value0);
  if ( ambiguous > 0 ) {
    //Here surface might be ambiguous and we need to connect the polygons and
    //see how many polyhedrons we have
    int *not_used = new int[Npolygons];
    for (int i=0; i < Npolygons; i++) {
      not_used[i] = 1;
    }
    int used = 0; //this keeps track how many lines we have used
    //We may found several polyhedrons with this
    do {
      //First we initialize polyhedron
      polyhedrons[Npolyhedrons].init();
      //Then we go through all polygons and try to add them to polyhedron
      for (int i=0; i < Npolygons; i++) {
        if ( not_used[i] ) {
          //add_polygon returns true if the polygon is succesfully added
          if ( polyhedrons[Npolyhedrons].add_polygon(polygons[i],0) ) {
            not_used[i]=0;
            used++;
            //if polygon is succesfully added we start the loop from the
            //beginning
            i=0;
          }
        }
      }
      //When we have reached this point one complete polyhedron is formed
      Npolyhedrons++;
    } while ( used < Npolygons );
    delete[] not_used;
    /*if ( ambiguous == 0 && Npolyhedrons != 1 ) {
      cout << "error" << endl;
    }*/
  } else {
    //Here surface cannot be ambiguous and all polygons can be added to
    //the polygehdron without ordering them
    polyhedrons[Npolyhedrons].init();
    for (int i=0; i < Npolygons; i++) {
      polyhedrons[Npolyhedrons].add_polygon(polygons[i],1);
    }
    //When we have reached this point one complete polyhedron is formed
    Npolyhedrons++;
  }
}

void Hypercube::check_ambiguous(double value0)
{
  for (int i=0; i < NCUBES; i++) {
    if ( cubes[i].is_ambiguous() ) {
      ambiguous++;
    }
  }
  if ( ambiguous == 0 ) {
    int Nlines = 0;
    for (int i=0; i < NCUBES; i++) {
      Nlines += cubes[i].get_Nlines();
    }
    int points = 0;
    for (int i1=0; i1 < STEPS; i1++) {
      for (int i2=0; i2 < STEPS; i2++) {
        for (int i3=0; i3 < STEPS; i3++) {
          for (int i4=0; i4 < STEPS; i4++) {
            if ( hcube[i1][i2][i3][i4] < value0 ) {
              points++;
            }
          }
        }
      }
    }
    if ( points > 8 ) {
      points = 16-points;
    }
    /* these are not needed
    if ( Nlines == 42 ) {
      //ambiguous++;
    } else if ( Nlines == 36 && ( points == 5 || points == 4 ) ) {
      //ambiguous++;
    } else if ( Nlines == 30 && points == 3 ) {
      //ambiguous++;
    }*/
    if ( Nlines == 24 && points == 2 ) {
      ambiguous++;
    }
  }
}

int Hypercube::get_Npolyhedrons()
{
  return Npolyhedrons;
}

Polyhedron* Hypercube::get_polyhedrons()
{
  return polyhedrons;
}

class Cornelius
{
  private:
    static const int STEPS = 2;
    static const int DIM = 4;
    static const int MAX_ELEMENTS = 10;
    int Nelements;
    double **normals;
    double **centroids;
    int cube_dim;
    int initialized;
    int print_initialized;
    double value0;
    double *dx;
    ofstream output_print;
    void surface_3d(double***,double*,int);
    Square cu2d;
    Cube cu3d;
    Hypercube cu4d;
  public:
    Cornelius();
    ~Cornelius();
    void init(int,double,double*);
    void init_print(string);
    void find_surface_2d(double**);
    void find_surface_3d(double***);
    void find_surface_3d_print(double***,double*);
    void find_surface_4d(double****);
    int get_Nelements();
    double **get_normals();
    double **get_centroids();
    double **get_normals_4d();
    double **get_centroids_4d();
    double get_centroid_elem(int,int);
    double get_normal_elem(int,int);
};

Cornelius::Cornelius()
{
  Nelements = 0;
  initialized = 0;
  print_initialized = 0;
  normals = new double*[MAX_ELEMENTS];
  centroids = new double*[MAX_ELEMENTS];
  for (int i=0; i < MAX_ELEMENTS; i++) {
    normals[i] = new double[DIM];
    centroids[i] = new double[DIM];
  }
}

Cornelius::~Cornelius()
{
  for (int i=0; i < MAX_ELEMENTS; i++) {
    delete[] normals[i];
    delete[] centroids[i];
  }
  delete[] normals;
  delete[] centroids;
  if ( initialized ) {
    delete[] dx;
  }
  //If file for printing was opened we close it here.
  if ( print_initialized ) {
    output_print.close();
  }
}

void Cornelius::init(int d, double v0, double *dex)
{
  cube_dim = d;
  value0 = v0;
  if ( initialized != 1 ) {
    dx = new double[DIM];
  }
  for (int i=0; i < DIM; i++) {
    if ( i < DIM-cube_dim ) {
      dx[i] = 1;
    } else {
      dx[i] = dex[i-(DIM-cube_dim)];
    }
  }
  initialized = 1;
}

void Cornelius::init_print(string filename)
{
  output_print.open(filename.c_str());
  print_initialized = 1;
}

void Cornelius::find_surface_2d(double **cube)
{
  if ( !initialized || cube_dim != 2 ) {
    cout << "Cornelius not initialized for 2D case" << endl;
    exit(1);
  }
  int *c_i = new int[cube_dim];
  double *c_v = new double[cube_dim];
  c_i[0] = 0;
  c_i[1] = 1;
  c_v[0] = 0;
  c_v[1] = 0;
  cu2d.init(cube,c_i,c_v,dx);
  cu2d.construct_lines(value0);
  Nelements = cu2d.get_Nlines();
  Line *l = cu2d.get_lines();
  for (int i=0; i < Nelements; i++) {
    for (int j=0; j < DIM; j++) {
      normals[i][j] = l[i].get_normal()[j];
      centroids[i][j] = l[i].get_centroid()[j];
    }
  }
  delete[] c_i;
  delete[] c_v;
}

void Cornelius::find_surface_3d(double ***cube)
{
  double *pos = NULL;
  surface_3d(cube,pos,0);
}

void Cornelius::find_surface_3d_print(double ***cube, double *pos)
{
  surface_3d(cube,pos,1);
}

void Cornelius::surface_3d(double ***cube, double *pos, int do_print)
{
  if ( !initialized || cube_dim != 3 ) {
    cout << "Cornelius not initialized for 3D case" << endl;
    exit(1);
  }
  //First we check if the cube actually contains surface elements.
  //If all or none of the elements are below the criterion, no surface
  //elements exist.
  int above = 0;
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) { 
      for (int k=0; k < STEPS; k++) {
        if ( cube[i][j][k] >= value0 )
          above++;
      }
    }
  }
  if ( above == 0 || above == 8 ) {
    //No elements in this cube
    Nelements = 0;
    return;
  }
  //This cube has surface elements, so let's start by constructing
  //the cube.
  int c_i = 0;
  double c_v = 0;
  cu3d.init(cube,c_i,c_v,dx);
  //Then we find the elements
  cu3d.construct_polygons(value0);
  //Now let's get the information about the elements
  Nelements = cu3d.get_Npolygons();
  Polygon *p = cu3d.get_polygons();
  for (int i=0; i < Nelements; i++) {
    //Here we always work with 4-dimensions
    for (int j=0; j < DIM; j++) {
      normals[i][j] = p[i].get_normal()[j];
      centroids[i][j] = p[i].get_centroid()[j];
    }
    //If we want to print the actual triangles, which are found, we must do it here
    //before polygons are removed from the memory.
    if ( print_initialized && do_print ) {
      p[i].print(output_print,pos);
    }
  }
}

void Cornelius::find_surface_4d(double ****cube)
{
  if ( !initialized || cube_dim != 4 ) {
    cout << "Cornelius not initialized for 4D case" << endl;
    exit(1);
  }
  //First we check if the cube actually contains surface elements.
  //If all or none of the elements are below the criterion, no surface
  //elements exist.
  int above = 0;
  for (int i=0; i < STEPS; i++) {
    for (int j=0; j < STEPS; j++) { 
      for (int k=0; k < STEPS; k++) {
        for (int l=0; l < STEPS; l++) {
          if ( cube[i][j][k][l] >= value0 )
            above++;
        }
      }
    }
  }
  if ( above == 0 || above == 16 ) {
    //No elements in this cube
    Nelements = 0;
    return;
  }
  //This cube has surface elements, so let's start by constructing
  //the hypercube.
  cu4d.init(cube,dx);
  //Then we find the elements
  cu4d.construct_polyhedrons(value0);
  //Now let's get the information about the elements
  Nelements = cu4d.get_Npolyhedrons();
  Polyhedron *p = cu4d.get_polyhedrons();
  for (int i=0; i < Nelements; i++) {
    for (int j=0; j < DIM; j++) {
      centroids[i][j] = p[i].get_centroid()[j];
      normals[i][j] = p[i].get_normal()[j];
    }
  }
}

int Cornelius::get_Nelements()
{
  return Nelements;
}

double** Cornelius::get_centroids_4d()
{
  double **vect = new double*[Nelements];
  for (int i=0; i < Nelements; i++) {
    vect[i] = new double[DIM];
    for (int j=0; j < DIM; j++) {
      vect[i][j] = centroids[i][j];
    }
  }
  return vect;
}

double** Cornelius::get_normals_4d()
{
  double **vect = new double*[Nelements];
  for (int i=0; i < Nelements; i++) {
    vect[i] = new double[DIM];
    for (int j=0; j < DIM; j++) {
      vect[i][j] = normals[i][j];
    }
  }
  return vect;
}

double** Cornelius::get_centroids()
{
  double **vect = new double*[Nelements];
  for (int i=0; i < Nelements; i++) {
    vect[i] = new double[cube_dim];
    for (int j=0; j < cube_dim; j++) {
      vect[i][j] = centroids[i][j+(DIM-cube_dim)];
    }
  }
  return vect;
}

double** Cornelius::get_normals()
{
  double **vect = new double*[Nelements];
  for (int i=0; i < Nelements; i++) {
    vect[i] = new double[cube_dim];
    for (int j=0; j < cube_dim; j++) {
      vect[i][j] = normals[i][j+(DIM-cube_dim)];
    }
  }
  return vect;
}

double Cornelius::get_centroid_elem(int i, int j)
{
  if ( i >= Nelements || j >= cube_dim ) {
    cout << "Cornelius error: asking for an element which does not exist." << endl;
    exit(1);
  }
  return centroids[i][j+(DIM-cube_dim)];
}

double Cornelius::get_normal_elem(int i, int j)
{
  if ( i >= Nelements || j >= cube_dim ) {
    cout << "Cornelius error: asking for an element which does not exist." << endl;
    exit(1);
  }
  return normals[i][j+(DIM-cube_dim)];
}