view pkg/octree/vertex.go @ 959:6ab012d0f0c2

Started with writing an importer job for sounding results.
author Sascha L. Teichmann <sascha.teichmann@intevation.de>
date Tue, 16 Oct 2018 18:20:50 +0200
parents be7b83638ec8
children a4fe07a21ba7
line wrap: on
line source

package octree

import (
	"bytes"
	"encoding/binary"
	"io"
	"log"
	"math"
	"sort"
)

type (
	Vertex struct {
		X float64
		Y float64
		Z float64
	}

	Triangle [3]Vertex

	Line [2]Vertex

	MultiPointZ      []Vertex
	LineStringZ      []Vertex
	MultiLineStringZ []LineStringZ

	Box2D struct {
		X1 float64
		Y1 float64
		X2 float64
		Y2 float64
	}

	Plane2D struct {
		A float64
		B float64
		C float64
	}
)

const (
	wkbNDR byte = 1

	wkbLineString       uint32 = 2
	wkbMultiLineString  uint32 = 5
	wkbPointZ           uint32 = 1000 + 1
	wkbLineStringZ      uint32 = 1000 + 2
	wkbMultiPointZ      uint32 = 1000 + 4
	wkbMultiLineStringZ uint32 = 1000 + 5
)

func (v *Vertex) Minimize(w Vertex) {
	if w.X < v.X {
		v.X = w.X
	}
	if w.Y < v.Y {
		v.Y = w.Y
	}
	if w.Z < v.Z {
		v.Z = w.Z
	}
}

func (v *Vertex) Maximize(w Vertex) {
	if w.X > v.X {
		v.X = w.X
	}
	if w.Y > v.Y {
		v.Y = w.Y
	}
	if w.Z > v.Z {
		v.Z = w.Z
	}
}

func (v Vertex) Sub(w Vertex) Vertex {
	return Vertex{
		v.X - w.X,
		v.Y - w.Y,
		v.Z - w.Z,
	}
}

func (v Vertex) Add(w Vertex) Vertex {
	return Vertex{
		v.X + w.X,
		v.Y + w.Y,
		v.Z + w.Z,
	}
}

func (v Vertex) Scale(s float64) Vertex {
	return Vertex{
		s * v.X,
		s * v.Y,
		s * v.Z,
	}
}

func Interpolate(v1, v2 Vertex) func(Vertex) Vertex {
	v2 = v2.Sub(v1)
	return func(s Vertex) Vertex {
		return Vertex{
			v2.X*s.X + v1.X,
			v2.Y*s.Y + v1.Y,
			v2.Z*s.Z + v1.Z,
		}
	}
}

func (a Vertex) Less(b Vertex) bool {
	return a.X < b.X || a.Y < b.Y || a.Z < b.Z
}

func NewLine(p1, p2 Vertex) Line {
	return Line{
		p2.Sub(p1),
		p1,
	}
}

func (l Line) Eval(t float64) Vertex {
	return l[0].Scale(t).Add(l[1])
}

func (l Line) IntersectHorizontal(h float64) Vertex {
	t := (h - l[1].Z) / l[0].Z
	return l.Eval(t)
}

func side(z, h float64) int {
	switch {
	case z < h:
		return -1
	case z > h:
		return +1
	}
	return 0
}

func (t *Triangle) IntersectHorizontal(h float64) LineStringZ {
	sides := [3]int{
		side(t[0].Z, h),
		side(t[1].Z, h),
		side(t[2].Z, h),
	}

	var points LineStringZ

	for i := 0; i < 3; i++ {
		j := (i + 1) % 3
		si := sides[i]
		sj := sides[j]

		switch {
		case si == 0 && sj == 0:
			// both on plane
			points = append(points, t[i], t[j])
		case si == 0 && sj != 0:
			// first on plane
			points = append(points, t[i])
		case si != 0 && sj == 0:
			// second on plane
			points = append(points, t[j])
		case si == sj:
			// both on same side
		default:
			// real intersection
			v := NewLine(t[i], t[j]).IntersectHorizontal(h)
			points = append(points, v)
		}
	}

	return points
}

func linearScale(x1, y1, x2, y2 float64) func(Vertex) float64 {
	dx := x2 - x1
	dy := y2 - y1

	switch {
	case dx != 0:
		return func(v Vertex) float64 {
			return (v.X - x1) / dx
		}
	case dy != 0:
		return func(v Vertex) float64 {
			return (v.Y - y1) / dy
		}
	default:
		return func(Vertex) float64 {
			return 0
		}
	}
}

func (ls LineStringZ) order(position func(Vertex) float64) {
	type posVertex struct {
		pos float64
		v   Vertex
	}
	positions := make([]posVertex, len(ls))
	for i, v := range ls {
		positions[i] = posVertex{position(v), v}
	}
	sort.Slice(positions, func(i, j int) bool {
		return positions[i].pos < positions[j].pos
	})
	for i := range positions {
		ls[i] = positions[i].v
	}
}

func (v Vertex) EpsEquals(w Vertex) bool {
	const eps = 1e-5
	return math.Abs(v.X-w.X) < eps &&
		math.Abs(v.Y-w.Y) < eps && math.Abs(v.Z-w.Z) < eps
}

func (mls MultiLineStringZ) JoinOnLine(x1, y1, x2, y2 float64) MultiLineStringZ {

	position := linearScale(x1, y1, x2, y2)

	type posLineString struct {
		pos  float64
		line LineStringZ
	}

	positions := make([]posLineString, 0, len(mls))

	for _, ls := range mls {
		if len(ls) == 0 {
			continue
		}
		// order the atoms first
		ls.order(position)
		positions = append(positions, posLineString{position(ls[0]), ls})
	}

	sort.Slice(positions, func(i, j int) bool {
		return positions[i].pos < positions[j].pos
	})

	out := make(MultiLineStringZ, 0, len(positions))

	var ignored int

	for i := range positions {
		curr := positions[i].line
		if l := len(out); l > 0 {
			last := out[l-1]

			if last[len(last)-1].EpsEquals(curr[0]) {
				out[l-1] = append(last[:len(last)-1], curr...)
				continue
			}
			if position(last[len(last)-1]) > position(curr[0]) {
				ignored++
				continue
			}
		}
		out = append(out, curr)
	}

	log.Printf("ignored parts: %d\n", ignored)

	return out
}

func (v *Vertex) Write(w io.Writer) error {
	if err := binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.X)); err != nil {
		return err
	}
	if err := binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.Y)); err != nil {
		return err
	}
	return binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.Z))
}

func (v *Vertex) Read(r io.Reader) error {
	var buf [8]byte
	b := buf[:]
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.X = math.Float64frombits(binary.LittleEndian.Uint64(b))
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.Y = math.Float64frombits(binary.LittleEndian.Uint64(b))
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.Z = math.Float64frombits(binary.LittleEndian.Uint64(b))
	return nil
}

func (mls MultiLineStringZ) AsWKB() []byte {

	// pre-calculate size to avoid reallocations.
	size := 1 + 4 + 4
	for _, ml := range mls {
		size += 1 + 4 + 4 + len(ml)*3*8
	}

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkbNDR)
	binary.Write(buf, binary.LittleEndian, wkbMultiLineStringZ)
	binary.Write(buf, binary.LittleEndian, uint32(len(mls)))

	for _, ml := range mls {
		binary.Write(buf, binary.LittleEndian, wkbNDR)
		binary.Write(buf, binary.LittleEndian, wkbLineStringZ)
		binary.Write(buf, binary.LittleEndian, uint32(len(ml)))
		for _, p := range ml {
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Z))
		}
	}

	return buf.Bytes()
}

func (mls MultiLineStringZ) AsWKB2D() []byte {

	// pre-calculate size to avoid reallocations.
	size := 1 + 4 + 4
	for _, ml := range mls {
		size += 1 + 4 + 4 + len(ml)*2*8
	}

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkbNDR)
	binary.Write(buf, binary.LittleEndian, wkbMultiLineString)
	binary.Write(buf, binary.LittleEndian, uint32(len(mls)))

	for _, ml := range mls {
		binary.Write(buf, binary.LittleEndian, wkbNDR)
		binary.Write(buf, binary.LittleEndian, wkbLineString)
		binary.Write(buf, binary.LittleEndian, uint32(len(ml)))
		for _, p := range ml {
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
		}
	}

	return buf.Bytes()
}

// Join joins two lines leaving the first of the secoung out.
func (ls LineStringZ) Join(other LineStringZ) LineStringZ {
	nline := make(LineStringZ, len(ls)+len(other)-1)
	copy(nline, ls)
	copy(nline[len(ls):], other[1:])
	return nline
}

func (mls MultiLineStringZ) Merge() MultiLineStringZ {

	var out MultiLineStringZ

	min := Vertex{math.MaxFloat64, math.MaxFloat64, math.MaxFloat64}

	for _, line := range mls {
		for _, v := range line {
			min.Minimize(v)
		}
	}

	type point struct {
		x int64
		y int64
	}

	const precision = 1e7

	quant := func(v Vertex) point {
		return point{
			x: int64(math.Round((v.X - min.X) * precision)),
			y: int64(math.Round((v.Y - min.Y) * precision)),
		}
	}

	heads := make(map[point]*[]LineStringZ)

	for _, line := range mls {
		if len(line) < 2 {
			out = append(out, line)
			continue
		}
		head := quant(line[0])
		tail := quant(line[len(line)-1])
		if head == tail { // its already a ring
			out = append(out, line)
			continue
		}

		if hs := heads[tail]; hs != nil {
			l := len(*hs)
			last := (*hs)[l-1]
			if l == 1 {
				delete(heads, tail)
			} else {
				(*hs)[l-1] = nil
				*hs = (*hs)[:l-1]
			}
			line = line.Join(last)

			if head == quant(line[len(line)-1]) { // its a ring
				out = append(out, line)
				continue
			}
		}

		if hs := heads[head]; hs != nil {
			*hs = append(*hs, line)
		} else {
			heads[head] = &[]LineStringZ{line}
		}
	}

again:
	for head, lines := range heads {
		for i, line := range *lines {
			tail := quant(line[len(line)-1])
			for hs := heads[tail]; hs != nil && len(*hs) > 0; hs = heads[tail] {
				l := len(*hs)
				last := (*hs)[l-1]
				(*hs)[l-1] = nil
				*hs = (*hs)[:l-1]
				line = line.Join(last)

				if tail = quant(line[len(line)-1]); head == tail { // its a ring
					out = append(out, line)
					// remove from current lines
					copy((*lines)[i:], (*lines)[i+1:])
					(*lines)[len(*lines)-1] = nil
					*lines = (*lines)[:len(*lines)-1]
					goto again
				}
				// overwrite in current lines
				(*lines)[i] = line
			}
		}
	}

	rings := len(out)

	// The rest are open line strings.
	for _, lines := range heads {
		for _, line := range *lines {
			out = append(out, line)
		}
	}

	log.Printf("segments before/after merge: %d/%d (%d rings)\n",
		len(mls), len(out), rings)

	return out
}

func (a Box2D) Intersects(b Box2D) bool {
	return !(a.X2 < a.X1 || a.X2 < b.X1 ||
		a.Y2 < a.Y1 || a.Y2 < b.Y1)
}

func (a Box2D) Mid() (float64, float64) {
	return (a.X2-a.X1)*0.5 + a.X1, (a.Y2-a.Y1)*0.5 + a.Y1
}

func (a Box2D) Xi(i int) float64 {
	if i == 0 {
		return a.X1
	}
	return a.X2
}

func (a Box2D) Yi(i int) float64 {
	if i == 0 {
		return a.Y1
	}
	return a.Y2
}

func NewPlane2D(x1, y1, x2, y2 float64) Plane2D {
	b := x2 - x1
	a := -(y2 - y1)

	l := math.Sqrt(a*a + b*b)
	a /= l
	b /= l

	// a*x1 + b*y1 + c = 0
	c := -(a*x1 + b*y1)
	return Plane2D{a, b, c}
}

func (p Plane2D) Eval(x, y float64) float64 {
	return p.A*x + p.B*y + p.C
}

const epsPlane = 1e-5

func sides(s int, x float64) int {
	if math.Signbit(x) {
		return s | 2
	}
	return s | 1
}

func (a Box2D) IntersectsPlane(p Plane2D) bool {
	var s int
	for i := 0; i < 2; i++ {
		x := a.Xi(i)
		for j := 0; j < 2; j++ {
			y := a.Yi(j)
			v := p.Eval(x, y)
			if math.Abs(v) < epsPlane {
				//log.Printf("on line")
				return true
			}
			if s = sides(s, v); s == 3 {
				//log.Printf("... on both sides (djt)")
				return true
			}
		}
	}
	//log.Printf("side: %d\n", s)
	return false
}

func (a Vertex) Cross(b Vertex) Vertex {
	return Vertex{
		a.Y*b.Z - a.Z*b.Y,
		a.Z*b.X - a.X*b.Z,
		a.X*b.Y - a.Y*b.X,
	}
}

func (p1 Plane2D) Intersection(p2 Plane2D) (float64, float64, bool) {

	u1 := Vertex{p1.A, p1.B, p1.C}
	u2 := Vertex{p2.A, p2.B, p2.C}

	p := u1.Cross(u2)

	if p.Z == 0 {
		return 0, 0, false
	}

	return p.X / p.Z, p.Y / p.Z, true
}

type VerticalLine struct {
	x1 float64
	y1 float64
	x2 float64
	y2 float64

	line Plane2D
}

func NewVerticalLine(x1, y1, x2, y2 float64) *VerticalLine {
	return &VerticalLine{
		x1:   x1,
		y1:   y1,
		x2:   x2,
		y2:   y2,
		line: NewPlane2D(x1, y1, x2, y2),
	}
}

func onPlane(x float64) bool { return math.Abs(x) < epsPlane }

func relative(v1, v2 Vertex) func(x, y float64) float64 {
	ls := linearScale(v1.X, v1.Y, v2.X, v2.Y)
	return func(x, y float64) float64 {
		return ls(Vertex{x, y, 0})
	}
}

func interpolate(a, b float64) func(float64) float64 {
	return func(x float64) float64 {
		return (b-a)*x + a
	}
}

func linear(v1, v2 Vertex) func(t float64) Vertex {
	return func(t float64) Vertex {
		return Vertex{
			(v2.X-v1.X)*t + v1.X,
			(v2.Y-v1.Y)*t + v1.Y,
			(v2.Z-v1.Z)*t + v1.Z,
		}
	}
}

func inRange(a float64) bool { return 0 <= a && a <= 1 }

func (vl *VerticalLine) Intersection(t *Triangle) LineStringZ {

	var out LineStringZ

	//defer func() { log.Printf("length out: %d\n", len(out)) }()

edges:
	for i := 0; i < 3 && len(out) < 2; i++ {
		j := (i + 1) % 3
		edge := NewPlane2D(t[i].X, t[i].Y, t[j].X, t[j].Y)

		s1 := edge.Eval(vl.x1, vl.y1)
		s2 := edge.Eval(vl.x2, vl.y2)

		o1 := onPlane(s1)
		o2 := onPlane(s2)

		// log.Printf("s1, s2: %t %t (%f %f)\n", o1, o2, s1, s2)

		switch {
		case o1 && o2:
			pos := relative(t[i], t[j])
			t1 := pos(vl.x1, vl.y1)
			t2 := pos(vl.x2, vl.y2)

			r1 := inRange(t1)
			r2 := inRange(t2)

			switch {
			case r1 && r2:
				lin := linear(t[i], t[j])
				out = append(out, lin(t1), lin(t2))
				return out

			case !r1 && !r2: // the hole edge
				out = append(out, t[i], t[j])
				return out
			case !r1:
				if t1 < 0 {
					// below first -> clip by first
					lin := linear(t[i], t[j])
					out = append(out, t[i], lin(t2))
				} else {
					// above second -> clip by second
					lin := linear(t[i], t[j])
					out = append(out, lin(t2), t[j])
				}
				return out
			case !r2:
				if t2 < 0 {
					// below first -> clip by first
					lin := linear(t[i], t[j])
					out = append(out, t[i], lin(t1))
				} else {
					// above second -> clip by second
					lin := linear(t[i], t[j])
					out = append(out, lin(t1), t[j])
				}
				return out
			}

		case o1:
			t1 := relative(t[i], t[j])(vl.x1, vl.y1)
			if !inRange(t1) {
				continue edges
			}
			out = append(out, linear(t[i], t[j])(t1))

		case o2:
			t2 := relative(t[i], t[j])(vl.x2, vl.y2)
			if !inRange(t2) {
				continue edges
			}
			out = append(out, linear(t[i], t[j])(t2))

		default:
			x, y, intersects := vl.line.Intersection(edge)
			if !intersects {
				continue edges
			}

			// log.Println("Intersection -----------------------------")
			t1 := relative(t[i], t[j])(x, y)
			// log.Printf("relative pos: %f\n", t1)
			if !inRange(t1) {
				continue edges
			}

			// log.Println("valid ***************")

			z := interpolate(t[j].Z, t[i].Z)(t1)
			n := Vertex{x, y, z}

			if math.Signbit(s1) != math.Signbit(s2) {
				// log.Println("\ton different sides")
				// different sides -> vertex on edge.
				out = append(out, n)
			} else { // same side -> inside. Find peer
				if len(out) > 0 { // we already have our peer
					out = append(out, n)
					return out
				}

				for k := 0; k < 3; k++ {
					if k == i {
						continue
					}
					l := (k + 1) % 3
					other := NewPlane2D(t[k].X, t[k].Y, t[l].X, t[l].Y)
					xo, yo, intersects := vl.line.Intersection(other)
					if !intersects {
						continue
					}
					t2 := relative(t[k], t[l])(xo, yo)
					if !inRange(t2) {
						continue
					}
					zo := interpolate(t[k].Z, t[l].Z)(t2)

					m := Vertex{xo, yo, zo}

					var xn, yn, xf, yf float64

					// Which is nearer to current edge?
					if math.Abs(s1) < math.Abs(s2) {
						xn, yn = vl.x1, vl.y1
						xf, yf = vl.x2, vl.y2
					} else {
						xn, yn = vl.x2, vl.y2
						xf, yf = vl.x1, vl.y1
					}

					if onPlane(other.Eval(xn, yn)) {
						// triangle intersect in only point
						// treat as no intersection
						break edges
					}

					pos := relative(n, m)

					tzn := pos(xn, yn)
					tzf := pos(xf, yf)

					if !inRange(tzn) {
						// if near is out of range far is, too.
						return out
					}

					lin := interpolate(n.Z, m.Z)

					if inRange(tzf) {
						m.X = xf
						m.Y = yf
						m.Z = lin(tzf)
					} // else m is clipping

					n.X = xn
					n.Y = yn
					n.Z = lin(tzn)

					out = append(out, n, m)
					return out
				}
			}
		}
	}

	// supress single point touches.
	if len(out) == 1 {
		out = out[:0]
	}

	return out
}

func (mpz MultiPointZ) AsWKB() []byte {
	size := 1 + 4 + 4 + len(mpz)*(1+4+3*8)

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkbNDR)
	binary.Write(buf, binary.LittleEndian, wkbMultiPointZ)
	binary.Write(buf, binary.LittleEndian, uint32(len(mpz)))

	for _, p := range mpz {
		binary.Write(buf, binary.LittleEndian, wkbNDR)
		binary.Write(buf, binary.LittleEndian, wkbPointZ)
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Z))
	}

	return buf.Bytes()
}