--- a/cmd/srsimplify/main.go	Tue Jun 25 21:04:16 2019 +0200
+++ b/cmd/srsimplify/main.go	Wed Jun 26 12:06:28 2019 +0200
@@ -64,7 +64,7 @@
 func storeXYZ(points octree.MultiPointZ, w io.Writer) error {
 	out := bufio.NewWriter(w)
 	for i := range points {
-		fmt.Fprintf(out, "%.5f %.5f %.5f\n",
+		fmt.Fprintf(out, "%.5f,%.5f,%.5f\n",
 			points[i].X, points[i].Y, points[i].Z)
 	}
 	return out.Flush()
@@ -76,6 +76,10 @@
 		return points
 	}
 
+	if tolerance < 0 {
+		tolerance = -tolerance
+	}
+
 	min := octree.Vertex{X: math.MaxFloat64, Y: math.MaxFloat64, Z: math.MaxFloat64}
 	max := octree.Vertex{X: -math.MaxFloat64, Y: -math.MaxFloat64, Z: -math.MaxFloat64}
 
@@ -111,11 +115,11 @@
 		min.X, min.Y, min.Z,
 		max.X, max.Y, max.Z)
 
-	below := min.Z - 2*tolerance
-	xMin := min.X - 1
-	xMax := max.X + 1
-	yMin := min.Y - 1
-	yMax := max.Y + 1
+	below := min.Z - 3*tolerance
+	xMin := min.X - tolerance
+	xMax := max.X + tolerance
+	yMin := min.Y - tolerance
+	yMax := max.Y + tolerance
 
 	corners := []octree.Vertex{
 		{xMin, yMin, below},
@@ -136,7 +140,6 @@
 	}
 
 	parts := make([][]octree.Vertex, len(tris))
-	var rest int
 
 	maxDists := make([]float64, len(planes))
 	maxIdxs := make([]int, len(planes))
@@ -157,16 +160,83 @@
 				continue nextPoint
 			}
 		}
-		rest++
 	}
 
-	for i, part := range parts {
-		log.Printf("%d: %d %.5f\n", i, len(part), maxDists[i])
+	result := make([]octree.Vertex, 0, len(points))
+
+	var handleTriangle func(*octree.Triangle, float64, int, []octree.Vertex) bool
+
+	handleTriangle = func(
+		t *octree.Triangle,
+		maxDist float64, maxIdx int,
+		points []octree.Vertex,
+	) bool {
+		if maxDist <= tolerance {
+			return false
+		}
+
+		if len(points) == 1 {
+			result = append(result, points[0])
+		}
+
+		var (
+			tris     [3]octree.Triangle
+			planes   [3]octree.Plane3D
+			maxDists [3]float64
+			maxIdxs  [3]int
+			parts    [3][]octree.Vertex
+		)
+
+		top := points[maxIdx]
+		for i := 0; i < 3; i++ {
+			tris[i] = octree.Triangle{t[i], t[(i+1)%3], top}
+			planes[i] = tris[i].Plane3D()
+		}
+
+	nextPoint:
+		for i, v := range points {
+			if i == maxIdx {
+				continue
+			}
+
+			for j := range tris {
+				if tris[j].Contains(v.X, v.Y) {
+					if dist := math.Abs(planes[j].Eval(v)); dist > maxDists[j] {
+						maxDists[j] = dist
+						maxIdxs[j] = len(parts[j])
+					}
+					parts[j] = append(parts[j], v)
+					continue nextPoint
+				}
+			}
+		}
+
+		var found bool
+		for i, part := range parts {
+			if len(part) > 0 && handleTriangle(&tris[i], maxDists[i], maxIdxs[i], part) {
+				found = true
+			}
+		}
+
+		if found {
+			result = append(result, top)
+		}
+
+		return found
 	}
-	log.Printf("rest: %d\n", rest)
 
-	// TODO: Implement me!
-	return points
+	var found bool
+	for i, part := range parts {
+		if len(part) > 0 && handleTriangle(&tris[i], maxDists[i], maxIdxs[i], part) {
+			found = true
+		}
+	}
+
+	if found {
+		result = append(result, top)
+	}
+
+	return result
 }
 
 func main() {