Mercurial > gemma
view pkg/octree/vertex.go @ 1141:a473d91b0856
merge
author | Markus Kottlaender <markus@intevation.de> |
---|---|
date | Mon, 12 Nov 2018 14:48:18 +0100 |
parents | a244b18cb916 |
children | ea2143adc6d3 |
line wrap: on
line source
// This is Free Software under GNU Affero General Public License v >= 3.0 // without warranty, see README.md and license for details. // // SPDX-License-Identifier: AGPL-3.0-or-later // License-Filename: LICENSES/AGPL-3.0.txt // // Copyright (C) 2018 by via donau // – Österreichische Wasserstraßen-Gesellschaft mbH // Software engineering by Intevation GmbH // // Author(s): // * Sascha L. Teichmann <sascha.teichmann@intevation.de> 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 } ) 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() }