diff --git a/src/srf/ratpoly.cpp b/src/srf/ratpoly.cpp index d4fd703f8..70c66b558 100644 --- a/src/srf/ratpoly.cpp +++ b/src/srf/ratpoly.cpp @@ -439,9 +439,14 @@ void SSurface::ClosestPointTo(Vector p, double *u, double *v, bool mustConverge) bu = (ctrl[1][0]).Minus(orig), bv = (ctrl[0][1]).Minus(orig); if((ctrl[1][1]).Equals(orig.Plus(bu).Plus(bv))) { + + Vector n = bu.Cross(bv); + Vector ty = n.Cross(bu).ScaledBy(1.0/bu.MagSquared()); + Vector tx = bv.Cross(n).ScaledBy(1.0/bv.MagSquared()); + Vector dp = p.Minus(orig); - *u = dp.Dot(bu) / bu.MagSquared(); - *v = dp.Dot(bv) / bv.MagSquared(); + *u = dp.Dot(bu) / tx.MagSquared(); + *v = dp.Dot(bv) / ty.MagSquared(); return; } } @@ -501,16 +506,28 @@ bool SSurface::ClosestPointNewton(Vector p, double *u, double *v, bool mustConve } } - Vector tu, tv; + Vector tu, tv, tx, ty; TangentsAt(*u, *v, &tu, &tv); + Vector n = tu.Cross(tv); + // since tu and tv may not be orthogonal, use y in place of v. + // |y| = |v|sin(theta) where theta is the angle between tu and tv. + ty = n.Cross(tu).ScaledBy(1.0/tu.MagSquared()); + tx = tv.Cross(n).ScaledBy(1.0/tv.MagSquared()); // Project the point into a plane through p0, with basis tu, tv; a // second-order thing would converge faster but needs second // derivatives. Vector dp = p.Minus(p0); - double du = dp.Dot(tu), dv = dp.Dot(tv); - *u += du / (tu.MagSquared()); - *v += dv / (tv.MagSquared()); + double du = dp.Dot(tx), + dv = dp.Dot(ty); + *u += du / (tx.MagSquared()); + *v += dv / (ty.MagSquared()); + + if (*u < 0.0) *u = 0.0; + else if (*u > 1.0) *u = 1.0; + if (*v < 0.0) *v = 0.0; + else if (*v > 1.0) *v = 1.0; + } if(mustConverge) { @@ -540,13 +557,17 @@ bool SSurface::PointIntersectingLine(Vector p0, Vector p1, double *u, double *v) // Check for convergence if(pi.Equals(p, RATPOLY_EPS)) return true; + n = tu.Cross(tv); + Vector ty = n.Cross(tu).ScaledBy(1.0/tu.MagSquared()); + Vector tx = tv.Cross(n).ScaledBy(1.0/tv.MagSquared()); + // Adjust our guess and iterate Vector dp = pi.Minus(p); - double du = dp.Dot(tu), dv = dp.Dot(tv); - *u += du / (tu.MagSquared()); - *v += dv / (tv.MagSquared()); + double du = dp.Dot(tx), dv = dp.Dot(ty); + *u += du / tx.MagSquared(); + *v += dv / ty.MagSquared(); } -// dbp("didn't converge (surface intersecting line)"); + dbp("didn't converge (surface intersecting line)"); return false; } @@ -582,10 +603,14 @@ Vector SSurface::ClosestPointOnThisAndSurface(SSurface *srf2, Vector p) { // Adjust our guess and iterate for(j = 0; j < 2; j++) { + Vector n = tu[j].Cross(tv[j]); + Vector ty = n.Cross(tu[j]).ScaledBy(1.0/tu[j].MagSquared()); + Vector tx = tv[j].Cross(n).ScaledBy(1.0/tv[j].MagSquared()); + Vector dc = pc.Minus(cp[j]); - double du = dc.Dot(tu[j]), dv = dc.Dot(tv[j]); - puv[j].x += du / ((tu[j]).MagSquared()); - puv[j].y += dv / ((tv[j]).MagSquared()); + double du = dc.Dot(tx), dv = dc.Dot(ty); + puv[j].x += du / tx.MagSquared(); + puv[j].y += dv / ty.MagSquared(); } } if(i >= 10) { @@ -637,10 +662,15 @@ void SSurface::PointOnSurfaces(SSurface *s1, SSurface *s2, double *up, double *v if(parallel) break; for(j = 0; j < 3; j++) { + Vector n = tu[j].Cross(tv[j]); + Vector ty = n.Cross(tu[j]).ScaledBy(1.0/tu[j].MagSquared()); + Vector tx = tv[j].Cross(n).ScaledBy(1.0/tv[j].MagSquared()); + Vector dp = pi.Minus(p[j]); - double du = dp.Dot(tu[j]), dv = dp.Dot(tv[j]); - u[j] += du / (tu[j]).MagSquared(); - v[j] += dv / (tv[j]).MagSquared(); + double du = dp.Dot(tx), dv = dp.Dot(ty); + + u[j] += du / tx.MagSquared(); + v[j] += dv / ty.MagSquared(); } } dbp("didn't converge (three surfaces intersecting)");