# HG changeset patch
# User Sascha L. Teichmann <sascha.teichmann@intevation.de>
# Date 1551115293 -3600
# Node ID a1e751c08c567935e4f9797b168afe12b514f794
# Parent  86c7a023400e22b5331a4c5faf941accb00b7d51
Calculate difference on single core.

diff -r 86c7a023400e -r a1e751c08c56 cmd/octreediff/main.go
--- a/cmd/octreediff/main.go	Mon Feb 25 17:02:33 2019 +0100
+++ b/cmd/octreediff/main.go	Mon Feb 25 18:21:33 2019 +0100
@@ -125,8 +125,29 @@
 
 		log.Printf("loading took %v\n", time.Since(start))
 
-		// TODO: Do the diff.
-		_, _ = first, second
+		setin := time.Now()
+
+		vf := first.Vertices()
+		nvf := make([]octree.Vertex, 0, len(vf))
+		for i := range vf {
+			v := &vf[i]
+			if z, found := second.Value(v.X, v.Y); found {
+				nvf = append(nvf, octree.Vertex{v.X, v.Y, v.Z - z})
+			}
+		}
+
+		vs := second.Vertices()
+		nsf := make([]octree.Vertex, 0, len(vs))
+		for i := range vs {
+			v := &vs[i]
+			if z, found := first.Value(v.X, v.Y); found {
+				nsf = append(nsf, octree.Vertex{v.X, v.Y, v.Z - z})
+			}
+		}
+
+		log.Printf("setting in took %v\n", time.Since(setin))
+		log.Printf("new vertices first: %d\n", len(nvf))
+		log.Printf("new vertices second: %d\n", len(nsf))
 
 		return nil
 	})
diff -r 86c7a023400e -r a1e751c08c56 pkg/octree/tree.go
--- a/pkg/octree/tree.go	Mon Feb 25 17:02:33 2019 +0100
+++ b/pkg/octree/tree.go	Mon Feb 25 18:21:33 2019 +0100
@@ -32,6 +32,10 @@
 	Max Vertex
 }
 
+func (ot *Tree) Vertices() []Vertex {
+	return ot.vertices
+}
+
 var scale = [4][4]float64{
 	{0.0, 0.0, 0.5, 0.5},
 	{0.5, 0.0, 1.0, 0.5},
@@ -39,6 +43,76 @@
 	{0.5, 0.5, 1.0, 1.0},
 }
 
+func (ot *Tree) Value(x, y float64) (float64, bool) {
+
+	// out of bounding box
+	if x < ot.Min.X || ot.Max.X < x ||
+		y < ot.Min.Y || ot.Max.Y < y {
+		return 0, false
+	}
+
+	type frame struct {
+		pos int32
+		Box2D
+	}
+
+	all := Box2D{ot.Min.X, ot.Min.Y, ot.Max.X, ot.Max.Y}
+
+	stack := []frame{{1, all}}
+
+	for len(stack) > 0 {
+		top := stack[len(stack)-1]
+		stack = stack[:len(stack)-1]
+
+		if top.pos > 0 { // node
+			if index := ot.index[top.pos:]; len(index) > 7 {
+				for i := 0; i < 4; i++ {
+					a := index[i]
+					b := index[i+4]
+					if a == 0 && b == 0 {
+						continue
+					}
+					dx := top.X2 - top.X1
+					dy := top.Y2 - top.Y1
+					nbox := Box2D{
+						dx*scale[i][0] + top.X1,
+						dy*scale[i][1] + top.Y1,
+						dx*scale[i][2] + top.X1,
+						dy*scale[i][3] + top.Y1,
+					}
+					if nbox.Contains(x, y) {
+						if a != 0 {
+							stack = append(stack, frame{a, nbox})
+						}
+						if b != 0 {
+							stack = append(stack, frame{b, nbox})
+						}
+						break
+					}
+				}
+			}
+		} else { // leaf
+			pos := -top.pos - 1
+			n := ot.index[pos]
+			indices := ot.index[pos+1 : pos+1+n]
+
+			for _, idx := range indices {
+				tri := ot.triangles[idx]
+				t := Triangle{
+					ot.vertices[tri[0]],
+					ot.vertices[tri[1]],
+					ot.vertices[tri[2]],
+				}
+				if t.Contains(x, y) {
+					return t.Plane3D().Z(x, y), true
+				}
+			}
+		}
+	}
+
+	return 0, false
+}
+
 // Vertical does a vertical cross cut from (x1, y1) to (x2, y2).
 func (ot *Tree) Vertical(x1, y1, x2, y2 float64, fn func(*Triangle)) {
 
diff -r 86c7a023400e -r a1e751c08c56 pkg/octree/vertex.go
--- a/pkg/octree/vertex.go	Mon Feb 25 17:02:33 2019 +0100
+++ b/pkg/octree/vertex.go	Mon Feb 25 18:21:33 2019 +0100
@@ -60,8 +60,54 @@
 		B float64
 		C float64
 	}
+
+	Plane3D struct {
+		A float64
+		B float64
+		C float64
+		D float64
+	}
 )
 
+func (t *Triangle) Plane3D() Plane3D {
+
+	v0 := t[1].Sub(t[0])
+	v1 := t[2].Sub(t[0])
+	n := v0.Cross(v1).Normalize()
+
+	// x*nx+ y*ny+ z*nz + d = 0
+	// d = - (x*nx+ y*ny + z*nz)
+	d := -t[0].Dot(n)
+	return Plane3D{
+		A: n.X,
+		B: n.Y,
+		C: n.Z,
+		D: d,
+	}
+}
+
+func (p Plane3D) Z(x, y float64) float64 {
+	// p.A*x + p.B*y + p.C*z + p.D = 0
+	return -(p.A*x + p.B*y + p.D) / p.C
+}
+
+func (v Vertex) Normalize() Vertex {
+	s := 1 / v.Length()
+	return Vertex{
+		X: s * v.X,
+		Y: s * v.Y,
+		Z: s * v.Z,
+	}
+}
+
+func (v Vertex) Dot(w Vertex) float64 {
+	return v.X*w.X + v.Y*w.Y + v.Z*w.Z
+}
+
+func (v Vertex) Length() float64 {
+	return math.Sqrt(v.Dot(v))
+}
+
 // Minimize adjust this vertex v to hold the minimum
 // values component-wise of v and w.
 func (v *Vertex) Minimize(w Vertex) {
@@ -166,6 +212,30 @@
 	return 0
 }
 
+func (v Vertex) Dot2(w Vertex) float64 {
+	return v.X*w.X + v.Y*w.Y
+}
+
+func (t *Triangle) Contains(x, y float64) bool {
+	v0 := t[2].Sub(t[0])
+	v1 := t[1].Sub(t[0])
+	v2 := Vertex{X: x, Y: y}.Sub(t[0])
+
+	dot00 := v0.Dot2(v0)
+	dot01 := v0.Dot2(v1)
+	dot02 := v0.Dot2(v2)
+	dot11 := v1.Dot2(v1)
+	dot12 := v1.Dot2(v2)
+
+	// Compute barycentric coordinates
+	invDenom := 1 / (dot00*dot11 - dot01*dot01)
+	u := (dot11*dot02 - dot01*dot12) * invDenom
+	v := (dot00*dot12 - dot01*dot02) * invDenom
+
+	// Check if point is in triangle
+	return u >= 0 && v >= 0 && u+v < 1
+}
+
 // IntersectHorizontal calculates the line string that
 // results when cutting a triangle a a certain height.
 // Can be empty (on intersection),
@@ -518,6 +588,11 @@
 		a.Y2 < a.Y1 || a.Y2 < b.Y1)
 }
 
+func (a Box2D) Contains(x, y float64) bool {
+	return a.X1 <= x && x <= a.X2 &&
+		a.Y1 <= y && y <= a.Y2
+}
+
 // Xi returns the i-th x component.
 func (a Box2D) Xi(i int) float64 {
 	if i == 0 {