/*
 * Creates an image with a color gradient in the direction of the y-axis
 */

PImage createFloor(color backgroundColor){
  PImage img = createImage(width,height,RGB);

  img.loadPixels();
  for (int y = 0; y < height; y++){
    for (int x = 0; x < width; x++){
      int loc = x + y*width;
      float gradientR = map(y,0,height,red(backgroundColor),40);
      float gradientG = map(y,0,height,green(backgroundColor),40);
      float gradientB = map(y,0,height,blue(backgroundColor),40);
      img.pixels[loc] = color(gradientR-15*y/height,gradientG-15*y/height,gradientB);
    }
  }
  img.updatePixels();
  
  return img;
}

/*
 * Scene controls
 */

void mouseDragged(){
  noCursor();
  rotX -= map(mouseY-pmouseY,-height,height,-TWO_PI,TWO_PI);
  rotY -= map(mouseX-pmouseX,-width,width,-TWO_PI,TWO_PI);
}

void mouseReleased(){
  cursor();
}

void keyPressed(){
  switch(keyCode){
    case UP:
      zoom *= 1.02;
      break;
    case DOWN:
      zoom /= 1.02;
      break;
    case LEFT:
      resolution++;
      if(resolution > 10){
        resolution = 10;
      }
      else{
        scan = new ScanContainer(scans[currentScan]);
        scan = scan.reduceResolution(resolution);
        backSide = scan.offsetScan(offsetDist);
      }
      break;
    case RIGHT:
      resolution--;
      if(resolution < 1){
        resolution = 1;
      }
      else{
        scan = new ScanContainer(scans[currentScan]);
        scan = scan.reduceResolution(resolution);
        backSide = scan.offsetScan(offsetDist);
      }
      break;
  }
  if(key == ' '){
    psychedelic = !psychedelic;
  }
  if(key == 'l'){
    asLines = !asLines;
    asPixels = false;
  }
  if(key == 'p'){
    asPixels = !asPixels;
    asLines = false;
  }
  if(key == '+'){
    pixelSize++;
    if(pixelSize > 3) pixelSize = 3;
  }
  if(key == '-'){
    pixelSize--;
    if(pixelSize < 1) pixelSize = 1;
  } 
}

/* 
 * Scan viewer (JGC, version 3).
 *
 * Description: Reads and draws scans obtained with the Kinect 3D scanner.
 *   http://www.openprocessing.org/sketch/78606
 * 
 * Controls:
 *   - Use the mouse to rotate the scan
 *   - Use "Up" and "Down" arrows to zoom
 *   - Use "Left" and "Right" arrows to decrease/increase the scan resolution
 *   - 'p' switchs between the mesh view and the pixels view
 *   - 'l' switchs between the mesh view and the lines view
 *   - "+" and "-" increase/decrease the pixel size (only in pixel view)
 *   - 'Space' creates some psychedelic effects (only in mesh view) 
 */

import controlP5.*;

String[] files = {"scan1.points","scan2.points","scan3.points","scan4.points","scan-slit1.points","scan-slit2.points"};
ScanContainer[] scans = new ScanContainer[files.length];
ScanContainer scan, backSide;
int currentScan = 0;
int resolution = 1;
int pixelSize = 1;
float offsetDist = 0.1; 
boolean asPixels = false;
boolean asLines = false;
boolean psychedelic = false;

ControlP5 cp5;
PImage fakeFloor;

float zoom = 1;
float rotX = PI;
float rotY = 0;

void setup(){
  size(700,550,P3D);
  
  // Read the scans
  for(int i = 0; i < scans.length; i++){
    ScanContainer s = new ScanContainer(files[i]);
    s = s.reduceResolution(resolution);
    s.fillHoles();
    s = s.gaussianSmooth(5);
    scans[i] = s;
  }
 
  // Start with the first scan as default 
  currentScan = 0;
  scan = new ScanContainer(scans[currentScan]);
  backSide = scan.offsetScan(offsetDist);
  
  // Create an image to represent the floor
  fakeFloor = createFloor(color(200));

  // Initialize ControlP5
  controlPanel();
}

void draw(){
  background(200);
  
  // Draw the floor image in an inclined plane
  noStroke();
  beginShape();
    texture(fakeFloor);
    vertex(-width,0.75*height,-300,0,0);
    vertex(2*width,0.75*height,-300,width,0);
    vertex(width,height,0,width,height);
    vertex(0,height,0,0,height);
  endShape();

  // Draw the control panel
  cp5.draw();

  // Position the scene
  translate(width/2,height/2,0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoom);
  
  // Draw the scan
  if(asPixels){
    scan.drawAsPixels(pixelSize);
  }
  else if(asLines){
    scan.drawAsLines();
  }
  else if(psychedelic){
    scan.drawAsPsyTriangles();
    lights();
    backSide.drawAsTriangles();
  }
  else{
    scan.drawAsTriangles();
    lights();
    backSide.drawAsTriangles();
  }
}

/*
 * ControlP5 selection panel
 */

void controlPanel(){
  cp5 = new ControlP5(this);
  cp5.addButton("scan0",0, 40,40,20,20).setCaptionLabel("");
  cp5.addButton("scan1",0, 70,40,20,20).setCaptionLabel("");
  cp5.addButton("scan2",0,100,40,20,20).setCaptionLabel("");
  cp5.addButton("scan3",0,130,40,20,20).setCaptionLabel("");
  cp5.addButton("scan4",0,160,40,20,20).setCaptionLabel("");
  cp5.addButton("scan5",0,190,40,20,20).setCaptionLabel("");
  cp5.setAutoDraw(false);
}

void controlEvent(ControlEvent event){
  if(event.isFrom("scan0")){
    currentScan = 0;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(offsetDist);
    resolution = 1;
  }
  if(event.isFrom("scan1")){
    currentScan = 1;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(offsetDist);
    resolution = 1;
  }
  if(event.isFrom("scan2")){
    currentScan = 2;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(offsetDist);
    resolution = 1;
  }
  if(event.isFrom("scan3")){
    currentScan = 3;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(offsetDist);
    resolution = 1;
  }
  if(event.isFrom("scan4")){
    currentScan = 4;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(-offsetDist);
    resolution = 1;
  }
  if(event.isFrom("scan5")){
    currentScan = 5;
    scan = new ScanContainer(scans[currentScan]);
    backSide = scan.offsetScan(offsetDist);
    resolution = 1;
  }
}

/*
 * ScanContainer class.
 */

class ScanContainer{
  int xSize;          // Hozizontal dimension
  int ySize;          // Vertical dimension
  int nPoints;        // Number of points
  PVector[] map3D;    // 3D points
  PImage rgbImg;      // Point colors 
  boolean[] consImg;  // Valid points 
  int[] ini;          // Start of the valid points (in the line)
  int[] end;          // End of the valid points (in the line)
  boolean[] empty;    // No valid points in the line
  
  
  //
  // Constructor
  //

  ScanContainer(String tempFile){
    String[] pointsAndColors = loadStrings(tempFile);
    String[] header = split(pointsAndColors[0]," ");

    xSize = int(header[0]);
    ySize = int(header[1]);
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints]; 
    ini = new int[ySize];
    end = new int[ySize];
    empty = new boolean[ySize];
  
    // Populate the arrays
    rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      String[] row = split(pointsAndColors[i+1]," ");
      map3D[i] = new PVector(float(row[0]),float(row[1]),float(row[2]));
      if(float(row[3]) < 0){
        rgbImg.pixels[i] = color(0);
        consImg[i] = false;
      }
      else{
        rgbImg.pixels[i] = color(float(row[3]),float(row[4]),float(row[5]));
        consImg[i] = true;
      }
    }
    rgbImg.updatePixels();

    // Find the extremes
    for(int y = 0; y < ySize; y++){
      ini[y] = -1;
      end[y] = -1;
      empty[y] = true;
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(consImg[index]){
          if(ini[y] < 0){
            ini[y] = x;
          }
          end[y] = x;
          empty[y] = false;
        }
      }
    }
  }
  
  
  ScanContainer(PVector[] tempMap3D, PImage tempRGBimg, boolean[] tempConsImg){
    xSize = tempRGBimg.width;
    ySize = tempRGBimg.height;
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints]; 
    ini = new int[ySize];
    end = new int[ySize];
    empty = new boolean[ySize];
  
    // Populate the arrays
    rgbImg.loadPixels();
    tempRGBimg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3D[i] = tempMap3D[i].get();
      rgbImg.pixels[i] = tempRGBimg.pixels[i];
      consImg[i] = tempConsImg[i];
    }
    rgbImg.updatePixels();

    // Find the extremes
    for(int y = 0; y < ySize; y++){
      ini[y] = -1;
      end[y] = -1;
      empty[y] = true;
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(consImg[index]){
          if(ini[y] < 0){
            ini[y] = x;
          }
          end[y] = x;
          empty[y] = false;
        }
      }
    }
  }  
  

  ScanContainer(ScanContainer tempScan){
    this(tempScan.map3D,tempScan.rgbImg,tempScan.consImg);
  }
  
  
  //
  // Class Methods 
  //

  // fillHoles
  void fillHoles(){
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        int start = 0;
        int finish = 0;
        for(int x = ini[y]+1; x < end[y]; x++){
          int index = x + y*xSize;
          if(!consImg[index]){
            // Calculate the limits of the hole
            if(index > finish){
              start = index-1;
              for(int i = x+1; i <= end[y]; i++){
                int iterIndex = i + y*xSize;
                if(consImg[iterIndex]){
                  finish = iterIndex;
                  break;
                }
              }
            }
            // Fill the hole if the gap is not too big
            if(finish-start < 4){ 
              PVector deltaPos = PVector.sub(map3D[finish],map3D[start]);
              float deltaR = (red(rgbImg.pixels[finish])-red(rgbImg.pixels[start]));
              float deltaG = (green(rgbImg.pixels[finish])-green(rgbImg.pixels[start]));
              float deltaB = (blue(rgbImg.pixels[finish])-blue(rgbImg.pixels[start]));

              float step = float(index-start)/float(finish-start);
              deltaPos.mult(step);
              deltaR = deltaR*step;
              deltaG = deltaG*step;
              deltaB = deltaB*step;
              
              map3D[index] = PVector.add(map3D[start],deltaPos);
              rgbImg.pixels[index] = color(red(rgbImg.pixels[start])+deltaR,green(rgbImg.pixels[start])+deltaG,blue(rgbImg.pixels[start])+deltaB);
              consImg[index] = true;
            }
          } 
        }    
      }
    }
    rgbImg.updatePixels();
  }


  // gaussianSmooth
  ScanContainer gaussianSmooth(int n){
    if(n > 0){
      // Create the gaussian mask
      float[][] m = new float[2*n+1][2*n+1];
      for(int i = -n; i <= n; i++){
        for(int j = -n; j <= n; j++){
          float distSq = sq(i)+sq(j);
          if(distSq <= sq(n)){
            m[i+n][j+n] = pow(2.718,-distSq/(2*sq(n/2)));
          }
          else{
            m[i+n][j+n] = 0;
          }
        }
      }

      // Create the array for the smoothed 3Dpoints
      PVector[] smMap3D = new PVector[nPoints];

      // Populate the array
      for(int y = 0; y < ySize; y++){
        for(int x = 0; x < xSize; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            // Average between nearby pixels
            PVector p = new PVector(0,0,0);
            float multCounter = 0;
            for(int i = -n; i <= n; i++){
              for(int j = -n; j <= n; j++){
                int xNearby = x+i;
                int yNearby = y+j;
                if(xNearby >=0 && xNearby < xSize && yNearby >= 0 && yNearby < ySize){
                  int indexNearby = xNearby + yNearby*xSize;
                  if(consImg[indexNearby]){
                    p.add(PVector.mult(map3D[indexNearby],m[i+n][j+n]));
                    multCounter += m[i+n][j+n];
                  }
                }
              }
            }
            if(multCounter > 0){
              p.div(multCounter);
              smMap3D[index] = p.get();
            }
            else{
              smMap3D[index] = map3D[index].get();
            }
          }
          else{
            smMap3D[index] = map3D[index].get();
          }
        }
      }
      return new ScanContainer(smMap3D,rgbImg,consImg);
    }
    else{
      return this;
    }
  }


  // reduceResolution
  ScanContainer reduceResolution(int n){
    if(n > 1){
      // Dimensions of the reduced scan
      int xSizeRed = xSize/n;
      int ySizeRed = ySize/n;
      int nPointsRed = xSizeRed*ySizeRed;
      PVector[] redMap3D = new PVector[nPointsRed];
      PImage redRGBimg = createImage(xSizeRed,ySizeRed,RGB);
      boolean[] redConsImg = new boolean[nPointsRed]; 

      // Populate the arrays
      redRGBimg.loadPixels();
      rgbImg.loadPixels();
      for(int y = 0; y < ySizeRed; y++){
        for(int x = 0; x < xSizeRed; x++){
          int index = x*n + y*n*xSize;
          int indexRed = x + y*xSizeRed;
          
          // Average between nearby pixels
          PVector p = new PVector(0,0,0);
          float r = 0;
          float g = 0;
          float b = 0;
          float pointsCounter = 0;
          for(int i = -n/2; i <= n/2; i++){
            for(int j = -n/2; j <= n/2; j++){
              int xNearby = (x*n)+i;
              int yNearby = (y*n)+j;
              if(xNearby >=0 && xNearby < xSize && yNearby >= 0 && yNearby < ySize){
                int indexNearby = xNearby + yNearby*xSize;
                if(consImg[indexNearby]){
                  p.add(map3D[indexNearby]);
                  r += red(rgbImg.pixels[indexNearby]);
                  g += green(rgbImg.pixels[indexNearby]);
                  b += blue(rgbImg.pixels[indexNearby]);
                  pointsCounter++;
                }
              }
            }
          }
          if(pointsCounter > 0){
            p.div(pointsCounter);
            r = r/pointsCounter;
            g = g/pointsCounter;
            b = b/pointsCounter;

            redMap3D[indexRed] = p.get();
            redRGBimg.pixels[indexRed] = color(r,g,b);
            redConsImg[indexRed] = true;
          }
          else{
            redMap3D[indexRed] = map3D[index].get();
            redRGBimg.pixels[indexRed] = rgbImg.pixels[index];
            redConsImg[indexRed] = consImg[index];
          }
        }
      }
      redRGBimg.updatePixels();
    
      return new ScanContainer(redMap3D,redRGBimg,redConsImg);
    }
    else{
      return this;
    }
  }

  
  // offsetScan
  ScanContainer offsetScan(float offset){
    // Create the arrays for the offset scan
    PVector[] offMap3D = new PVector[nPoints];
    PImage offRGBimg = createImage(xSize,ySize,RGB);
    boolean[] offConsImg = new boolean[nPoints];

    // Populate the arrays
    offRGBimg.loadPixels();
    for(int y = 0; y < ySize; y++){
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(x-1 >= 0 && y-1 >= 0 && x+1 < xSize && y+1 < ySize && 
           consImg[index] && consImg[index+1] && consImg[index-1] && consImg[index+xSize] && consImg[index-xSize]){
          PVector v1 = PVector.sub(map3D[index+1],map3D[index]);
          PVector v2 = PVector.sub(map3D[index+xSize],map3D[index]);
          PVector v3 = PVector.sub(map3D[index-1],map3D[index]);
          PVector v4 = PVector.sub(map3D[index-xSize],map3D[index]);
          PVector perp1 = v2.cross(v1);
          PVector perp2 = v3.cross(v2);
          PVector perp3 = v4.cross(v3);
          PVector perp4 = v1.cross(v4);
          perp1.normalize();
          perp2.normalize();
          perp3.normalize();
          perp4.normalize();
          PVector perp = PVector.add(perp1,perp2);
          perp.add(perp3);
          perp.add(perp4);
          perp.mult(offset/4);
          offMap3D[index] = PVector.add(map3D[index],perp);
          offRGBimg.pixels[index] = 255;
          offConsImg[index] = true;
        }
        else{
          offMap3D[index] = map3D[index].get();
          offRGBimg.pixels[index] = 255;
          offConsImg[index] = consImg[index];
        }
      }
    }
    offRGBimg.updatePixels();
    
    return new ScanContainer(offMap3D,offRGBimg,offConsImg);
  }


  // drawAsPixels
  void drawAsPixels(int pSize){
    strokeWeight(pSize);
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            stroke(rgbImg.pixels[index]);
            point(map3D[index].x,map3D[index].y,map3D[index].z);
          }
        }
      }  
    }
    noStroke();
  }
  
  
  // drawAsTriangles
  void drawAsTriangles(){
    float maxSep = 90;  // Maximum separation allowed between two consecutive points

    noStroke();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      if(!empty[y] && !empty[y+1]){
        int xStart = min(ini[y],ini[y+1]);
        int xEnd = max(end[y],end[y+1]);
        for(int x = xStart; x < xEnd; x++){
          int index = x + y*xSize;
          // First triangle
          if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(rgbImg.pixels[index]);
                vertex(p1.x,p1.y,p1.z);
                fill(rgbImg.pixels[index+1]);
                vertex(p2.x,p2.y,p2.z);
                fill(rgbImg.pixels[index+xSize]);
                vertex(p3.x,p3.y,p3.z);
              endShape(); 
            }
          }
          else if(consImg[index] && consImg[index+1+xSize] && consImg[index+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1+xSize].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(rgbImg.pixels[index]);
                vertex(p1.x,p1.y,p1.z);
                fill(rgbImg.pixels[index+1+xSize]);
                vertex(p2.x,p2.y,p2.z);
                fill(rgbImg.pixels[index+xSize]);
                vertex(p3.x,p3.y,p3.z);
              endShape(); 
            }
          }
          // Second triangle
          if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
            PVector p1 = map3D[index+1].get();
            PVector p2 = map3D[index+1+xSize].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(rgbImg.pixels[index+1]);
                vertex(p1.x,p1.y,p1.z);
                fill(rgbImg.pixels[index+1+xSize]);
                vertex(p2.x,p2.y,p2.z);
                fill(rgbImg.pixels[index+xSize]);
                vertex(p3.x,p3.y,p3.z);
              endShape();
            } 
          }
          else if(consImg[index] && consImg[index+1] && consImg[index+1+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1].get();
            PVector p3 = map3D[index+1+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(rgbImg.pixels[index]);
                vertex(p1.x,p1.y,p1.z);
                fill(rgbImg.pixels[index+1]);
                vertex(p2.x,p2.y,p2.z);
                fill(rgbImg.pixels[index+1+xSize]);
                vertex(p3.x,p3.y,p3.z);
              endShape();
            } 
          }
        }
      }
    }
  }


  // drawAsPsyTriangles
  void drawAsPsyTriangles(){
    float maxSep = 90;  // Maximum separation allowed between two consecutive points

    noStroke();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      if(!empty[y] && !empty[y+1]){
        int xStart = min(ini[y],ini[y+1]);
        int xEnd = max(end[y],end[y+1]);
        for(int x = xStart; x < xEnd; x++){
          int index = x + y*xSize;
          // First triangle
          if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(blendColor(rgbImg.pixels[index],color(random(-150,150),random(-150,150),0),ADD));
                vertex(p1.x,p1.y,p1.z);
                vertex(p2.x,p2.y,p2.z);
                vertex(p3.x,p3.y,p3.z);
              endShape(); 
            }
          }
          else if(consImg[index] && consImg[index+1+xSize] && consImg[index+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1+xSize].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(blendColor(rgbImg.pixels[index],color(random(-150,150),random(-150,150),0),ADD));
                vertex(p1.x,p1.y,p1.z);
                vertex(p2.x,p2.y,p2.z);
                vertex(p3.x,p3.y,p3.z);
              endShape(); 
            }
          }
          // Second triangle
          if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
            PVector p1 = map3D[index+1].get();
            PVector p2 = map3D[index+1+xSize].get();
            PVector p3 = map3D[index+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(blendColor(rgbImg.pixels[index+1],color(random(-150,150),random(-150,150),0),ADD));
                vertex(p1.x,p1.y,p1.z);
                vertex(p2.x,p2.y,p2.z);
                vertex(p3.x,p3.y,p3.z);
              endShape(); 
            }
          }
          else if(consImg[index] && consImg[index+1] && consImg[index+1+xSize]){
            PVector p1 = map3D[index].get();
            PVector p2 = map3D[index+1].get();
            PVector p3 = map3D[index+1+xSize].get();
            float dist1 = p1.dist(p2);
            float dist2 = p1.dist(p3);
            float dist3 = p2.dist(p3);
            boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                           (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
            if(cond){
              beginShape(TRIANGLES);
                fill(blendColor(rgbImg.pixels[index+1],color(random(-150,150),random(-150,150),0),ADD));
                vertex(p1.x,p1.y,p1.z);
                vertex(p2.x,p2.y,p2.z);
                vertex(p3.x,p3.y,p3.z);
              endShape();
            } 
          }
        }
      }
    }
  }
  
  
  // drawAsLines
  void drawAsLines(){
    float maxSep = 90;  // Maximum separation allowed between two consecutive points

    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(x+1 < xSize && consImg[index] && consImg[index+1] && map3D[index].dist(map3D[index+1]) < maxSep){
            stroke(rgbImg.pixels[index]);
            line(map3D[index].x,map3D[index].y,map3D[index].z,map3D[index+1].x,map3D[index+1].y,map3D[index+1].z);
          }
          if(y+1 < ySize && consImg[index] && consImg[index+xSize] && map3D[index].dist(map3D[index+xSize]) < maxSep){
            stroke(rgbImg.pixels[index]);
            line(map3D[index].x,map3D[index].y,map3D[index].z,map3D[index+xSize].x,map3D[index+xSize].y,map3D[index+xSize].z);
          }
          if(x+1 < xSize && y+1 < ySize && consImg[index] && consImg[index+1+xSize] && map3D[index].dist(map3D[index+1+xSize]) < maxSep){
            stroke(rgbImg.pixels[index]);
            line(map3D[index].x,map3D[index].y,map3D[index].z,map3D[index+1+xSize].x,map3D[index+1+xSize].y,map3D[index+1+xSize].z);
          }
        }
      }
    }
    noStroke();
  }

}