Example #1
0
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))
}