Douglas-Peucker point simplification

Simplify.cs

using System.Collections.Generic;

namespace KSoft.Asodu.TrendAnalyzer
{
	public class Simplify
	{

		public static double[][] simplify(double[][] points, double tolerance)
		{
			double sqTolerance = tolerance * tolerance;

			return simplifyDouglasPeucker(points, sqTolerance);
		}

		public static double[][] simplify(double[][] points, double tolerance, bool highestQuality)
		{
			double sqTolerance = tolerance * tolerance;

			if (!highestQuality)
				points = SimplifyRadialDistance(points, sqTolerance);

			points = simplifyDouglasPeucker(points, sqTolerance);

			return points;
		}

		private static double[][] SimplifyRadialDistance(double[][] points,
													    double sqTolerance)
		{
			int len = points.Length;

			double[] point = new double[2];
			double[] prevPoint = points[0];

			List<double[]> newPoints = new List<double[]> { prevPoint };

			for (int i = 1; i < len; i++) {
				point = points[i];

				if (GetSquareDistance(point, prevPoint) > sqTolerance) {
					newPoints.Add(point);
					prevPoint = point;
				}
			}

			if (!prevPoint.Equals(point)) {
				newPoints.Add(point);
			}

			double[][] coolPoints = new double[newPoints.Count][];
			newPoints.CopyTo(coolPoints);
			return coolPoints;
		}

		public static double[][] simplifyDouglasPeucker(double[][] points,
													   double sqTolerance)
		{
			int len = points.Length;

			int?[] markers = new int?[len];

			int first = 0;
			int last = len - 1;

			int index = -1;

			List<int> firstStack = new List<int>();
			List<int> lastStack = new List<int>();

			List<double[]> newPoints = new List<double[]>();

			markers[first] = markers[last] = 1;

			while (last != -1) {
				double maxSqDist = 0;

				for (int i = first + 1; i < last; i++) {
					double sqDist = GetSquareSegmentDistance(points[i], points[first],
					                                        points[last]);

					if (sqDist > maxSqDist) {
						index = i;
						maxSqDist = sqDist;
					}
				}

				if (maxSqDist > sqTolerance) {
					markers[index] = 1;

					firstStack.Add(first);
					lastStack.Add(index);

					firstStack.Add(index);
					lastStack.Add(last);
				}

				if (firstStack.Count > 1) {
					first = firstStack[firstStack.Count - 1];
					firstStack.RemoveAt(firstStack.Count - 1);
				} else
					first = -1;

				if (lastStack.Count > 1) {
					last = lastStack[lastStack.Count - 1];
					lastStack.RemoveAt(lastStack.Count - 1);
				} else
					last = -1;
			}

			for (int i = 0; i < len; i++)
				if (markers[i] != null)
					newPoints.Add(points[i]);


			double[][] haha = new double[newPoints.Count][];
			newPoints.CopyTo(haha);
			return haha;
		}

		private static double GetSquareDistance(double[] p1, double[] p2)
		{
			double dx = p1[0] - p2[0];
			double dy = p1[1] - p2[1];
			return dx * dx + dy * dy;
		}


		private static double GetSquareSegmentDistance(double[] p, double[] p1, double[] p2)
		{
			double x = p1[0];
			double y = p1[1];

			double dx = p2[0] - x;
			double dy = p2[1] - y;

			if (dx != 0 || dy != 0) {
				double t = ((p[0] - x) * dx + (p[1] - y) * dy) / (dx * dx + dy * dy);

				if (t > 1) {
					x = p2[0];
					y = p2[1];
				} else if (t > 0) {
					x += dx * t;
					y += dy * t;
				}
			}

			dx = p[0] - x;
			dy = p[1] - y;

			return dx * dx + dy * dy;
		}
	}
}