func (this PolyhedralUpperEnvelope) Add(i int, P1, P2, Q []float64) { // fmt.Printf("p1,P1 == %s,%s\n", this.p1, P1) // // fmt.Printf("p2,P2 == %s,%s\n", this.p2, P2) if !(reflect.DeepEqual(this.p1, P1)) { panic("p1 is not equal to P1") } if !(reflect.DeepEqual(this.p2, P2)) { panic("p2 is not equal to P2") } for f := 0; f < this.distfunc.Complexity(); f++ { fl := this.sortedfacets[f] fle := FacetListElement{index: i} fle.slope = fl.slope fle.height = this.distfunc.getFacetDistance(vectorutil.Subtract(this.p1, Q), fl.facet) for fl.Size() > 0 && fl.Left().(FacetListElement).height <= fle.height { fl.PopLeft() } fl.PushLeft(fle) } }
func (this PolyhedralDistanceFunction) getFacetDistanceFromTwo(p, q []float64, facet int) float64 { return this.getFacetDistance(vectorutil.Subtract(q, p), facet) }
func (p PolyhedralDistanceFunction) getFacetSlope(p1, p2 []float64, facet int) float64 { return p.getFacetDistance(vectorutil.Subtract(p2, p1), facet) }
func (this PolyhedralDistanceFunction) DistanceFromTwo(p, q []float64) float64 { return this.Distance(vectorutil.Subtract(q, p)) }