//
// Sample a NURBS curve at 3 points, facilitating adaptive sampling
//
// **params**
// + NurbsCurveData object
// + start parameter for sampling
// + end parameter for sampling
// + whether to prefix the point with the parameter
//
// **returns**
// + an array of dim + 1 length where the first element is the param where it was sampled and the remaining the pt
//
func (this *NurbsCurve) adaptiveSampleRange(start, end, tol float64, includeU bool) []CurvePoint {
// sample curve at three pts
p1, p3 := this.Point(start), this.Point(end)
t := 0.5 + 0.2*rand.Float64()
mid := start + (end-start)*t
p2 := this.Point(mid)
// if the two end control points are coincident, the three point test will always return 0, let's split the curve
diff := vec3.Sub(&p1, &p3)
diff2 := vec3.Sub(&p1, &p2)
// the first condition checks if the curve makes up a loop, if so, we will need to continue evaluation
if (vec3.Dot(&diff, &diff) < tol && vec3.Dot(&diff2, &diff2) > tol) || !threePointsAreCollinear(&p1, &p2, &p3, tol) {
// get the exact middle
exact_mid := start + (end-start)*0.5
// recurse on the two halves
left_pts := this.adaptiveSampleRange(start, exact_mid, tol, includeU)
right_pts := this.adaptiveSampleRange(exact_mid, end, tol, includeU)
// concatenate the two
leftEnd := len(left_pts) - 1
return append(left_pts[:leftEnd:leftEnd], right_pts...)
} else {
return []CurvePoint{{start, p1}, {end, p3}}
}
}
// Find the closest point on a ray
//
// **params**
// + point to project
// + origin for ray
// + direction of ray 1, assumed normalized
//
// **returns**
// + pt
func (this Ray) ClosestPoint(pt vec3.T) vec3.T {
o2pt := vec3.Sub(&pt, &this.Origin)
do2ptr := vec3.Dot(&o2pt, &this.Dir)
dirScaled := this.Dir.Scaled(do2ptr)
proj := vec3.Add(&this.Origin, &dirScaled)
return proj
}
// Vec3Diff returns the rotation quaternion between two vectors.
func Vec3Diff(a, b *vec3.T) T {
cr := vec3.Cross(a, b)
sr := math.Sqrt(2 * (1 + vec3.Dot(a, b)))
oosr := 1 / sr
q := T{cr[0] * oosr, cr[1] * oosr, cr[2] * oosr, sr * 0.5}
return q.Normalized()
}
func (this Triangle) Plane() Plane {
v0, v1, v2 := this[0], this[1], this[2]
normal := vec3.Cross(v1.Sub(&v0), (v2.Sub(&v0)))
invNormal := normal.Scaled(-1)
offset := vec3.Dot(&invNormal, &v0)
return Plane{normal, offset}
}
// Determine if three points form a straight line within a given tolerance for their 2 * squared area
//
// * p2
// / \
// / \
// / \
// / \
// * p1 ---- * p3
//
// The area metric is 2 * the squared norm of the cross product of two edges, requiring no square roots and no divisions
//
// **params**
// + p1
// + p2
// + p3
// + The tolerance
//
// **returns**
// + Whether the triangle passes the test
//
func threePointsAreCollinear(p1, p2, p3 *vec3.T, tol float64) bool {
// find the area of the triangle without using a square root
p2mp1 := vec3.Sub(p2, p1)
p3mp1 := vec3.Sub(p3, p1)
norm := vec3.Cross(&p2mp1, &p3mp1)
area := vec3.Dot(&norm, &norm)
return area < tol
}
func (this Line) SameSide(p0, p1 vec3.T) bool {
lineVec := vec3.Sub(&this[1], &this[0])
p0Vec := vec3.Sub(&p0, &this[0])
p1Vec := vec3.Sub(&p1, &this[0])
cp0 := vec3.Cross(&p0Vec, &lineVec)
cp1 := vec3.Cross(&p1Vec, &lineVec)
return vec3.Dot(&cp0, &cp1) >= 0
}
func (this Plane) IntersectLine(line Line) *vec3.T {
lineVec := vec3.Sub(&line[1], &line[0])
invLineVec := vec3.Sub(&line[0], &line[1])
denom := vec3.Dot(&this.Normal, &invLineVec)
if math.Abs(denom) < epsilon {
// Line is parallel to plane
return nil
}
numer := vec3.Dot(&this.Normal, &line[0]) + this.Offset
t := numer / denom
if t < 0 || t > 1 {
return nil
}
lineVec.Scale(t)
intersectPt := vec3.Add(&lineVec, &line[0])
return &intersectPt
}
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 closest point on a segment
//
// **params**
// + point to project
// + first point of segment
// + second point of segment
// + first param of segment
// + second param of segment
//
// **returns**
// + *Object* with u and pt properties
func segmentClosestPoint(pt, segpt0, segpt1 *vec3.T, u0, u1 float64) CurvePoint {
dif := vec3.Sub(segpt1, segpt0)
l := dif.Length()
if l < Epsilon {
return CurvePoint{u0, *segpt0}
}
o := segpt0
r := dif.Normalize()
o2pt := vec3.Sub(pt, o)
do2ptr := vec3.Dot(&o2pt, r)
if do2ptr < 0 {
return CurvePoint{u0, *segpt0}
} else if do2ptr > l {
return CurvePoint{u1, *segpt1}
}
return CurvePoint{
u0 + (u1-u0)*do2ptr/l,
vec3.Add(o, r.Scale(do2ptr)),
}
}
func (this Plane) TriangleCrosses(tri Triangle) bool {
sign1 := sign(vec3.Dot(&this.Normal, &tri[0]) + this.Offset)
sign2 := sign(vec3.Dot(&this.Normal, &tri[1]) + this.Offset)
sign3 := sign(vec3.Dot(&this.Normal, &tri[2]) + this.Offset)
return !((sign1 == sign2 && sign2 == sign3) && sign1 != 0)
}
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
}
// Dot returns the dot product of two (dived by w) vectors.
func Dot(a, b *T) float64 {
a3 := a.Vec3DividedByW()
b3 := b.Vec3DividedByW()
return vec3.Dot(&a3, &b3)
}
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
}