// Generate the control points, weights, and knots of a polyline curve
//
// **params**
// + array of points in curve
//
// **returns**
// + a NurbsCurveData object representing a NURBS curve
func Polyline(pts []vec3.T) *verb.NurbsCurve {
knots := make([]float64, len(pts)+1)
var lsum float64
for i := 0; i < len(pts)-1; i++ {
lsum += vec3.Distance(&pts[i], &pts[i+1])
knots[i+2] = lsum
}
knots[len(knots)-1] = lsum
// normalize the knot array
for i := range knots {
knots[i] /= lsum
}
weights := make([]float64, len(pts))
for i := range weights {
weights[i] = 1
}
return verb.NewNurbsCurveUnchecked(1, pts, weights, knots)
}
func distToSegment(a, b, c *vec3.T) float64 {
// check if ac is zero length
acv := vec3.Sub(c, a)
acl := acv.Length()
// subtract b from a
var bma = vec3.Sub(b, a)
if acl < Tolerance {
return bma.Length()
}
// normalize acv
acv.Normalize()
// project b - a to acv = p
p := vec3.Dot(&bma, &acv)
// multiply ac by d = acd
acv.Scale(p).Add(a)
return vec3.Distance(&acv, b)
}
// Find the distance of a point to a ray
//
// **params**
// + point to project
// + origin for ray
// + direction of ray 1, assumed normalized
//
// **returns**
// + the distance
func (this Ray) DistToPoint(pt vec3.T) float64 {
d := this.ClosestPoint(pt)
return vec3.Distance(&d, &pt)
}
func interpCurve(points []vec3.T, degree int, _startTangent, _endTangent *vec3.T) (deg int, controlPoints []vec3.T, weights []float64, knots KnotVec) {
// 0) build knot vector for curve by normalized chord length
// 1) construct effective basis function in square matrix (W)
// 2) construct set of coordinattes to interpolate vector (p)
// 3) set of control points (c)
// Wc = p
// 4) solve for c in all 3 dimensions
if len(points) < degree+1 {
panic("Must supply at least degree + 1 points")
}
us := make([]float64, len(points))
for i := 1; i < len(points); i++ {
chord := vec3.Distance(&points[1], &points[i-1])
us[i] = us[i-1] + chord
}
// normalize
max := us[len(us)-1]
for i := range us {
us[i] /= max
}
// we need two more control points, two more knots
hasTangents := _startTangent != nil && _endTangent != nil
var start, end int
if hasTangents {
end = len(us) - degree + 1
} else {
start = 1
end = len(us) - degree
}
knots = make(KnotVec, 2*(degree+1)+(end-start))
middleKnots := knots[degree+1 : len(knots)-(degree+1)]
for i := range middleKnots {
var weightSums float64
for j := 0; j < degree; j++ {
weightSums += us[i+j]
}
middleKnots[i] = (1.0 / float64(degree) * weightSums)
}
for i := (degree + 1) + len(middleKnots); i < len(knots); i++ {
knots[i] = 1
}
// build matrix of basis function coeffs (TODO: use sparse rep)
oldA := make(Matrix, len(us)+2)
A := oldA[1 : len(oldA)-1]
var n, ld int
if hasTangents {
n = len(points) + 1
ld = len(points) - (degree - 1)
} else {
n = len(points) - 1
ld = len(points) - (degree + 1)
}
for i, u := range us {
span := knots.SpanGivenN(n, degree, u)
basisFuncs := BasisFunctionsGivenKnotSpanIndex(span, u, degree, knots)
row := make([]float64, ld+len(basisFuncs))
ls := span - degree
copy(row[ls:], basisFuncs)
A[i] = row
}
if hasTangents {
tanRow0 := make([]float64, len(A[0]))
tanRow1 := make([]float64, len(A[0]))
tanRow0[0], tanRow0[1] = -1, 1
tanRow1[len(tanRow1)-2], tanRow1[len(tanRow1)-1] = -1, 1
A = oldA
A[0], A[1] = A[1], tanRow0
A[len(A)-1] = tanRow1
}
// for each dimension, solve
xs := make(Matrix, 3)
mult1 := (1 - knots[len(knots)-degree-2]) / float64(degree)
mult0 := knots[degree+1] / float64(degree)
for i := range xs {
var b []float64
if !hasTangents {
b = make([]float64, len(points))
for j := range b {
b[j] = points[j][i]
}
} else {
b = make([]float64, len(points)+2)
// insert the tangents at the second and second to last index
b[0] = points[0][i]
b[1] = mult0 * (*_startTangent)[i]
for j := 1; j < len(points)-1; j++ {
b[j+1] = points[j][i]
}
b[len(b)-2] = mult1 * (*_endTangent)[i]
b[len(b)-1] = points[len(points)-1][i]
}
xs[i] = A.Solve(b)
}
controlPoints = make([]vec3.T, len(xs[0]))
for i := range xs[0] {
controlPoints[i][0] = xs[0][i]
controlPoints[i][1] = xs[1][i]
controlPoints[i][2] = xs[2][i]
}
weights = make([]float64, len(controlPoints))
for i := range weights {
weights[i] = 1
}
deg = degree
return
}
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
}
