Concave Hull in Processing

concave_hull.pde

/**
 * concave_hull.pde -- THIS VERSION IS FOR PROCESSING 4
 * by Udo Schlegel - Udo.3.Schlegel(at)uni-konstanz.de
 * Ported to Processing v4 by Golan Levin and Aren Davey
 * This is an implementation of the algorithm described by Adriano Moreira and Maribel Yasmina Santos:
 * CONCAVE HULL: A K-NEAREST NEIGHBOURS APPROACH FOR THE COMPUTATION OF THE REGION OCCUPIED BY A SET OF POINTS.
 * GRAPP 2007 - International Conference on Computer Graphics Theory and Applications; pp 61-68.
 * https://repositorium.sdum.uminho.pt/bitstream/1822/6429/1/ConcaveHull_ACM_MYS.pdf
 * With help from https://github.com/detlevn/QGIS-ConcaveHull-Plugin/blob/master/concavehull.py
 */

import java.util.*;
ArrayList<Point> myPointArrayList;
ArrayList<Point> myConcaveHull;

//------------------------------------------
void setup() {
  size(400, 400);
  randomSeed(5);

  myPointArrayList = new ArrayList<Point>();

  int nPoints = 500;
  float cx = width/2;
  float cy = height/2;
  float radius = width/4;
  for (int i=0; i<nPoints; i++) {
    float px = width/2 + 40 * randomGaussian();
    float py = height/2 + 40 * randomGaussian();
    Point newP = new Point(px, py);
    myPointArrayList.add(newP);
  }

  int myK = 3;
  int then = millis();
  myConcaveHull = calculateConcaveHull (myPointArrayList, myK);
  int now = millis();
  println("Took: " + (now-then) );
}



void draw() {

  background(255);
  strokeWeight(1);
  for (int i=0; i<myPointArrayList.size(); i++) {
    Point aPoint = myPointArrayList.get(i);
    ellipse(aPoint.x, aPoint.y, 4, 4);
  }

  noFill();
  strokeWeight(2);
  beginShape();
  for (int i=0; i<myConcaveHull.size(); i++) {
    Point aPoint = myConcaveHull.get(i);
    vertex(aPoint.x, aPoint.y);
  }
  endShape(CLOSE);
}


//------------------------------------------
class Pair {
  Float f;
  Point p;
  Pair (Float inf, Point inp) {
    f = inf;
    p = inp;
  }
  Float getKey() {
    return f;
  }
  Point getValue() {
    return p;
  }
}

//------------------------------------------
class Point {

  Float x;
  Float y;

  Point(Float x, Float y) {
    this.x = x;
    this.y = y;
  }

  Float getX() {
    return x;
  }

  Float getY() {
    return y;
  }

  String toString() {
    return "(" + x + " " + y + ")";
  }

  boolean equals(Point obj) {
    if ((abs(x - obj.x) < EPSILON) && (abs(y - obj.y) < EPSILON)) {
      return true;
    } else {
      return false;
    }
  }


  int hashCode() {
    // http://stackoverflow.com/questions/22826326/good-hashcode-function-for-2d-coordinates
    // http://www.cs.upc.edu/~alvarez/calculabilitat/enumerabilitat.pdf
    int tmp = (int) (y + ((x + 1) / 2));
    return abs((int) (x + (tmp * tmp)));
  }
}


//------------------------------------------
Float euclideanDistance(Point a, Point b) {
  return sqrt(pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2));
}

ArrayList<Point> kNearestNeighbors(ArrayList<Point> l, Point q, int k) {
  ArrayList<Pair> nearestList = new ArrayList<>();
  for (Point o : l) {
    nearestList.add(new Pair(euclideanDistance(q, o), o));
  }

  Collections.sort(nearestList, new Comparator<Pair>() {
    public int compare(Pair o1, Pair o2) {
      return o1.getKey().compareTo(o2.getKey());
    }
  }
  );

  ArrayList<Point> result = new ArrayList<>();

  for (int i = 0; i < Math.min(k, nearestList.size()); i++) {
    result.add(nearestList.get(i).getValue());
  }

  return result;
}

Point findMinYPoint(ArrayList<Point> l) {
  Collections.sort(l, new Comparator<Point>() {
    int compare(Point o1, Point o2) {
      return o1.getY().compareTo(o2.getY());
    }
  }
  );
  return l.get(0);
}

Float calculateAngle(Point o1, Point o2) {
  return atan2(o2.getY() - o1.getY(), o2.getX() - o1.getX());
}

Float angleDifference(Float a1, Float a2) {
  // calculate angle difference in clockwise directions as radians
  if ((a1 > 0 && a2 >= 0) && a1 > a2) {
    return abs(a1 - a2);
  } else if ((a1 >= 0 && a2 > 0) && a1 < a2) {
    return 2 * PI + a1 - a2;
  } else if ((a1 < 0 && a2 <= 0) && a1 < a2) {
    return 2 * PI + a1 + abs(a2);
  } else if ((a1 <= 0 && a2 < 0) && a1 > a2) {
    return abs(a1 - a2);
  } else if (a1 <= 0 && 0 < a2) {
    return 2 * PI + a1 - a2;
  } else if (a1 >= 0 && 0 >= a2) {
    return a1 + abs(a2);
  } else {
    return 0.0;
  }
}

ArrayList<Point> sortByAngle(ArrayList<Point> l, Point q, Float a) {
  // Sort by angle descending
  Collections.sort(l, new Comparator<Point>() {
    int compare(Point o1, Point o2) {
      Float a1 = angleDifference(a, calculateAngle(q, o1));
      Float a2 = angleDifference(a, calculateAngle(q, o2));
      return a2.compareTo(a1);
    }
  }
  );
  return l;
}


boolean intersect(Point l1p1, Point l1p2, Point l2p1, Point l2p2) {
  // calculate part equations for line-line intersection
  Float a1 = l1p2.getY() - l1p1.getY();
  Float b1 = l1p1.getX() - l1p2.getX();
  Float c1 = a1 * l1p1.getX() + b1 * l1p1.getY();
  Float a2 = l2p2.getY() - l2p1.getY();
  Float b2 = l2p1.getX() - l2p2.getX();
  Float c2 = a2 * l2p1.getX() + b2 * l2p1.getY();
  // calculate the divisor
  Float tmp = (a1 * b2 - a2 * b1);

  // calculate intersection point x coordinate
  Float pX = (c1 * b2 - c2 * b1) / tmp;

  // check if intersection x coordinate lies in line line segment
  if ((pX > l1p1.getX() && pX > l1p2.getX()) || (pX > l2p1.getX() && pX > l2p2.getX())
    || (pX < l1p1.getX() && pX < l1p2.getX()) || (pX < l2p1.getX() && pX < l2p2.getX())) {
    return false;
  }

  // calculate intersection point y coordinate
  Float pY = (a1 * c2 - a2 * c1) / tmp;

  // check if intersection y coordinate lies in line line segment
  if ((pY > l1p1.getY() && pY > l1p2.getY()) || (pY > l2p1.getY() && pY > l2p2.getY())
    || (pY < l1p1.getY() && pY < l1p2.getY()) || (pY < l2p1.getY() && pY < l2p2.getY())) {
    return false;
  }
  return true;
}

boolean pointInPolygon(Point p, ArrayList<Point> pp) {
  boolean result = false;
  for (int i = 0, j = pp.size() - 1; i < pp.size(); j = i++) {
    if ((pp.get(i).getY() > p.getY()) != (pp.get(j).getY() > p.getY()) &&
      (p.getX() < (pp.get(j).getX() - pp.get(i).getX()) * (p.getY() - pp.get(i).getY()) / (pp.get(j).getY() - pp.get(i).getY()) + pp.get(i).getX())) {
      result = !result;
    }
  }
  return result;
}


ArrayList<Point> calculateConcaveHull(ArrayList<Point> pointArrayList, int k) {

  // the resulting concave hull
  ArrayList<Point> concaveHull = new ArrayList<>();

  // optional remove duplicates
  HashSet<Point> set = new HashSet<>(pointArrayList);
  ArrayList<Point> pointArraySet = new ArrayList<>(set);

  // k has to be greater than 3 to execute the algorithm
  int kk = max(k, 3);

  // return Points if already Concave Hull
  if (pointArraySet.size() < 3) {
    return pointArraySet;
  }

  // make sure that k neighbors can be found
  kk = min(kk, pointArraySet.size() - 1);

  // find first point and remove from point list
  Point firstPoint = findMinYPoint(pointArraySet);
  concaveHull.add(firstPoint);
  Point currentPoint = firstPoint;
  pointArraySet.remove(firstPoint);

  Float previousAngle = 0.0;
  int step = 2;

  while ((currentPoint != firstPoint || step == 2) && pointArraySet.size() > 0) {

    // after 3 steps add first point to dataset, otherwise hull cannot be closed
    if (step == 5) {
      pointArraySet.add(firstPoint);
    }

    // get k nearest neighbors of current point
    ArrayList<Point> kNearestPoints = kNearestNeighbors(pointArraySet, currentPoint, kk);

    // sort points by angle clockwise
    ArrayList<Point> clockwisePoints = sortByAngle(kNearestPoints, currentPoint, previousAngle);

    // check if clockwise angle nearest neighbors are candidates for concave hull
    boolean its = true;
    int i = -1;
    while (its && i < clockwisePoints.size() - 1) {
      i++;

      int lastPoint = 0;
      if (clockwisePoints.get(i) == firstPoint) {
        lastPoint = 1;
      }

      // check if possible new concave hull point intersects with others
      int j = 2;
      its = false;
      while (!its && j < concaveHull.size() - lastPoint) {
        its = intersect(concaveHull.get(step - 2), clockwisePoints.get(i), concaveHull.get(step - 2 - j), concaveHull.get(step - 1 - j));
        j++;
      }
    }

    // if there is no candidate increase k - try again
    if (its) {
      return calculateConcaveHull(pointArrayList, k + 1);
    }

    // add candidate to concave hull and remove from dataset
    currentPoint = clockwisePoints.get(i);
    concaveHull.add(currentPoint);
    pointArraySet.remove(currentPoint);

    // calculate last angle of the concave hull line
    previousAngle = calculateAngle(concaveHull.get(step - 1), concaveHull.get(step - 2));

    step++;
  }

  // Check if all points are contained in the concave hull
  boolean insideCheck = true;
  int i = pointArraySet.size() - 1;

  while (insideCheck && i > 0) {
    insideCheck = pointInPolygon(pointArraySet.get(i), concaveHull);
    i--;
  }

  // if not all points inside -  try again
  if (!insideCheck) {
    return calculateConcaveHull(pointArrayList, k + 1);
  } else {
    return concaveHull;
  }
}

/**
 * concave_hull.pde — THIS VERSION IS FOR PROCESSING 3.5.4
 * by Udo Schlegel - Udo.3.Schlegel(at)uni-konstanz.de
 * Ported to Processing v4 by Golan Levin and Aren Davey
 * This is an implementation of the algorithm described by Adriano Moreira and Maribel Yasmina Santos:
 * CONCAVE HULL: A K-NEAREST NEIGHBOURS APPROACH FOR THE COMPUTATION OF THE REGION OCCUPIED BY A SET OF POINTS.
 * GRAPP 2007 - International Conference on Computer Graphics Theory and Applications; pp 61-68.
 * https://repositorium.sdum.uminho.pt/bitstream/1822/6429/1/ConcaveHull_ACM_MYS.pdf
 * With help from https://github.com/detlevn/QGIS-ConcaveHull-Plugin/blob/master/concavehull.py
 */

import java.util.*;
ArrayList<Point> myPointArrayList;
ArrayList<Point> myConcaveHull;

//------------------------------------------
void setup() {
  size(400, 400);
  randomSeed(5);

  myPointArrayList = new ArrayList<Point>();

  int nPoints = 500;
  float cx = width/2;
  float cy = height/2;
  float radius = width/4;
  for (int i=0; i<nPoints; i++) {
    float px = width/2 + 40 * randomGaussian();
    float py = height/2 + 40 * randomGaussian();
    Point newP = new Point(px, py);
    myPointArrayList.add(newP);
  }

  int myK = 3;
  int then = millis();
  myConcaveHull = calculateConcaveHull (myPointArrayList, myK);
  int now = millis();
  println("Took: " + (now-then) );
}



void draw() {

  background(255);
  strokeWeight(1);
  for (int i=0; i<myPointArrayList.size(); i++) {
    Point aPoint = myPointArrayList.get(i);
    ellipse(aPoint.x, aPoint.y, 4, 4);
  }

  noFill();
  strokeWeight(2);
  beginShape();
  for (int i=0; i<myConcaveHull.size(); i++) {
    Point aPoint = myConcaveHull.get(i);
    vertex(aPoint.x, aPoint.y);
  }
  endShape(CLOSE);
}


//------------------------------------------
class Pair {
  Float f;
  Point p;
  Pair (Float inf, Point inp) {
    f = inf;
    p = inp;
  }
  Float getKey() {
    return f;
  }
  Point getValue() {
    return p;
  }
}

//------------------------------------------
class Point {

  Float x;
  Float y;

  Point(Float x, Float y) {
    this.x = x;
    this.y = y;
  }

  Float getX() {
    return x;
  }

  Float getY() {
    return y;
  }

  String toString() {
    return "(" + x + " " + y + ")";
  }

  boolean equals(Point obj) {
    if ((abs(x - obj.x) < EPSILON) && (abs(y - obj.y) < EPSILON)) {
      return true;
    } else {
      return false;
    }
  }


  int hashCode() {
    // http://stackoverflow.com/questions/22826326/good-hashcode-function-for-2d-coordinates
    // http://www.cs.upc.edu/~alvarez/calculabilitat/enumerabilitat.pdf
    int tmp = (int) (y + ((x + 1) / 2));
    return abs((int) (x + (tmp * tmp)));
  }
}


//------------------------------------------
Float euclideanDistance(Point a, Point b) {
  return sqrt(pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2));
}

ArrayList<Point> kNearestNeighbors(ArrayList<Point> l, Point q, int k) {
  ArrayList<Pair> nearestList = new ArrayList<Pair>();
  for (Point o : l) {
    nearestList.add(new Pair(euclideanDistance(q, o), o));
  }

  Collections.sort(nearestList, new Comparator<Pair>() {
    public int compare(Pair o1, Pair o2) {
      return o1.getKey().compareTo(o2.getKey());
    }
  }
  );

  ArrayList<Point> result = new ArrayList<Point>();

  for (int i = 0; i < Math.min(k, nearestList.size()); i++) {
    result.add(nearestList.get(i).getValue());
  }

  return result;
}

Point findMinYPoint(ArrayList<Point> l) {
  Collections.sort(l, new Comparator<Point>() {
    public int compare(Point o1, Point o2) {
      return o1.getY().compareTo(o2.getY());
    }
  }
  );
  return l.get(0);
}

Float calculateAngle(Point o1, Point o2) {
  return atan2(o2.getY() - o1.getY(), o2.getX() - o1.getX());
}

Float angleDifference(Float a1, Float a2) {
  // calculate angle difference in clockwise directions as radians
  if ((a1 > 0 && a2 >= 0) && a1 > a2) {
    return abs(a1 - a2);
  } else if ((a1 >= 0 && a2 > 0) && a1 < a2) {
    return 2 * PI + a1 - a2;
  } else if ((a1 < 0 && a2 <= 0) && a1 < a2) {
    return 2 * PI + a1 + abs(a2);
  } else if ((a1 <= 0 && a2 < 0) && a1 > a2) {
    return abs(a1 - a2);
  } else if (a1 <= 0 && 0 < a2) {
    return 2 * PI + a1 - a2;
  } else if (a1 >= 0 && 0 >= a2) {
    return a1 + abs(a2);
  } else {
    return 0.0;
  }
}

ArrayList<Point> sortByAngle(ArrayList<Point> l, Point q, Float a) {
  // Sort by angle descending
  final Float aa = a;
  final Point qq = q;
  Collections.sort(l, new Comparator<Point>() {
    public int compare(Point o1, Point o2) {
      Float a1 = angleDifference(aa, calculateAngle(qq, o1));
      Float a2 = angleDifference(aa, calculateAngle(qq, o2));
      return a2.compareTo(a1);
    }
  }
  );
  return l;
}


boolean intersect(Point l1p1, Point l1p2, Point l2p1, Point l2p2) {
  // calculate part equations for line-line intersection
  Float a1 = l1p2.getY() - l1p1.getY();
  Float b1 = l1p1.getX() - l1p2.getX();
  Float c1 = a1 * l1p1.getX() + b1 * l1p1.getY();
  Float a2 = l2p2.getY() - l2p1.getY();
  Float b2 = l2p1.getX() - l2p2.getX();
  Float c2 = a2 * l2p1.getX() + b2 * l2p1.getY();
  // calculate the divisor
  Float tmp = (a1 * b2 - a2 * b1);

  // calculate intersection point x coordinate
  Float pX = (c1 * b2 - c2 * b1) / tmp;

  // check if intersection x coordinate lies in line line segment
  if ((pX > l1p1.getX() && pX > l1p2.getX()) || (pX > l2p1.getX() && pX > l2p2.getX())
    || (pX < l1p1.getX() && pX < l1p2.getX()) || (pX < l2p1.getX() && pX < l2p2.getX())) {
    return false;
  }

  // calculate intersection point y coordinate
  Float pY = (a1 * c2 - a2 * c1) / tmp;

  // check if intersection y coordinate lies in line line segment
  if ((pY > l1p1.getY() && pY > l1p2.getY()) || (pY > l2p1.getY() && pY > l2p2.getY())
    || (pY < l1p1.getY() && pY < l1p2.getY()) || (pY < l2p1.getY() && pY < l2p2.getY())) {
    return false;
  }
  return true;
}

boolean pointInPolygon(Point p, ArrayList<Point> pp) {
  boolean result = false;
  for (int i = 0, j = pp.size() - 1; i < pp.size(); j = i++) {
    if ((pp.get(i).getY() > p.getY()) != (pp.get(j).getY() > p.getY()) &&
      (p.getX() < (pp.get(j).getX() - pp.get(i).getX()) * (p.getY() - pp.get(i).getY()) / (pp.get(j).getY() - pp.get(i).getY()) + pp.get(i).getX())) {
      result = !result;
    }
  }
  return result;
}


ArrayList<Point> calculateConcaveHull(ArrayList<Point> pointArrayList, int k) {

  // the resulting concave hull
  ArrayList<Point> concaveHull = new ArrayList<Point>();

  // optional remove duplicates
  HashSet<Point> set = new HashSet<Point>(pointArrayList);
  ArrayList<Point> pointArraySet = new ArrayList<Point>(set);

  // k has to be greater than 3 to execute the algorithm
  int kk = max(k, 3);

  // return Points if already Concave Hull
  if (pointArraySet.size() < 3) {
    return pointArraySet;
  }

  // make sure that k neighbors can be found
  kk = min(kk, pointArraySet.size() - 1);

  // find first point and remove from point list
  Point firstPoint = findMinYPoint(pointArraySet);
  concaveHull.add(firstPoint);
  Point currentPoint = firstPoint;
  pointArraySet.remove(firstPoint);

  Float previousAngle = 0.0;
  int step = 2;

  while ((currentPoint != firstPoint || step == 2) && pointArraySet.size() > 0) {

    // after 3 steps add first point to dataset, otherwise hull cannot be closed
    if (step == 5) {
      pointArraySet.add(firstPoint);
    }

    // get k nearest neighbors of current point
    ArrayList<Point> kNearestPoints = kNearestNeighbors(pointArraySet, currentPoint, kk);

    // sort points by angle clockwise
    ArrayList<Point> clockwisePoints = sortByAngle(kNearestPoints, currentPoint, previousAngle);

    // check if clockwise angle nearest neighbors are candidates for concave hull
    boolean its = true;
    int i = -1;
    while (its && i < clockwisePoints.size() - 1) {
      i++;

      int lastPoint = 0;
      if (clockwisePoints.get(i) == firstPoint) {
        lastPoint = 1;
      }

      // check if possible new concave hull point intersects with others
      int j = 2;
      its = false;
      while (!its && j < concaveHull.size() - lastPoint) {
        its = intersect(concaveHull.get(step - 2), clockwisePoints.get(i), concaveHull.get(step - 2 - j), concaveHull.get(step - 1 - j));
        j++;
      }
    }

    // if there is no candidate increase k - try again
    if (its) {
      return calculateConcaveHull(pointArrayList, k + 1);
    }

    // add candidate to concave hull and remove from dataset
    currentPoint = clockwisePoints.get(i);
    concaveHull.add(currentPoint);
    pointArraySet.remove(currentPoint);

    // calculate last angle of the concave hull line
    previousAngle = calculateAngle(concaveHull.get(step - 1), concaveHull.get(step - 2));

    step++;
  }

  // Check if all points are contained in the concave hull
  boolean insideCheck = true;
  int i = pointArraySet.size() - 1;

  while (insideCheck && i > 0) {
    insideCheck = pointInPolygon(pointArraySet.get(i), concaveHull);
    i--;
  }

  // if not all points inside -  try again
  if (!insideCheck) {
    return calculateConcaveHull(pointArrayList, k + 1);
  } else {
    return concaveHull;
  }
}