89eb208660
Before this commit, a single chord tolerance was used for both displaying and exporting geometry. Moreover, this chord tolerance was specified in screen pixels, and as such depended on zoom level. This was inconvenient: exporting geometry with a required level of precision required awkward manipulations of viewport. Moreover, since some operations, e.g. mesh watertightness checking, were done on triangle meshes which are generated differently depending on the zoom level, these operations could report wildly different and quite confusing results depending on zoom level. The chord tolerance for display and export pursue completely distinct goals: display chord tolerance should be set high enough to achieve both fast regeneration and legible rendering, whereas export chord tolerance should be set to match the dimension tolerance of the fabrication process. This commit introduces two distinct chord tolerances: a display and an export one. Both chord tolerances are absolute and expressed in millimeters; this is inappropriate for display purposes but will be fixed in the next commits. After exporting, the geometry is redrawn with the chord tolerance configured for the export and an overlay message is displayed; pressing Esc clears the message and returns the display back to normal.
616 lines
18 KiB
C++
616 lines
18 KiB
C++
//-----------------------------------------------------------------------------
|
|
// Math on rational polynomial surfaces and curves, typically in Bezier
|
|
// form. Evaluate, root-find (by Newton's methods), evaluate derivatives,
|
|
// and so on.
|
|
//
|
|
// Copyright 2008-2013 Jonathan Westhues.
|
|
//-----------------------------------------------------------------------------
|
|
#include "../solvespace.h"
|
|
|
|
// Converge it to better than LENGTH_EPS; we want two points, each
|
|
// independently projected into uv and back, to end up equal with the
|
|
// LENGTH_EPS. Best case that requires LENGTH_EPS/2, but more is better
|
|
// and convergence should be fast by now.
|
|
#define RATPOLY_EPS (LENGTH_EPS/(1e2))
|
|
|
|
double SolveSpace::Bernstein(int k, int deg, double t)
|
|
{
|
|
if(k > deg || k < 0) return 0;
|
|
|
|
switch(deg) {
|
|
case 0:
|
|
return 1;
|
|
|
|
case 1:
|
|
if(k == 0) {
|
|
return (1 - t);
|
|
} else if(k == 1) {
|
|
return t;
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
if(k == 0) {
|
|
return (1 - t)*(1 - t);
|
|
} else if(k == 1) {
|
|
return 2*(1 - t)*t;
|
|
} else if(k == 2) {
|
|
return t*t;
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
if(k == 0) {
|
|
return (1 - t)*(1 - t)*(1 - t);
|
|
} else if(k == 1) {
|
|
return 3*(1 - t)*(1 - t)*t;
|
|
} else if(k == 2) {
|
|
return 3*(1 - t)*t*t;
|
|
} else if(k == 3) {
|
|
return t*t*t;
|
|
}
|
|
break;
|
|
}
|
|
oops();
|
|
}
|
|
|
|
double SolveSpace::BernsteinDerivative(int k, int deg, double t)
|
|
{
|
|
switch(deg) {
|
|
case 0:
|
|
return 0;
|
|
|
|
case 1:
|
|
if(k == 0) {
|
|
return -1;
|
|
} else if(k == 1) {
|
|
return 1;
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
if(k == 0) {
|
|
return -2 + 2*t;
|
|
} else if(k == 1) {
|
|
return 2 - 4*t;
|
|
} else if(k == 2) {
|
|
return 2*t;
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
if(k == 0) {
|
|
return -3 + 6*t - 3*t*t;
|
|
} else if(k == 1) {
|
|
return 3 - 12*t + 9*t*t;
|
|
} else if(k == 2) {
|
|
return 6*t - 9*t*t;
|
|
} else if(k == 3) {
|
|
return 3*t*t;
|
|
}
|
|
break;
|
|
}
|
|
oops();
|
|
}
|
|
|
|
Vector SBezier::PointAt(double t) {
|
|
Vector pt = Vector::From(0, 0, 0);
|
|
double d = 0;
|
|
|
|
int i;
|
|
for(i = 0; i <= deg; i++) {
|
|
double B = Bernstein(i, deg, t);
|
|
pt = pt.Plus(ctrl[i].ScaledBy(B*weight[i]));
|
|
d += weight[i]*B;
|
|
}
|
|
pt = pt.ScaledBy(1.0/d);
|
|
return pt;
|
|
}
|
|
|
|
Vector SBezier::TangentAt(double t) {
|
|
Vector pt = Vector::From(0, 0, 0), pt_p = Vector::From(0, 0, 0);
|
|
double d = 0, d_p = 0;
|
|
|
|
int i;
|
|
for(i = 0; i <= deg; i++) {
|
|
double B = Bernstein(i, deg, t),
|
|
Bp = BernsteinDerivative(i, deg, t);
|
|
|
|
pt = pt.Plus(ctrl[i].ScaledBy(B*weight[i]));
|
|
d += weight[i]*B;
|
|
|
|
pt_p = pt_p.Plus(ctrl[i].ScaledBy(Bp*weight[i]));
|
|
d_p += weight[i]*Bp;
|
|
}
|
|
|
|
// quotient rule; f(t) = n(t)/d(t), so f' = (n'*d - n*d')/(d^2)
|
|
Vector ret;
|
|
ret = (pt_p.ScaledBy(d)).Minus(pt.ScaledBy(d_p));
|
|
ret = ret.ScaledBy(1.0/(d*d));
|
|
return ret;
|
|
}
|
|
|
|
void SBezier::ClosestPointTo(Vector p, double *t, bool converge) {
|
|
int i;
|
|
double minDist = VERY_POSITIVE;
|
|
*t = 0;
|
|
double res = (deg <= 2) ? 7.0 : 20.0;
|
|
for(i = 0; i < (int)res; i++) {
|
|
double tryt = (i/res);
|
|
|
|
Vector tryp = PointAt(tryt);
|
|
double d = (tryp.Minus(p)).Magnitude();
|
|
if(d < minDist) {
|
|
*t = tryt;
|
|
minDist = d;
|
|
}
|
|
}
|
|
|
|
Vector p0;
|
|
for(i = 0; i < (converge ? 15 : 5); i++) {
|
|
p0 = PointAt(*t);
|
|
if(p0.Equals(p, RATPOLY_EPS)) {
|
|
return;
|
|
}
|
|
|
|
Vector dp = TangentAt(*t);
|
|
Vector pc = p.ClosestPointOnLine(p0, dp);
|
|
*t += (pc.Minus(p0)).DivPivoting(dp);
|
|
}
|
|
if(converge) {
|
|
dbp("didn't converge (closest point on bezier curve)");
|
|
}
|
|
}
|
|
|
|
bool SBezier::PointOnThisAndCurve(SBezier *sbb, Vector *p) {
|
|
double ta, tb;
|
|
this->ClosestPointTo(*p, &ta, false);
|
|
sbb ->ClosestPointTo(*p, &tb, false);
|
|
|
|
int i;
|
|
for(i = 0; i < 20; i++) {
|
|
Vector pa = this->PointAt(ta),
|
|
pb = sbb ->PointAt(tb),
|
|
da = this->TangentAt(ta),
|
|
db = sbb ->TangentAt(tb);
|
|
|
|
if(pa.Equals(pb, RATPOLY_EPS)) {
|
|
*p = pa;
|
|
return true;
|
|
}
|
|
|
|
double tta, ttb;
|
|
Vector::ClosestPointBetweenLines(pa, da, pb, db, &tta, &ttb);
|
|
ta += tta;
|
|
tb += ttb;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void SBezier::SplitAt(double t, SBezier *bef, SBezier *aft) {
|
|
Vector4 ct[4];
|
|
int i;
|
|
for(i = 0; i <= deg; i++) {
|
|
ct[i] = Vector4::From(weight[i], ctrl[i]);
|
|
}
|
|
|
|
switch(deg) {
|
|
case 1: {
|
|
Vector4 cts = Vector4::Blend(ct[0], ct[1], t);
|
|
*bef = SBezier::From(ct[0], cts);
|
|
*aft = SBezier::From(cts, ct[1]);
|
|
break;
|
|
}
|
|
case 2: {
|
|
Vector4 ct01 = Vector4::Blend(ct[0], ct[1], t),
|
|
ct12 = Vector4::Blend(ct[1], ct[2], t),
|
|
cts = Vector4::Blend(ct01, ct12, t);
|
|
|
|
*bef = SBezier::From(ct[0], ct01, cts);
|
|
*aft = SBezier::From(cts, ct12, ct[2]);
|
|
break;
|
|
}
|
|
case 3: {
|
|
Vector4 ct01 = Vector4::Blend(ct[0], ct[1], t),
|
|
ct12 = Vector4::Blend(ct[1], ct[2], t),
|
|
ct23 = Vector4::Blend(ct[2], ct[3], t),
|
|
ct01_12 = Vector4::Blend(ct01, ct12, t),
|
|
ct12_23 = Vector4::Blend(ct12, ct23, t),
|
|
cts = Vector4::Blend(ct01_12, ct12_23, t);
|
|
|
|
*bef = SBezier::From(ct[0], ct01, ct01_12, cts);
|
|
*aft = SBezier::From(cts, ct12_23, ct23, ct[3]);
|
|
break;
|
|
}
|
|
default: oops();
|
|
}
|
|
}
|
|
|
|
void SBezier::MakePwlInto(SEdgeList *sel, double chordTol) {
|
|
List<Vector> lv = {};
|
|
MakePwlInto(&lv, chordTol);
|
|
int i;
|
|
for(i = 1; i < lv.n; i++) {
|
|
sel->AddEdge(lv.elem[i-1], lv.elem[i]);
|
|
}
|
|
lv.Clear();
|
|
}
|
|
void SBezier::MakePwlInto(List<SCurvePt> *l, double chordTol) {
|
|
List<Vector> lv = {};
|
|
MakePwlInto(&lv, chordTol);
|
|
int i;
|
|
for(i = 0; i < lv.n; i++) {
|
|
SCurvePt scpt;
|
|
scpt.tag = 0;
|
|
scpt.p = lv.elem[i];
|
|
scpt.vertex = (i == 0) || (i == (lv.n - 1));
|
|
l->Add(&scpt);
|
|
}
|
|
lv.Clear();
|
|
}
|
|
void SBezier::MakePwlInto(SContour *sc, double chordTol) {
|
|
List<Vector> lv = {};
|
|
MakePwlInto(&lv, chordTol);
|
|
int i;
|
|
for(i = 0; i < lv.n; i++) {
|
|
sc->AddPoint(lv.elem[i]);
|
|
}
|
|
lv.Clear();
|
|
}
|
|
void SBezier::MakePwlInto(List<Vector> *l, double chordTol) {
|
|
if(EXACT(chordTol == 0)) {
|
|
// Use the default chord tolerance.
|
|
chordTol = SS.ChordTolMm();
|
|
}
|
|
l->Add(&(ctrl[0]));
|
|
if(deg == 1) {
|
|
l->Add(&(ctrl[1]));
|
|
} else {
|
|
// Never do fewer than one intermediate point; people seem to get
|
|
// unhappy when their circles turn into squares, but maybe less
|
|
// unhappy with octagons.
|
|
MakePwlInitialWorker(l, 0.0, 0.5, chordTol);
|
|
MakePwlInitialWorker(l, 0.5, 1.0, chordTol);
|
|
}
|
|
}
|
|
void SBezier::MakePwlWorker(List<Vector> *l, double ta, double tb, double chordTol)
|
|
{
|
|
Vector pa = PointAt(ta);
|
|
Vector pb = PointAt(tb);
|
|
|
|
Vector pm = PointAt((ta + tb) / 2.0);
|
|
double d = pm.DistanceToLine(pa, pb.Minus(pa));
|
|
|
|
double step = 1.0/SS.GetMaxSegments();
|
|
if((tb - ta) < step || d < chordTol) {
|
|
// A previous call has already added the beginning of our interval.
|
|
l->Add(&pb);
|
|
} else {
|
|
double tm = (ta + tb) / 2;
|
|
MakePwlWorker(l, ta, tm, chordTol);
|
|
MakePwlWorker(l, tm, tb, chordTol);
|
|
}
|
|
}
|
|
void SBezier::MakePwlInitialWorker(List<Vector> *l, double ta, double tb, double chordTol)
|
|
{
|
|
Vector pa = PointAt(ta);
|
|
Vector pb = PointAt(tb);
|
|
|
|
double tm1 = ta + (tb - ta) * 0.25;
|
|
double tm2 = ta + (tb - ta) * 0.5;
|
|
double tm3 = ta + (tb - ta) * 0.75;
|
|
|
|
Vector pm1 = PointAt(tm1);
|
|
Vector pm2 = PointAt(tm2);
|
|
Vector pm3 = PointAt(tm3);
|
|
Vector dir = pb.Minus(pa);
|
|
|
|
double d = max({
|
|
pm1.DistanceToLine(pa, dir),
|
|
pm2.DistanceToLine(pa, dir),
|
|
pm3.DistanceToLine(pa, dir)
|
|
});
|
|
|
|
double step = 1.0/SS.GetMaxSegments();
|
|
if((tb - ta) < step || d < chordTol) {
|
|
// A previous call has already added the beginning of our interval.
|
|
l->Add(&pb);
|
|
} else {
|
|
double tm = (ta + tb) / 2;
|
|
MakePwlWorker(l, ta, tm, chordTol);
|
|
MakePwlWorker(l, tm, tb, chordTol);
|
|
}
|
|
}
|
|
|
|
Vector SSurface::PointAt(Point2d puv) {
|
|
return PointAt(puv.x, puv.y);
|
|
}
|
|
Vector SSurface::PointAt(double u, double v) {
|
|
Vector num = Vector::From(0, 0, 0);
|
|
double den = 0;
|
|
|
|
int i, j;
|
|
for(i = 0; i <= degm; i++) {
|
|
for(j = 0; j <= degn; j++) {
|
|
double Bi = Bernstein(i, degm, u),
|
|
Bj = Bernstein(j, degn, v);
|
|
|
|
num = num.Plus(ctrl[i][j].ScaledBy(Bi*Bj*weight[i][j]));
|
|
den += weight[i][j]*Bi*Bj;
|
|
}
|
|
}
|
|
num = num.ScaledBy(1.0/den);
|
|
return num;
|
|
}
|
|
|
|
void SSurface::TangentsAt(double u, double v, Vector *tu, Vector *tv) {
|
|
Vector num = Vector::From(0, 0, 0),
|
|
num_u = Vector::From(0, 0, 0),
|
|
num_v = Vector::From(0, 0, 0);
|
|
double den = 0,
|
|
den_u = 0,
|
|
den_v = 0;
|
|
|
|
int i, j;
|
|
for(i = 0; i <= degm; i++) {
|
|
for(j = 0; j <= degn; j++) {
|
|
double Bi = Bernstein(i, degm, u),
|
|
Bj = Bernstein(j, degn, v),
|
|
Bip = BernsteinDerivative(i, degm, u),
|
|
Bjp = BernsteinDerivative(j, degn, v);
|
|
|
|
num = num.Plus(ctrl[i][j].ScaledBy(Bi*Bj*weight[i][j]));
|
|
den += weight[i][j]*Bi*Bj;
|
|
|
|
num_u = num_u.Plus(ctrl[i][j].ScaledBy(Bip*Bj*weight[i][j]));
|
|
den_u += weight[i][j]*Bip*Bj;
|
|
|
|
num_v = num_v.Plus(ctrl[i][j].ScaledBy(Bi*Bjp*weight[i][j]));
|
|
den_v += weight[i][j]*Bi*Bjp;
|
|
}
|
|
}
|
|
// quotient rule; f(t) = n(t)/d(t), so f' = (n'*d - n*d')/(d^2)
|
|
*tu = ((num_u.ScaledBy(den)).Minus(num.ScaledBy(den_u)));
|
|
*tu = tu->ScaledBy(1.0/(den*den));
|
|
|
|
*tv = ((num_v.ScaledBy(den)).Minus(num.ScaledBy(den_v)));
|
|
*tv = tv->ScaledBy(1.0/(den*den));
|
|
}
|
|
|
|
Vector SSurface::NormalAt(Point2d puv) {
|
|
return NormalAt(puv.x, puv.y);
|
|
}
|
|
Vector SSurface::NormalAt(double u, double v) {
|
|
Vector tu, tv;
|
|
TangentsAt(u, v, &tu, &tv);
|
|
return tu.Cross(tv);
|
|
}
|
|
|
|
void SSurface::ClosestPointTo(Vector p, Point2d *puv, bool converge) {
|
|
ClosestPointTo(p, &(puv->x), &(puv->y), converge);
|
|
}
|
|
void SSurface::ClosestPointTo(Vector p, double *u, double *v, bool converge) {
|
|
// A few special cases first; when control points are coincident the
|
|
// derivative goes to zero at the conrol points, and would result in
|
|
// nonconvergence. We avoid that here, and also guarantee a consistent
|
|
// (u, v) (of the infinitely many possible in one parameter).
|
|
if(p.Equals(ctrl[0] [0] )) { *u = 0; *v = 0; return; }
|
|
if(p.Equals(ctrl[degm][0] )) { *u = 1; *v = 0; return; }
|
|
if(p.Equals(ctrl[degm][degn])) { *u = 1; *v = 1; return; }
|
|
if(p.Equals(ctrl[0] [degn])) { *u = 0; *v = 1; return; }
|
|
|
|
// And planes are trivial, so don't waste time iterating over those.
|
|
if(degm == 1 && degn == 1) {
|
|
Vector orig = ctrl[0][0],
|
|
bu = (ctrl[1][0]).Minus(orig),
|
|
bv = (ctrl[0][1]).Minus(orig);
|
|
if((ctrl[1][1]).Equals(orig.Plus(bu).Plus(bv))) {
|
|
Vector dp = p.Minus(orig);
|
|
*u = dp.Dot(bu) / bu.MagSquared();
|
|
*v = dp.Dot(bv) / bv.MagSquared();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Try whatever the previous guess was. This is likely to do something
|
|
// good if we're working our way along a curve or something else where
|
|
// we project successive points that are close to each other; something
|
|
// like a 20% speedup empirically.
|
|
if(converge) {
|
|
double ut = cached.x, vt = cached.y;
|
|
if(ClosestPointNewton(p, &ut, &vt, converge)) {
|
|
cached.x = *u = ut;
|
|
cached.y = *v = vt;
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Search for a reasonable initial guess
|
|
int i, j;
|
|
double minDist = VERY_POSITIVE;
|
|
int res = (max(degm, degn) == 2) ? 7 : 20;
|
|
for(i = 0; i < res; i++) {
|
|
for(j = 0; j < res; j++) {
|
|
double tryu = (i + 0.5)/res, tryv = (j + 0.5)/res;
|
|
|
|
Vector tryp = PointAt(tryu, tryv);
|
|
double d = (tryp.Minus(p)).Magnitude();
|
|
if(d < minDist) {
|
|
*u = tryu;
|
|
*v = tryv;
|
|
minDist = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(ClosestPointNewton(p, u, v, converge)) {
|
|
cached.x = *u;
|
|
cached.y = *v;
|
|
return;
|
|
}
|
|
|
|
// If we failed to converge, then at least don't return NaN.
|
|
if(isnan(*u) || isnan(*v)) {
|
|
*u = *v = 0;
|
|
}
|
|
}
|
|
|
|
bool SSurface::ClosestPointNewton(Vector p, double *u, double *v, bool converge)
|
|
{
|
|
// Initial guess is in u, v; refine by Newton iteration.
|
|
Vector p0 = Vector::From(0, 0, 0);
|
|
for(int i = 0; i < (converge ? 25 : 5); i++) {
|
|
p0 = PointAt(*u, *v);
|
|
if(converge) {
|
|
if(p0.Equals(p, RATPOLY_EPS)) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
Vector tu, tv;
|
|
TangentsAt(*u, *v, &tu, &tv);
|
|
|
|
// 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());
|
|
}
|
|
|
|
if(converge) {
|
|
dbp("didn't converge");
|
|
dbp("have %.3f %.3f %.3f", CO(p0));
|
|
dbp("want %.3f %.3f %.3f", CO(p));
|
|
dbp("distance = %g", (p.Minus(p0)).Magnitude());
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool SSurface::PointIntersectingLine(Vector p0, Vector p1, double *u, double *v)
|
|
{
|
|
int i;
|
|
for(i = 0; i < 15; i++) {
|
|
Vector pi, p, tu, tv;
|
|
p = PointAt(*u, *v);
|
|
TangentsAt(*u, *v, &tu, &tv);
|
|
|
|
Vector n = (tu.Cross(tv)).WithMagnitude(1);
|
|
double d = p.Dot(n);
|
|
|
|
bool parallel;
|
|
pi = Vector::AtIntersectionOfPlaneAndLine(n, d, p0, p1, ¶llel);
|
|
if(parallel) break;
|
|
|
|
// Check for convergence
|
|
if(pi.Equals(p, RATPOLY_EPS)) return true;
|
|
|
|
// 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());
|
|
}
|
|
// dbp("didn't converge (surface intersecting line)");
|
|
return false;
|
|
}
|
|
|
|
Vector SSurface::ClosestPointOnThisAndSurface(SSurface *srf2, Vector p) {
|
|
// This is untested.
|
|
int i, j;
|
|
Point2d puv[2];
|
|
SSurface *srf[2] = { this, srf2 };
|
|
|
|
for(j = 0; j < 2; j++) {
|
|
(srf[j])->ClosestPointTo(p, &(puv[j]), false);
|
|
}
|
|
|
|
for(i = 0; i < 10; i++) {
|
|
Vector tu[2], tv[2], cp[2], n[2];
|
|
double d[2];
|
|
|
|
for(j = 0; j < 2; j++) {
|
|
(srf[j])->TangentsAt(puv[j].x, puv[j].y, &(tu[j]), &(tv[j]));
|
|
|
|
cp[j] = (srf[j])->PointAt(puv[j]);
|
|
|
|
n[j] = ((tu[j]).Cross(tv[j])).WithMagnitude(1);
|
|
d[j] = (n[j]).Dot(cp[j]);
|
|
}
|
|
|
|
if((cp[0]).Equals(cp[1], RATPOLY_EPS)) break;
|
|
|
|
Vector p0 = Vector::AtIntersectionOfPlanes(n[0], d[0], n[1], d[1]),
|
|
dp = (n[0]).Cross(n[1]);
|
|
|
|
Vector pc = p.ClosestPointOnLine(p0, dp);
|
|
|
|
// Adjust our guess and iterate
|
|
for(j = 0; j < 2; j++) {
|
|
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());
|
|
}
|
|
}
|
|
if(i >= 10) {
|
|
dbp("this and srf, didn't converge, d=%g",
|
|
(puv[0].Minus(puv[1])).Magnitude());
|
|
}
|
|
|
|
// If this converged, then the two points are actually equal.
|
|
return ((srf[0])->PointAt(puv[0])).Plus(
|
|
((srf[1])->PointAt(puv[1]))).ScaledBy(0.5);
|
|
}
|
|
|
|
void SSurface::PointOnSurfaces(SSurface *s1, SSurface *s2,
|
|
double *up, double *vp)
|
|
{
|
|
double u[3] = { *up, 0, 0 }, v[3] = { *vp, 0, 0 };
|
|
SSurface *srf[3] = { this, s1, s2 };
|
|
|
|
// Get initial guesses for (u, v) in the other surfaces
|
|
Vector p = PointAt(*u, *v);
|
|
(srf[1])->ClosestPointTo(p, &(u[1]), &(v[1]), false);
|
|
(srf[2])->ClosestPointTo(p, &(u[2]), &(v[2]), false);
|
|
|
|
int i, j;
|
|
for(i = 0; i < 20; i++) {
|
|
// Approximate each surface by a plane
|
|
Vector p[3], tu[3], tv[3], n[3];
|
|
double d[3];
|
|
for(j = 0; j < 3; j++) {
|
|
p[j] = (srf[j])->PointAt(u[j], v[j]);
|
|
(srf[j])->TangentsAt(u[j], v[j], &(tu[j]), &(tv[j]));
|
|
n[j] = ((tu[j]).Cross(tv[j])).WithMagnitude(1);
|
|
d[j] = (n[j]).Dot(p[j]);
|
|
}
|
|
|
|
// If a = b and b = c, then does a = c? No, it doesn't.
|
|
if((p[0]).Equals(p[1], RATPOLY_EPS) &&
|
|
(p[1]).Equals(p[2], RATPOLY_EPS) &&
|
|
(p[2]).Equals(p[0], RATPOLY_EPS))
|
|
{
|
|
*up = u[0];
|
|
*vp = v[0];
|
|
return;
|
|
}
|
|
|
|
bool parallel;
|
|
Vector pi = Vector::AtIntersectionOfPlanes(n[0], d[0],
|
|
n[1], d[1],
|
|
n[2], d[2], ¶llel);
|
|
if(parallel) break;
|
|
|
|
for(j = 0; j < 3; j++) {
|
|
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();
|
|
}
|
|
}
|
|
dbp("didn't converge (three surfaces intersecting)");
|
|
}
|
|
|