// uDir normally true
func (this *NurbsSurface) isClosed(uDir bool) bool {
var cpts [][]HomoPoint
if uDir {
cpts = this.controlPoints
} else {
cpts := make([][]HomoPoint, len(this.controlPoints))
for i := range cpts {
cpts[i] = make([]HomoPoint, len(this.controlPoints[0]))
copy(cpts[i], this.controlPoints[i])
}
cpts = transposed(cpts)
}
for i := range cpts[0] {
// TODO there's probably a more efficient, equally effective way
first, last := cpts[0][i], cpts[len(cpts)-1][i]
dist := math.Sqrt(
vec3.SquareDistance(&first.Vec3, &last.Vec3) +
(first.W-last.W)*(first.W-last.W),
)
if dist >= Epsilon {
return false
}
}
return true
}
func (this *NurbsCurve) ClosestParam(p vec3.T) float64 {
// We want to solve:
//
// C'(u) * ( C(u) - P ) = 0 = f(u)
//
// C(u) is the curve, p is the point, * is a dot product
//
// We'll use newton's method:
//
// u* = u - f / f'
//
// We use the product rule in order to form the derivative, f':
//
// f' = C"(u) * ( C(u) - p ) + C'(u) * C'(u)
//
// What is the conversion criteria? (Piegl & Tiller suggest)
//
// |C(u) - p| < e1
//
// |C'(u)*(C(u) - P)|
// ------------------ < e2
// |C'(u)| |C(u) - P|
//
// 1) first check 2 & 3
// 2) if at least one of these is not, compute new value, otherwise halt
// 3) ensure the parameter stays within range
// * if not closed, don't allow outside of range a-b
// * if closed (e.g. circle), allow to move back to beginning
// 4) if |(u* - u)C'(u)| < e1, halt
//
min := math.MaxFloat64
var u float64
pts := this.regularSample(len(this.controlPoints) * this.degree)
for i := 0; i < len(pts)-1; i++ {
u0, u1 := pts[i].U, pts[i+1].U
p0 := pts[i].Pt
p1 := pts[i+1].Pt
proj := segmentClosestPoint(&p, &p0, &p1, u0, u1)
dv := vec3.Sub(&p, &proj.Pt)
d := dv.Length()
if d < min {
min = d
u = proj.U
}
}
maxits := 5
var i int
var e []vec3.T
eps1, eps2 := 0.0001, 0.0005
var dif vec3.T
minu, maxu := this.knots[0], this.knots[len(this.knots)-1]
firstCtrlPt := this.controlPoints[0].Dehomogenized()
lastCtrlPt := this.controlPoints[len(this.controlPoints)-1].Dehomogenized()
closed := vec3.SquareDistance(&firstCtrlPt, &lastCtrlPt) < Epsilon
cu := u
f := func(u float64) []vec3.T {
return this.Derivatives(u, 2)
}
n := func(u float64, e []vec3.T, d vec3.T) float64 {
// C'(u) * ( C(u) - P ) = 0 = f(u)
f := vec3.Dot(&e[1], &d)
// f' = C"(u) * ( C(u) - p ) + C'(u) * C'(u)
s0 := vec3.Dot(&e[2], &d)
s1 := vec3.Dot(&e[1], &e[1])
df := s0 + s1
return u - f/df
}
for i < maxits {
e = f(cu)
dif = vec3.Sub(&e[0], &p)
// |C(u) - p| < e1
c1v := dif.Length()
// C'(u) * (C(u) - P)
// ------------------ < e2
// |C'(u)| |C(u) - P|
c2n := vec3.Dot(&e[1], &dif)
c2d := e[1].Length() * c1v
c2v := c2n / c2d
c1 := c1v < eps1
c2 := math.Abs(c2v) < eps2
// if both tolerances are met
if c1 && c2 {
return cu
}
ct := n(cu, e, dif)
// are we outside of the bounds of the curve?
if ct < minu {
if closed {
ct = maxu - (ct - minu)
} else {
ct = minu
}
} else if ct > maxu {
if closed {
ct = minu + (ct - maxu)
} else {
ct = maxu
}
}
// will our next step force us out of the curve?
c3vv := e[1].Scaled(ct - cu)
c3v := c3vv.Length()
if c3v < eps1 {
return cu
}
cu = ct
i++
}
return cu
}
func (this *NurbsSurface) ClosestParam(p vec3.T) UV {
// for surfaces, we try to minimize the following:
//
// f = Su(u,v) * r = 0
// g = Sv(u,v) * r = 0
//
// where r = S(u,v) - P
//
// Again, this requires newton iteration, but this time our objective function is vector valued
//
// J d = k
//
// d = [ u* - u, v* - v ]
// k = - [ f(u,v), g(u,v) ]
// J =
// |Su|^2 + Suu * r Su*Sv + Suv * r
// Su*Sv + Svu * r |Sv|^2 + Svv * r
//
//
// we have similar halting conditions:
//
// point coincidence
//
// |S(u,v) - p| < e1
//
// cosine
//
// |Su(u,v)*(S(u,v) - P)|
// ---------------------- < e2
// |Su(u,v)| |S(u,v) - P|
//
// |Sv(u,v)*(S(u,v) - P)|
// ---------------------- < e2
// |Sv(u,v)| |S(u,v) - P|
//
// 1) first check 2 & 3
// 2) if at least one of these is not, compute new value, otherwise halt
// 3) ensure the parameter stays within range
// * if not closed, don't allow outside of range a-b
// * if closed (e.g. circle), allow to move back to beginning
// 4) if |(u* - u)C'(u)| < e1, halt
//
maxits := 5
var i int
var e [][]vec3.T
eps1, eps2 := 0.0001, 0.0005
var dif vec3.T
minu, maxu := this.knotsU[0], this.knotsU[len(this.knotsU)-1]
minv, maxv := this.knotsV[0], this.knotsV[len(this.knotsV)-1]
closedu, closedv := this.isClosed(true), this.isClosed(false)
var cuv UV
// TODO divide surface instead of a full on tessellation
// approximate closest point with tessellation
tess := this.tessellateAdaptive(&defaultAdaptiveRefinementOptions)
dmin := math.MaxFloat64
for i, x := range tess.Points {
d := vec3.SquareDistance(&p, &x)
if d < dmin {
dmin = d
cuv = tess.UVs[i]
}
}
f := func(uv UV) [][]vec3.T {
return this.Derivatives(uv, 2)
}
n := func(uv UV, e [][]vec3.T, r vec3.T) UV {
// f = Su(u,v) * r = 0
// g = Sv(u,v) * r = 0
Su, Sv := e[1][0], e[0][1]
Suu, Svv := e[2][0], e[0][2]
Suv, Svu := e[1][1], e[1][1]
f := vec3.Dot(&Su, &r)
g := vec3.Dot(&Sv, &r)
k := [2]float64{-f, -g}
J00 := vec3.Dot(&Su, &Su) + vec3.Dot(&Suu, &r)
J01 := vec3.Dot(&Su, &Sv) + vec3.Dot(&Suv, &r)
J10 := vec3.Dot(&Su, &Sv) + vec3.Dot(&Svu, &r)
J11 := vec3.Dot(&Sv, &Sv) + vec3.Dot(&Svv, &r)
//J := [2][2]float64{{J00, J01}, {J10, J11}}
//J := Mat2{J00, J01, J10, J11}
// d = [ u* - u, v* - v ]
// k = - [ f(u,v), g(u,v) ]
// J =
// |Su|^2 + Suu * r Su*Sv + Suv * r
// Su*Sv + Svu * r |Sv|^2 + Svv * r
//
//d := J.Solve(k)
x, y := Mat2Solve(J00, J01, J10, J11, k[0], k[1])
//return UV{d[0] + uv[0], d[1] + uv[1]}
return UV{x + uv[0], y + uv[1]}
}
for i < maxits {
e = f(cuv)
// point coincidence
//
// |S(u,v) - p| < e1
c1v := vec3.Distance(&e[0][0], &p)
//
// cosine
//
// |Su(u,v)*(S(u,v) - P)|
// ---------------------- < e2
// |Su(u,v)| |S(u,v) - P|
//
// |Sv(u,v)*(S(u,v) - P)|
// ---------------------- < e2
// |Sv(u,v)| |S(u,v) - P|
//
c2an := vec3.Dot(&e[1][0], &dif)
c2ad := e[1][0].Length() * c1v
c2bn := vec3.Dot(&e[0][1], &dif)
c2bd := e[0][1].Length() * c1v
c2av := c2an / c2ad
c2bv := c2bn / c2bd
c1 := c1v < eps1
c2a := c2av < eps2
c2b := c2bv < eps2
// if all of the tolerance are met, we're done
if c1 && c2a && c2b {
return cuv
}
// otherwise, take a step
ct := n(cuv, e, dif)
// correct for exceeding bounds
if ct[0] < minu {
if closedu {
ct = UV{maxu - (ct[0] - minu), ct[1]}
} else {
ct = UV{minu + Epsilon, ct[1]}
}
} else if ct[0] > maxu {
if closedu {
ct = UV{minu + (ct[0] - maxu), ct[1]}
} else {
ct = UV{maxu - Epsilon, ct[1]}
}
}
if ct[1] < minv {
if closedv {
ct = UV{ct[0], maxv - (ct[1] - minv)}
} else {
ct = UV{ct[0], minv + Epsilon}
}
} else if ct[1] > maxv {
if closedv {
ct = UV{ct[0], minv + (ct[0] - maxv)}
} else {
ct = UV{ct[0], maxv - Epsilon}
}
}
// if |(u* - u) C'(u)| < e1, halt
c3v0v := e[1][0].Scaled(ct[0] - cuv[0])
c3v0 := c3v0v.Length()
c3v1v := e[0][1].Scaled(ct[1] - cuv[1])
c3v1 := c3v1v.Length()
if c3v0+c3v1 < eps1 {
return cuv
}
cuv = ct
i++
}
return cuv
}