diff options
Diffstat (limited to 'Build/source/utils/asymptote/path.cc')
-rw-r--r-- | Build/source/utils/asymptote/path.cc | 1340 |
1 files changed, 1340 insertions, 0 deletions
diff --git a/Build/source/utils/asymptote/path.cc b/Build/source/utils/asymptote/path.cc new file mode 100644 index 00000000000..0ad028d5cc5 --- /dev/null +++ b/Build/source/utils/asymptote/path.cc @@ -0,0 +1,1340 @@ +/***** + * path.cc + * Andy Hammerlindl 2002/06/06 + * + * Stores and returns information on a predefined path. + * + * When changing the path algorithms, also update the corresponding + * three-dimensional algorithms in path3.cc. + *****/ + +#include "path.h" +#include "util.h" +#include "angle.h" +#include "camperror.h" +#include "mathop.h" +#include "arrayop.h" +#include "predicates.h" +#include "rounding.h" + +namespace camp { + +const double BigFuzz=10000.0*DBL_EPSILON; +const double Fuzz=1000.0*DBL_EPSILON; +const double Fuzz2=Fuzz*Fuzz; +const double sqrtFuzz=sqrt(Fuzz); +const double third=1.0/3.0; + +path nullpath; + +void checkEmpty(Int n) { + if(n == 0) + reportError("nullpath has no points"); +} + +// Accurate computation of sqrt(1+x)-1. +inline double sqrt1pxm1(double x) +{ + return x/(sqrt(1.0+x)+1.0); +} +inline pair sqrt1pxm1(pair x) +{ + return x/(Sqrt(1.0+x)+1.0); +} + +// Solve for the real roots of the quadratic equation ax^2+bx+c=0. +quadraticroots::quadraticroots(double a, double b, double c) +{ + if(a == 0.0) { + if(b != 0.0) { + distinct=quadraticroots::ONE; + roots=1; + t1=-c/b; + } else if(c == 0.0) { + distinct=quadraticroots::MANY; + roots=1; + t1=0.0; + } else { + distinct=quadraticroots::NONE; + roots=0; + } + } else if(b == 0.0) { + double x=-c/a; + if(x >= 0.0) { + distinct=quadraticroots::TWO; + roots=2; + t2=sqrt(x); + t1=-t2; + } else { + distinct=quadraticroots::NONE; + roots=0; + } + } else { + double factor=0.5*b/a; + double x=-2.0*c/(b*factor); + if(x > -1.0) { + distinct=quadraticroots::TWO; + roots=2; + double sqrtm1=sqrt1pxm1(x); + double r2=factor*sqrtm1; + double r1=-r2-2.0*factor; + if(r1 <= r2) { + t1=r1; + t2=r2; + } else { + t1=r2; + t2=r1; + } + } else if(x == -1.0) { + distinct=quadraticroots::ONE; + roots=2; + t1=t2=-factor; + } else { + distinct=quadraticroots::NONE; + roots=0; + } + } +} + +// Solve for the complex roots of the quadratic equation ax^2+bx+c=0. +Quadraticroots::Quadraticroots(pair a, pair b, pair c) +{ + if(a == 0.0) { + if(b != 0.0) { + roots=1; + z1=-c/b; + } else if(c == 0.0) { + roots=1; + z1=0.0; + } else + roots=0; + } else { + roots=2; + if(b == 0.0) { + z1=Sqrt(-c/a); + z2=-z1; + } else { + pair factor=0.5*b/a; + pair x=-2.0*c/(b*factor); + pair sqrtm1=sqrt1pxm1(x); + z1=factor*sqrtm1; + z2=-z1-2.0*factor; + } + } +} + +inline bool goodroot(double a, double b, double c, double t) +{ + return goodroot(t) && quadratic(a,b,c,t) >= 0.0; +} + +// Accurate computation of cbrt(sqrt(1+x)+1)-cbrt(sqrt(1+x)-1). +inline double cbrtsqrt1pxm(double x) +{ + double s=sqrt1pxm1(x); + return 2.0/(cbrt(x+2.0*(sqrt(1.0+x)+1.0))+cbrt(x)+cbrt(s*s)); +} + +// Taylor series of cos((atan(1.0/w)+pi)/3.0). +static inline double costhetapi3(double w) +{ + static const double c1=1.0/3.0; + static const double c3=-19.0/162.0; + static const double c5=425.0/5832.0; + static const double c7=-16829.0/314928.0; + double w2=w*w; + double w3=w2*w; + double w5=w3*w2; + return c1*w+c3*w3+c5*w5+c7*w5*w2; +} + +// Solve for the real roots of the cubic equation ax^3+bx^2+cx+d=0. +cubicroots::cubicroots(double a, double b, double c, double d) +{ + static const double ninth=1.0/9.0; + static const double fiftyfourth=1.0/54.0; + + // Remove roots at numerical infinity. + if(fabs(a) <= Fuzz*(fabs(b)+fabs(c)*Fuzz+fabs(d)*Fuzz2)) { + quadraticroots q(b,c,d); + roots=q.roots; + if(q.roots >= 1) t1=q.t1; + if(q.roots == 2) t2=q.t2; + return; + } + + // Detect roots at numerical zero. + if(fabs(d) <= Fuzz*(fabs(c)+fabs(b)*Fuzz+fabs(a)*Fuzz2)) { + quadraticroots q(a,b,c); + roots=q.roots+1; + t1=0; + if(q.roots >= 1) t2=q.t1; + if(q.roots == 2) t3=q.t2; + return; + } + + double ainv=1.0/a; + b *= ainv; c *= ainv; d *= ainv; + + double b2=b*b; + double Q=3.0*c-b2; + if(fabs(Q) < Fuzz*(3.0*fabs(c)+fabs(b2))) + Q=0.0; + + double R=(3.0*Q+b2)*b-27.0*d; + if(fabs(R) < Fuzz*((3.0*fabs(Q)+fabs(b2))*fabs(b)+27.0*fabs(d))) + R=0.0; + + Q *= ninth; + R *= fiftyfourth; + + double Q3=Q*Q*Q; + double R2=R*R; + double D=Q3+R2; + double mthirdb=-b*third; + + if(D > 0.0) { + roots=1; + t1=mthirdb; + if(R2 != 0.0) t1 += cbrt(R)*cbrtsqrt1pxm(Q3/R2); + } else { + roots=3; + double v=0.0,theta; + if(R2 > 0.0) { + v=sqrt(-D/R2); + theta=atan(v); + } else theta=0.5*PI; + double factor=2.0*sqrt(-Q)*(R >= 0 ? 1 : -1); + + t1=mthirdb+factor*cos(third*theta); + t2=mthirdb-factor*cos(third*(theta-PI)); + t3=mthirdb; + if(R2 > 0.0) + t3 -= factor*((v < 100.0) ? cos(third*(theta+PI)) : costhetapi3(1.0/v)); + } +} + +pair path::point(double t) const +{ + checkEmpty(n); + + Int i = Floor(t); + Int iplus; + t = fmod(t,1); + if (t < 0) t += 1; + + if (cycles) { + i = imod(i,n); + iplus = imod(i+1,n); + } + else if (i < 0) + return nodes[0].point; + else if (i >= n-1) + return nodes[n-1].point; + else + iplus = i+1; + + double one_t = 1.0-t; + + pair a = nodes[i].point, + b = nodes[i].post, + c = nodes[iplus].pre, + d = nodes[iplus].point, + ab = one_t*a + t*b, + bc = one_t*b + t*c, + cd = one_t*c + t*d, + abc = one_t*ab + t*bc, + bcd = one_t*bc + t*cd, + abcd = one_t*abc + t*bcd; + + return abcd; +} + +pair path::precontrol(double t) const +{ + checkEmpty(n); + + Int i = Floor(t); + Int iplus; + t = fmod(t,1); + if (t < 0) t += 1; + + if (cycles) { + i = imod(i,n); + iplus = imod(i+1,n); + } + else if (i < 0) + return nodes[0].pre; + else if (i >= n-1) + return nodes[n-1].pre; + else + iplus = i+1; + + double one_t = 1.0-t; + + pair a = nodes[i].point, + b = nodes[i].post, + c = nodes[iplus].pre, + ab = one_t*a + t*b, + bc = one_t*b + t*c, + abc = one_t*ab + t*bc; + + return (abc == a) ? nodes[i].pre : abc; +} + + +pair path::postcontrol(double t) const +{ + checkEmpty(n); + + Int i = Floor(t); + Int iplus; + t = fmod(t,1); + if (t < 0) t += 1; + + if (cycles) { + i = imod(i,n); + iplus = imod(i+1,n); + } + else if (i < 0) + return nodes[0].post; + else if (i >= n-1) + return nodes[n-1].post; + else + iplus = i+1; + + double one_t = 1.0-t; + + pair b = nodes[i].post, + c = nodes[iplus].pre, + d = nodes[iplus].point, + bc = one_t*b + t*c, + cd = one_t*c + t*d, + bcd = one_t*bc + t*cd; + + return (bcd == d) ? nodes[iplus].post : bcd; +} + +path path::reverse() const +{ + mem::vector<solvedKnot> nodes(n); + Int len=length(); + for (Int i = 0, j = len; i < n; i++, j--) { + nodes[i].pre = postcontrol(j); + nodes[i].point = point(j); + nodes[i].post = precontrol(j); + nodes[i].straight = straight(j-1); + } + return path(nodes, n, cycles); +} + +path path::subpath(Int a, Int b) const +{ + if(empty()) return path(); + + if (a > b) { + const path &rp = reverse(); + Int len=length(); + path result = rp.subpath(len-a, len-b); + return result; + } + + if (!cycles) { + if (a < 0) + a = 0; + if (b > n-1) + b = n-1; + } + + Int sn = b-a+1; + mem::vector<solvedKnot> nodes(sn); + + for (Int i = 0, j = a; j <= b; i++, j++) { + nodes[i].pre = precontrol(j); + nodes[i].point = point(j); + nodes[i].post = postcontrol(j); + nodes[i].straight = straight(j); + } + nodes[0].pre = nodes[0].point; + nodes[sn-1].post = nodes[sn-1].point; + + return path(nodes, sn); +} + +inline pair split(double t, const pair& x, const pair& y) { return x+(y-x)*t; } + +inline void splitCubic(solvedKnot sn[], double t, const solvedKnot& left_, + const solvedKnot& right_) +{ + solvedKnot &left=(sn[0]=left_), &mid=sn[1], &right=(sn[2]=right_); + if(left.straight) { + mid.point=split(t,left.point,right.point); + pair deltaL=third*(mid.point-left.point); + left.post=left.point+deltaL; + mid.pre=mid.point-deltaL; + pair deltaR=third*(right.point-mid.point); + mid.post=mid.point+deltaR; + right.pre=right.point-deltaR; + mid.straight=true; + } else { + pair x=split(t,left.post,right.pre); // m1 + left.post=split(t,left.point,left.post); // m0 + right.pre=split(t,right.pre,right.point); // m2 + mid.pre=split(t,left.post,x); // m3 + mid.post=split(t,x,right.pre); // m4 + mid.point=split(t,mid.pre,mid.post); // m5 + } +} + +path path::subpath(double a, double b) const +{ + if(empty()) return path(); + + if (a > b) { + const path &rp = reverse(); + Int len=length(); + return rp.subpath(len-a, len-b); + } + + solvedKnot aL, aR, bL, bR; + if (!cycles) { + if (a < 0) { + a = 0; + if (b < 0) + b = 0; + } + if (b > n-1) { + b = n-1; + if (a > n-1) + a = n-1; + } + aL = nodes[(Int)floor(a)]; + aR = nodes[(Int)ceil(a)]; + bL = nodes[(Int)floor(b)]; + bR = nodes[(Int)ceil(b)]; + } else { + if(run::validInt(a) && run::validInt(b)) { + aL = nodes[imod((Int) floor(a),n)]; + aR = nodes[imod((Int) ceil(a),n)]; + bL = nodes[imod((Int) floor(b),n)]; + bR = nodes[imod((Int) ceil(b),n)]; + } else reportError("invalid path index"); + } + + if (a == b) return path(point(a)); + + solvedKnot sn[3]; + path p = subpath(Ceil(a), Floor(b)); + if (a > floor(a)) { + if (b < ceil(a)) { + splitCubic(sn,a-floor(a),aL,aR); + splitCubic(sn,(b-a)/(ceil(b)-a),sn[1],sn[2]); + return path(sn[0],sn[1]); + } + splitCubic(sn,a-floor(a),aL,aR); + p=concat(path(sn[1],sn[2]),p); + } + if (ceil(b) > b) { + splitCubic(sn,b-floor(b),bL,bR); + p=concat(p,path(sn[0],sn[1])); + } + return p; +} + +// Special case of subpath for paths of length 1 used by intersect. +void path::halve(path &first, path &second) const +{ + solvedKnot sn[3]; + splitCubic(sn,0.5,nodes[0],nodes[1]); + first=path(sn[0],sn[1]); + second=path(sn[1],sn[2]); +} + +// Calculate the coefficients of a Bezier derivative divided by 3. +static inline void derivative(pair& a, pair& b, pair& c, + const pair& z0, const pair& c0, + const pair& c1, const pair& z1) +{ + a=z1-z0+3.0*(c0-c1); + b=2.0*(z0+c1)-4.0*c0; + c=c0-z0; +} + +bbox path::bounds() const +{ + if(!box.empty) return box; + + if (empty()) { + // No bounds + return bbox(); + } + + Int len=length(); + box.add(point(len)); + + for (Int i = 0; i < len; i++) { + addpoint(box,i); + if(straight(i)) continue; + + pair a,b,c; + derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1)); + + // Check x coordinate + quadraticroots x(a.getx(),b.getx(),c.getx()); + if(x.distinct != quadraticroots::NONE && goodroot(x.t1)) + addpoint(box,i+x.t1); + if(x.distinct == quadraticroots::TWO && goodroot(x.t2)) + addpoint(box,i+x.t2); + + // Check y coordinate + quadraticroots y(a.gety(),b.gety(),c.gety()); + if(y.distinct != quadraticroots::NONE && goodroot(y.t1)) + addpoint(box,i+y.t1); + if(y.distinct == quadraticroots::TWO && goodroot(y.t2)) + addpoint(box,i+y.t2); + } + return box; +} + +bbox path::bounds(double min, double max) const +{ + bbox box; + + Int len=length(); + for (Int i = 0; i < len; i++) { + addpoint(box,i,min,max); + if(straight(i)) continue; + + pair a,b,c; + derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1)); + + // Check x coordinate + quadraticroots x(a.getx(),b.getx(),c.getx()); + if(x.distinct != quadraticroots::NONE && goodroot(x.t1)) + addpoint(box,i+x.t1,min,max); + + if(x.distinct == quadraticroots::TWO && goodroot(x.t2)) + addpoint(box,i+x.t2,min,max); + + // Check y coordinate + quadraticroots y(a.gety(),b.gety(),c.gety()); + if(y.distinct != quadraticroots::NONE && goodroot(y.t1)) + addpoint(box,i+y.t1,min,max); + if(y.distinct == quadraticroots::TWO && goodroot(y.t2)) + addpoint(box,i+y.t2,min,max); + } + addpoint(box,len,min,max); + return box; +} + +inline void add(bbox& box, const pair& z, const pair& min, const pair& max) +{ + box += z+min; + box += z+max; +} + +bbox path::internalbounds(const bbox& padding) const +{ + bbox box; + + // Check interior nodes. + Int len=length(); + for (Int i = 1; i < len; i++) { + pair pre=point(i)-precontrol(i); + pair post=postcontrol(i)-point(i); + + // Check node x coordinate + if((pre.getx() >= 0.0) ^ (post.getx() >= 0)) + add(box,point(i),padding.left,padding.right); + + // Check node y coordinate + if((pre.gety() >= 0.0) ^ (post.gety() >= 0)) + add(box,point(i),pair(0,padding.bottom),pair(0,padding.top)); + } + + // Check interior segments. + for (Int i = 0; i < len; i++) { + if(straight(i)) continue; + + pair a,b,c; + derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1)); + + // Check x coordinate + quadraticroots x(a.getx(),b.getx(),c.getx()); + if(x.distinct != quadraticroots::NONE && goodroot(x.t1)) + add(box,point(i+x.t1),padding.left,padding.right); + if(x.distinct == quadraticroots::TWO && goodroot(x.t2)) + add(box,point(i+x.t2),padding.left,padding.right); + + // Check y coordinate + quadraticroots y(a.gety(),b.gety(),c.gety()); + if(y.distinct != quadraticroots::NONE && goodroot(y.t1)) + add(box,point(i+y.t1),pair(0,padding.bottom),pair(0,padding.top)); + if(y.distinct == quadraticroots::TWO && goodroot(y.t2)) + add(box,point(i+y.t2),pair(0,padding.bottom),pair(0,padding.top)); + } + return box; +} + +// {{{ Arclength Calculations + +static pair a,b,c; + +static double ds(double t) +{ + double dx=quadratic(a.getx(),b.getx(),c.getx(),t); + double dy=quadratic(a.gety(),b.gety(),c.gety(),t); + return sqrt(dx*dx+dy*dy); +} + +// Calculates arclength of a cubic using adaptive simpson integration. +double path::cubiclength(Int i, double goal) const +{ + const pair& z0=point(i); + const pair& z1=point(i+1); + double L; + if(straight(i)) { + L=(z1-z0).length(); + return (goal < 0 || goal >= L) ? L : -goal/L; + } + + const pair& c0=postcontrol(i); + const pair& c1=precontrol(i+1); + + double integral; + derivative(a,b,c,z0,c0,c1,z1); + + if(!simpson(integral,ds,0.0,1.0,DBL_EPSILON,1.0)) + reportError("nesting capacity exceeded in computing arclength"); + L=3.0*integral; + if(goal < 0 || goal >= L) return L; + + double t=goal/L; + goal *= third; + static double dxmin=sqrt(DBL_EPSILON); + if(!unsimpson(goal,ds,0.0,t,100.0*DBL_EPSILON,integral,1.0,dxmin)) + reportError("nesting capacity exceeded in computing arctime"); + return -t; +} + +double path::arclength() const +{ + if (cached_length != -1) return cached_length; + + double L=0.0; + for (Int i = 0; i < n-1; i++) { + L += cubiclength(i); + } + if(cycles) L += cubiclength(n-1); + cached_length = L; + return cached_length; +} + +double path::arctime(double goal) const +{ + if (cycles) { + if (goal == 0 || cached_length == 0) return 0; + if (goal < 0) { + const path &rp = this->reverse(); + double result = -rp.arctime(-goal); + return result; + } + if (cached_length > 0 && goal >= cached_length) { + Int loops = (Int)(goal / cached_length); + goal -= loops*cached_length; + return loops*n+arctime(goal); + } + } else { + if (goal <= 0) + return 0; + if (cached_length > 0 && goal >= cached_length) + return n-1; + } + + double l,L=0; + for (Int i = 0; i < n-1; i++) { + l = cubiclength(i,goal); + if (l < 0) + return (-l+i); + else { + L += l; + goal -= l; + if (goal <= 0) + return i+1; + } + } + if (cycles) { + l = cubiclength(n-1,goal); + if (l < 0) + return -l+n-1; + if (cached_length > 0 && cached_length != L+l) { + reportError("arclength != length.\n" + "path::arclength(double) must have broken semantics.\n" + "Please report this error."); + } + cached_length = L += l; + goal -= l; + return arctime(goal)+n; + } + else { + cached_length = L; + return length(); + } +} + +// }}} + +// {{{ Direction Time Calulation +// Algorithm Stolen from Knuth's MetaFont +inline double cubicDir(const solvedKnot& left, const solvedKnot& right, + const pair& rot) +{ + pair a,b,c; + derivative(a,b,c,left.point,left.post,right.pre,right.point); + a *= rot; b *= rot; c *= rot; + + quadraticroots ret(a.gety(),b.gety(),c.gety()); + switch(ret.distinct) { + case quadraticroots::MANY: + case quadraticroots::ONE: + { + if(goodroot(a.getx(),b.getx(),c.getx(),ret.t1)) return ret.t1; + } break; + + case quadraticroots::TWO: + { + if(goodroot(a.getx(),b.getx(),c.getx(),ret.t1)) return ret.t1; + if(goodroot(a.getx(),b.getx(),c.getx(),ret.t2)) return ret.t2; + } break; + + case quadraticroots::NONE: + break; + } + + return -1; +} + +// TODO: Check that we handle corner cases. +// Velocity(t) == (0,0) +double path::directiontime(const pair& dir) const { + if (dir == pair(0,0)) return 0; + pair rot = pair(1,0)/unit(dir); + + double t; double pre,post; + for (Int i = 0; i < n-1+cycles; ) { + t = cubicDir(this->nodes[i],(cycles && i==n-1) ? nodes[0]:nodes[i+1],rot); + if (t >= 0) return i+t; + i++; + if (cycles || i != n-1) { + pair Pre = (point(i)-precontrol(i))*rot; + pair Post = (postcontrol(i)-point(i))*rot; + static pair zero(0.0,0.0); + if(Pre != zero && Post != zero) { + pre = angle(Pre); + post = angle(Post); + if ((pre <= 0 && post >= 0 && pre >= post - PI) || + (pre >= 0 && post <= 0 && pre <= post + PI)) + return i; + } + } + } + + return -1; +} +// }}} + +// {{{ Path Intersection Calculations + +const unsigned maxdepth=DBL_MANT_DIG; +const unsigned mindepth=maxdepth-16; + +void roots(std::vector<double> &roots, double a, double b, double c, double d) +{ + cubicroots r(a,b,c,d); + if(r.roots >= 1) roots.push_back(r.t1); + if(r.roots >= 2) roots.push_back(r.t2); + if(r.roots == 3) roots.push_back(r.t3); +} + +void roots(std::vector<double> &r, double x0, double c0, double c1, double x1, + double x) +{ + double a=x1-x0+3.0*(c0-c1); + double b=3.0*(x0+c1)-6.0*c0; + double c=3.0*(c0-x0); + double d=x0-x; + roots(r,a,b,c,d); +} + +// Return all intersection times of path g with the pair z. +void intersections(std::vector<double>& T, const path& g, const pair& z, + double fuzz) +{ + double fuzz2=fuzz*fuzz; + Int n=g.length(); + bool cycles=g.cyclic(); + for(Int i=0; i < n; ++i) { + // Check both directions to circumvent degeneracy. + std::vector<double> r; + roots(r,g.point(i).getx(),g.postcontrol(i).getx(), + g.precontrol(i+1).getx(),g.point(i+1).getx(),z.getx()); + roots(r,g.point(i).gety(),g.postcontrol(i).gety(), + g.precontrol(i+1).gety(),g.point(i+1).gety(),z.gety()); + + size_t m=r.size(); + for(size_t j=0 ; j < m; ++j) { + double t=r[j]; + if(t >= -Fuzz && t <= 1.0+Fuzz) { + double s=i+t; + if((g.point(s)-z).abs2() <= fuzz2) { + if(cycles && s >= n-Fuzz) s=0; + T.push_back(s); + } + } + } + } +} + +inline bool online(const pair&p, const pair& q, const pair& z, double fuzz) +{ + if(p == q) return (z-p).abs2() <= fuzz*fuzz; + return (z.getx()-p.getx())*(q.gety()-p.gety()) == + (q.getx()-p.getx())*(z.gety()-p.gety()); +} + +// Return all intersection times of path g with the (infinite) +// line through p and q; if there are an infinite number of intersection points, +// the returned list is guaranteed to include the endpoint times of +// the intersection if endpoints=true. +void lineintersections(std::vector<double>& T, const path& g, + const pair& p, const pair& q, double fuzz, + bool endpoints=false) +{ + Int n=g.length(); + if(n == 0) { + if(online(p,q,g.point((Int) 0),fuzz)) T.push_back(0.0); + return; + } + bool cycles=g.cyclic(); + double dx=q.getx()-p.getx(); + double dy=q.gety()-p.gety(); + double det=p.gety()*q.getx()-p.getx()*q.gety(); + for(Int i=0; i < n; ++i) { + pair z0=g.point(i); + pair c0=g.postcontrol(i); + pair c1=g.precontrol(i+1); + pair z1=g.point(i+1); + pair t3=z1-z0+3.0*(c0-c1); + pair t2=3.0*(z0+c1)-6.0*c0; + pair t1=3.0*(c0-z0); + double a=dy*t3.getx()-dx*t3.gety(); + double b=dy*t2.getx()-dx*t2.gety(); + double c=dy*t1.getx()-dx*t1.gety(); + double d=dy*z0.getx()-dx*z0.gety()+det; + std::vector<double> r; + if(max(max(max(a*a,b*b),c*c),d*d) > + Fuzz2*max(max(max(z0.abs2(),z1.abs2()),c0.abs2()),c1.abs2())) + roots(r,a,b,c,d); + else r.push_back(0.0); + if(endpoints) { + path h=g.subpath(i,i+1); + intersections(r,h,p,fuzz); + intersections(r,h,q,fuzz); + if(online(p,q,z0,fuzz)) r.push_back(0.0); + if(online(p,q,z1,fuzz)) r.push_back(1.0); + } + size_t m=r.size(); + for(size_t j=0 ; j < m; ++j) { + double t=r[j]; + if(t >= -Fuzz && t <= 1.0+Fuzz) { + double s=i+t; + if(cycles && s >= n-Fuzz) s=0; + T.push_back(s); + } + } + } +} + +// An optimized implementation of intersections(g,p--q); +// if there are an infinite number of intersection points, the returned list is +// only guaranteed to include the endpoint times of the intersection. +void intersections(std::vector<double>& S, std::vector<double>& T, + const path& g, const pair& p, const pair& q, double fuzz) +{ + if(q == p) { + std::vector<double> S1; + intersections(S1,g,p,fuzz); + size_t n=S1.size(); + for(size_t i=0; i < n; ++i) { + S.push_back(S1[i]); + T.push_back(0); + } + } else { + pair factor=(q-p)/((q-p).abs2()); + std::vector<double> S1; + lineintersections(S1,g,p,q,fuzz,true); + size_t n=S1.size(); + for(size_t i=0; i < n; ++i) { + double s=S1[i]; + pair z=g.point(s); + double t=dot(g.point(s)-p,factor); + if(t >= -Fuzz && t <= 1.0+Fuzz) { + S.push_back(s); + T.push_back(t); + } + } + } +} + +void add(std::vector<double>& S, double s, const path& p, double fuzz2) +{ + pair P=p.point(s); + for(size_t i=0; i < S.size(); ++i) + if((p.point(S[i])-P).abs2() <= fuzz2) return; + S.push_back(s); +} + +void add(std::vector<double>& S, std::vector<double>& T, double s, double t, + const path& p, const path& q, double fuzz2) +{ + pair P=p.point(s); + for(size_t i=0; i < S.size(); ++i) + if((p.point(S[i])-P).abs2() <= fuzz2) return; + S.push_back(s); + T.push_back(t); +} + +void add(double& s, double& t, std::vector<double>& S, std::vector<double>& T, + std::vector<double>& S1, std::vector<double>& T1, + double pscale, double qscale, double poffset, double qoffset, + const path& p, const path& q, double fuzz, bool single) +{ + if(single) { + s=s*pscale+poffset; + t=t*qscale+qoffset; + } else { + double fuzz2=4.0*fuzz*fuzz; + size_t n=S1.size(); + for(size_t i=0; i < n; ++i) + add(S,T,pscale*S1[i]+poffset,qscale*T1[i]+qoffset,p,q,fuzz2); + } +} + +void add(double& s, double& t, std::vector<double>& S, std::vector<double>& T, + std::vector<double>& S1, std::vector<double>& T1, + const path& p, const path& q, double fuzz, bool single) +{ + size_t n=S1.size(); + if(single) { + if(n > 0) { + s=S1[0]; + t=T1[0]; + } + } else { + double fuzz2=4.0*fuzz*fuzz; + for(size_t i=0; i < n; ++i) + add(S,T,S1[i],T1[i],p,q,fuzz2); + } +} + +void intersections(std::vector<double>& S, path& g, + const pair& p, const pair& q, double fuzz) +{ + double fuzz2=fuzz*fuzz; + std::vector<double> S1; + lineintersections(S1,g,p,q,fuzz); + size_t n=S1.size(); + for(size_t i=0; i < n; ++i) + add(S,S1[i],g,fuzz2); +} + +bool intersections(double &s, double &t, std::vector<double>& S, + std::vector<double>& T, path& p, path& q, + double fuzz, bool single, bool exact, unsigned depth) +{ + if(errorstream::interrupt) throw interrupted(); + + Int lp=p.length(); + if(((lp == 1 && p.straight(0)) || lp == 0) && exact) { + std::vector<double> T1,S1; + intersections(T1,S1,q,p.point((Int) 0),p.point(lp),fuzz); + add(s,t,S,T,S1,T1,p,q,fuzz,single); + return S1.size() > 0; + } + + Int lq=q.length(); + if(((lq == 1 && q.straight(0)) || lq == 0) && exact) { + std::vector<double> S1,T1; + intersections(S1,T1,p,q.point((Int) 0),q.point(lq),fuzz); + add(s,t,S,T,S1,T1,p,q,fuzz,single); + return S1.size() > 0; + } + + pair maxp=p.max(); + pair minp=p.min(); + pair maxq=q.max(); + pair minq=q.min(); + + if(maxp.getx()+fuzz >= minq.getx() && + maxp.gety()+fuzz >= minq.gety() && + maxq.getx()+fuzz >= minp.getx() && + maxq.gety()+fuzz >= minp.gety()) { + // Overlapping bounding boxes + + --depth; + if((maxp-minp).length()+(maxq-minq).length() <= fuzz || depth == 0) { + if(single) { + s=0; + t=0; + } else { + S.push_back(0.0); + T.push_back(0.0); + } + return true; + } + + path p1,p2; + double pscale,poffset; + + if(lp <= 1) { + if(lp == 1) p.halve(p1,p2); + if(lp == 0 || p1 == p || p2 == p) { + std::vector<double> T1,S1; + intersections(T1,S1,q,p.point((Int) 0),p.point((Int) 0),fuzz); + add(s,t,S,T,S1,T1,p,q,fuzz,single); + return S1.size() > 0; + } + pscale=poffset=0.5; + } else { + Int tp=lp/2; + p1=p.subpath(0,tp); + p2=p.subpath(tp,lp); + poffset=tp; + pscale=1.0; + } + + path q1,q2; + double qscale,qoffset; + + if(lq <= 1) { + if(lq == 1) q.halve(q1,q2); + if(lq == 0 || q1 == q || q2 == q) { + std::vector<double> S1,T1; + intersections(S1,T1,p,q.point((Int) 0),q.point((Int) 0),fuzz); + add(s,t,S,T,S1,T1,p,q,fuzz,single); + return S1.size() > 0; + } + qscale=qoffset=0.5; + } else { + Int tq=lq/2; + q1=q.subpath(0,tq); + q2=q.subpath(tq,lq); + qoffset=tq; + qscale=1.0; + } + + bool Short=lp == 1 && lq == 1; + + static size_t maxcount=9; + size_t count=0; + + std::vector<double> S1,T1; + if(intersections(s,t,S1,T1,p1,q1,fuzz,single,exact,depth)) { + add(s,t,S,T,S1,T1,pscale,qscale,0.0,0.0,p,q,fuzz,single); + if(single || depth <= mindepth) + return true; + count += S1.size(); + if(Short && count > maxcount) return true; + } + + S1.clear(); + T1.clear(); + if(intersections(s,t,S1,T1,p1,q2,fuzz,single,exact,depth)) { + add(s,t,S,T,S1,T1,pscale,qscale,0.0,qoffset,p,q,fuzz,single); + if(single || depth <= mindepth) + return true; + count += S1.size(); + if(Short && count > maxcount) return true; + } + + S1.clear(); + T1.clear(); + if(intersections(s,t,S1,T1,p2,q1,fuzz,single,exact,depth)) { + add(s,t,S,T,S1,T1,pscale,qscale,poffset,0.0,p,q,fuzz,single); + if(single || depth <= mindepth) + return true; + count += S1.size(); + if(Short && count > maxcount) return true; + } + + S1.clear(); + T1.clear(); + if(intersections(s,t,S1,T1,p2,q2,fuzz,single,exact,depth)) { + add(s,t,S,T,S1,T1,pscale,qscale,poffset,qoffset,p,q,fuzz,single); + if(single || depth <= mindepth) + return true; + count += S1.size(); + if(Short && count > maxcount) return true; + } + + return S.size() > 0; + } + return false; +} + +// }}} + +ostream& operator<< (ostream& out, const path& p) +{ + Int n = p.length(); + if(n < 0) + out << "<nullpath>"; + else { + for(Int i = 0; i < n; i++) { + out << p.point(i); + if(p.straight(i)) out << "--"; + else + out << ".. controls " << p.postcontrol(i) << " and " + << p.precontrol(i+1) << newl << " .."; + } + if(p.cycles) + out << "cycle"; + else + out << p.point(n); + } + return out; +} + +path concat(const path& p1, const path& p2) +{ + Int n1 = p1.length(), n2 = p2.length(); + + if (n1 == -1) return p2; + if (n2 == -1) return p1; + pair a=p1.point(n1); + pair b=p2.point((Int) 0); + + mem::vector<solvedKnot> nodes(n1+n2+1); + + Int i = 0; + nodes[0].pre = p1.point((Int) 0); + for (Int j = 0; j < n1; j++) { + nodes[i].point = p1.point(j); + nodes[i].straight = p1.straight(j); + nodes[i].post = p1.postcontrol(j); + nodes[i+1].pre = p1.precontrol(j+1); + i++; + } + for (Int j = 0; j < n2; j++) { + nodes[i].point = p2.point(j); + nodes[i].straight = p2.straight(j); + nodes[i].post = p2.postcontrol(j); + nodes[i+1].pre = p2.precontrol(j+1); + i++; + } + nodes[i].point = nodes[i].post = p2.point(n2); + + return path(nodes, i+1); +} + +// Interface to orient2d predicate optimized for pairs. +double orient2d(const pair& a, const pair& b, const pair& c) +{ + double detleft, detright, det; + double detsum, errbound; + double orient; + + FPU_ROUND_DOUBLE; + + detleft = (a.getx() - c.getx()) * (b.gety() - c.gety()); + detright = (a.gety() - c.gety()) * (b.getx() - c.getx()); + det = detleft - detright; + + if (detleft > 0.0) { + if (detright <= 0.0) { + FPU_RESTORE; + return det; + } else { + detsum = detleft + detright; + } + } else if (detleft < 0.0) { + if (detright >= 0.0) { + FPU_RESTORE; + return det; + } else { + detsum = -detleft - detright; + } + } else { + FPU_RESTORE; + return det; + } + + errbound = ccwerrboundA * detsum; + if ((det >= errbound) || (-det >= errbound)) { + FPU_RESTORE; + return det; + } + + double pa[]={a.getx(),a.gety()}; + double pb[]={b.getx(),b.gety()}; + double pc[]={c.getx(),c.gety()}; + + orient = orient2dadapt(pa, pb, pc, detsum); + FPU_RESTORE; + return orient; +} + +// Returns true iff the point z lies in or on the bounding box +// of a,b,c, and d. +bool insidebbox(const pair& a, const pair& b, const pair& c, const pair& d, + const pair& z) +{ + bbox B(a); + B.addnonempty(b); + B.addnonempty(c); + B.addnonempty(d); + return B.left <= z.getx() && z.getx() <= B.right && B.bottom <= z.gety() + && z.gety() <= B.top; +} + +inline bool inrange(double x0, double x1, double x) +{ + return (x0 <= x && x <= x1) || (x1 <= x && x <= x0); +} + +// Return true if point z is on z0--z1; otherwise compute contribution to +// winding number. +bool checkstraight(const pair& z0, const pair& z1, const pair& z, Int& count) +{ + if(z0.gety() <= z.gety() && z.gety() <= z1.gety()) { + double side=orient2d(z0,z1,z); + if(side == 0.0 && inrange(z0.getx(),z1.getx(),z.getx())) + return true; + if(z.gety() < z1.gety() && side > 0) ++count; + } else if(z1.gety() <= z.gety() && z.gety() <= z0.gety()) { + double side=orient2d(z0,z1,z); + if(side == 0.0 && inrange(z0.getx(),z1.getx(),z.getx())) + return true; + if(z.gety() < z0.gety() && side < 0) --count; + } + return false; +} + +// returns true if point is on curve; otherwise compute contribution to +// winding number. +bool checkcurve(const pair& z0, const pair& c0, const pair& c1, + const pair& z1, const pair& z, Int& count, unsigned depth) +{ + if(depth == 0) return true; + --depth; + if(insidebbox(z0,c0,c1,z1,z)) { + const pair m0=0.5*(z0+c0); + const pair m1=0.5*(c0+c1); + const pair m2=0.5*(c1+z1); + const pair m3=0.5*(m0+m1); + const pair m4=0.5*(m1+m2); + const pair m5=0.5*(m3+m4); + if(checkcurve(z0,m0,m3,m5,z,count,depth) || + checkcurve(m5,m4,m2,z1,z,count,depth)) return true; + } else + if(checkstraight(z0,z1,z,count)) return true; + return false; +} + +// Return the winding number of the region bounded by the (cyclic) path +// relative to the point z, or the largest odd integer if the point lies on +// the path. +Int path::windingnumber(const pair& z) const +{ + static const Int undefined=Int_MAX+((Int_MAX % 2)-1);; + + if(!cycles) + reportError("path is not cyclic"); + + bbox b=bounds(); + + if(z.getx() < b.left || z.getx() > b.right || + z.gety() < b.bottom || z.gety() > b.top) return 0; + + Int count=0; + for(Int i=0; i < n; ++i) + if(straight(i)) { + if(checkstraight(point(i),point(i+1),z,count)) + return undefined; + } else + if(checkcurve(point(i),postcontrol(i),precontrol(i+1),point(i+1),z,count, + maxdepth)) return undefined; + return count; +} + +path path::transformed(const transform& t) const +{ + mem::vector<solvedKnot> nodes(n); + + for (Int i = 0; i < n; ++i) { + nodes[i].pre = t * this->nodes[i].pre; + nodes[i].point = t * this->nodes[i].point; + nodes[i].post = t * this->nodes[i].post; + nodes[i].straight = this->nodes[i].straight; + } + + path p(nodes, n, cyclic()); + return p; +} + +path transformed(const transform& t, const path& p) +{ + Int n = p.size(); + mem::vector<solvedKnot> nodes(n); + + for (Int i = 0; i < n; ++i) { + nodes[i].pre = t * p.precontrol(i); + nodes[i].point = t * p.point(i); + nodes[i].post = t * p.postcontrol(i); + nodes[i].straight = p.straight(i); + } + + return path(nodes, n, p.cyclic()); +} + +path nurb(pair z0, pair z1, pair z2, pair z3, + double w0, double w1, double w2, double w3, Int m) +{ + mem::vector<solvedKnot> nodes(m+1); + + if(m < 1) reportError("invalid sampling interval"); + + double step=1.0/m; + for(Int i=0; i <= m; ++i) { + double t=i*step; + double t2=t*t; + double onemt=1.0-t; + double onemt2=onemt*onemt; + double W0=w0*onemt2*onemt; + double W1=w1*3.0*t*onemt2; + double W2=w2*3.0*t2*onemt; + double W3=w3*t2*t; + nodes[i].point=(W0*z0+W1*z1+W2*z2+W3*z3)/(W0+W1+W2+W3); + } + + static const double twothirds=2.0/3.0; + pair z=nodes[0].point; + nodes[0].pre=z; + nodes[0].post=twothirds*z+third*nodes[1].point; + for(int i=1; i < m; ++i) { + pair z0=nodes[i].point; + pair zm=nodes[i-1].point; + pair zp=nodes[i+1].point; + pair pre=twothirds*z0+third*zm; + pair pos=twothirds*z0+third*zp; + pair dir=unit(pos-pre); + nodes[i].pre=z0-length(z0-pre)*dir; + nodes[i].post=z0+length(pos-z0)*dir; + } + z=nodes[m].point; + nodes[m].pre=twothirds*z+third*nodes[m-1].point; + nodes[m].post=z; + return path(nodes,m+1); +} + +} //namespace camp |