#include "sbasis-geometric.h" #include "sbasis.h" #include "sbasis-math.h" //#include "solver.h" #include "sbasis-geometric.h" /** Geometric operators on D2<SBasis> (1D->2D). * Copyright 2007 JF Barraud * Copyright 2007 N Hurst * * The functions defined in this header related to 2d geometric operations such as arc length, * unit_vector, curvature, and centroid. Most are built on top of unit_vector, which takes an * arbitrary D2 and returns a D2 with unit length with the same direction. * * Todo/think about: * arclength D2 -> sbasis (giving arclength function) * does uniform_speed return natural parameterisation? * integrate sb2d code from normal-bundle * angle(md<2>) -> sbasis (gives angle from vector - discontinuous?) * osculating circle center? * **/ //namespace Geom{ using namespace Geom; using namespace std; //Some utils first. //TODO: remove this!! static vector<double> vect_intersect(vector<double> const &a, vector<double> const &b, double tol=0.){ vector<double> inter; unsigned i=0,j=0; while ( i<a.size() && j<b.size() ){ if (fabs(a[i]-b[j])<tol){ inter.push_back(a[i]); i+=1; j+=1; }else if (a[i]<b[j]){ i+=1; }else if (a[i]>b[j]){ j+=1; } } return inter; } static SBasis divide_by_sk(SBasis const &a, int k) { assert( k<(int)a.size()); if(k < 0) return shift(a,-k); SBasis c; c.insert(c.begin(), a.begin()+k, a.end()); return c; } static SBasis divide_by_t0k(SBasis const &a, int k) { if(k < 0) { SBasis c = Linear(0,1); for (int i=2; i<=-k; i++){ c*=c; } c*=a; return(c); }else{ SBasis c = Linear(1,0); for (int i=2; i<=k; i++){ c*=c; } c*=a; return(divide_by_sk(c,k)); } } static SBasis divide_by_t1k(SBasis const &a, int k) { if(k < 0) { SBasis c = Linear(1,0); for (int i=2; i<=-k; i++){ c*=c; } c*=a; return(c); }else{ SBasis c = Linear(0,1); for (int i=2; i<=k; i++){ c*=c; } c*=a; return(divide_by_sk(c,k)); } } static D2<SBasis> RescaleForNonVanishingEnds(D2<SBasis> const &MM, double ZERO=1.e-4){ D2<SBasis> M = MM; //TODO: divide by all the s at once!!! while (fabs(M[0].at0())<ZERO && fabs(M[1].at0())<ZERO && fabs(M[0].at1())<ZERO && fabs(M[1].at1())<ZERO){ M[0] = divide_by_sk(M[0],1); M[1] = divide_by_sk(M[1],1); } while (fabs(M[0].at0())<ZERO && fabs(M[1].at0())<ZERO){ M[0] = divide_by_t0k(M[0],1); M[1] = divide_by_t0k(M[1],1); } while (fabs(M[0].at1())<ZERO && fabs(M[1].at1())<ZERO){ M[0] = divide_by_t1k(M[0],1); M[1] = divide_by_t1k(M[1],1); } return M; } //================================================================= //TODO: what's this for?!?! Piecewise<D2<SBasis> > Geom::cutAtRoots(Piecewise<D2<SBasis> > const &M, double ZERO){ vector<double> rts; for (unsigned i=0; i<M.size(); i++){ vector<double> seg_rts = roots((M.segs[i])[0]); seg_rts = vect_intersect(seg_rts, roots((M.segs[i])[1]), ZERO); Linear mapToDom = Linear(M.cuts[i],M.cuts[i+1]); for (unsigned r=0; r<seg_rts.size(); r++){ seg_rts[r]= mapToDom(seg_rts[r]); } rts.insert(rts.end(),seg_rts.begin(),seg_rts.end()); } return partition(M,rts); } Piecewise<SBasis> Geom::atan2(Piecewise<D2<SBasis> > const &vect, double tol, unsigned order){ Piecewise<SBasis> result; Piecewise<D2<SBasis> > v = cutAtRoots(vect); result.cuts.push_back(v.cuts.front()); for (unsigned i=0; i<v.size(); i++){ D2<SBasis> vi = RescaleForNonVanishingEnds(v.segs[i]); SBasis x=vi[0], y=vi[1]; Piecewise<SBasis> angle; angle = divide (x*derivative(y)-y*derivative(x), x*x+y*y, tol, order); //TODO: I don't understand this - sign. angle = integral(-angle); Point vi0 = vi.at0(); angle += -std::atan2(vi0[1],vi0[0]) - angle[0].at0(); //TODO: deal with 2*pi jumps form one seg to the other... //TODO: not exact at t=1 because of the integral. //TODO: force continuity? angle.setDomain(Interval(v.cuts[i],v.cuts[i+1])); result.concat(angle); } return result; } Piecewise<SBasis> Geom::atan2(D2<SBasis> const &vect, double tol, unsigned order){ return atan2(Piecewise<D2<SBasis> >(vect),tol,order); } //unitVector(x,y) is computed as (b,-a) where a and b are solutions of: // ax+by=0 (eqn1) and a^2+b^2=1 (eqn2) Piecewise<D2<SBasis> > Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){ D2<SBasis> V = RescaleForNonVanishingEnds(V_in); if (V[0].empty() && V[1].empty()) return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis())); SBasis x = V[0], y = V[1], a, b; SBasis r_eqn1, r_eqn2; Point v0 = unit_vector(V.at0()); Point v1 = unit_vector(V.at1()); a.push_back(Linear(-v0[1],-v1[1])); b.push_back(Linear( v0[0], v1[0])); r_eqn1 = -(a*x+b*y); r_eqn2 = Linear(1.)-(a*a+b*b); for (unsigned k=1; k<=order; k++){ double r0 = (k<r_eqn1.size())? r_eqn1.at(k).at0() : 0; double r1 = (k<r_eqn1.size())? r_eqn1.at(k).at1() : 0; double rr0 = (k<r_eqn2.size())? r_eqn2.at(k).at0() : 0; double rr1 = (k<r_eqn2.size())? r_eqn2.at(k).at1() : 0; double a0,a1,b0,b1;// coeffs in a[k] and b[k] //the equations to solve at this point are: // a0*x(0)+b0*y(0)=r0 & 2*a0*a(0)+2*b0*b(0)=rr0 //and // a1*x(1)+b1*y(1)=r1 & 2*a1*a(1)+2*b1*b(1)=rr1 a0 = r0/dot(v0,V(0))*v0[0]-rr0/2*v0[1]; b0 = r0/dot(v0,V(0))*v0[1]+rr0/2*v0[0]; a1 = r1/dot(v1,V(1))*v1[0]-rr1/2*v1[1]; b1 = r1/dot(v1,V(1))*v1[1]+rr1/2*v1[0]; a.push_back(Linear(a0,a1)); b.push_back(Linear(b0,b1)); //TODO: use "incremental" rather than explicit formulas. r_eqn1 = -(a*x+b*y); r_eqn2 = Linear(1)-(a*a+b*b); } //our candidate is: D2<SBasis> unitV; unitV[0] = b; unitV[1] = -a; //is it good? double rel_tol = std::max(1.,std::max(V_in[0].tailError(0),V_in[1].tailError(0)))*tol; if (r_eqn1.tailError(order)>rel_tol || r_eqn2.tailError(order)>tol){ //if not: subdivide and concat results. Piecewise<D2<SBasis> > unitV0, unitV1; unitV0 = unitVector(compose(V,Linear(0,.5)),tol,order); unitV1 = unitVector(compose(V,Linear(.5,1)),tol,order); unitV0.setDomain(Interval(0.,.5)); unitV1.setDomain(Interval(.5,1.)); unitV0.concat(unitV1); return(unitV0); }else{ //if yes: return it as pw. Piecewise<D2<SBasis> > result; result=(Piecewise<D2<SBasis> >)unitV; return result; } } Piecewise<D2<SBasis> > Geom::unitVector(Piecewise<D2<SBasis> > const &V, double tol, unsigned order){ Piecewise<D2<SBasis> > result; Piecewise<D2<SBasis> > VV = cutAtRoots(V); result.cuts.push_back(VV.cuts.front()); for (unsigned i=0; i<VV.size(); i++){ Piecewise<D2<SBasis> > unit_seg; unit_seg = unitVector(VV.segs[i],tol, order); unit_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1])); result.concat(unit_seg); } return result; } Piecewise<SBasis> Geom::arcLengthSb(Piecewise<D2<SBasis> > const &M, double tol){ Piecewise<D2<SBasis> > dM = derivative(M); Piecewise<SBasis> dMlength = sqrt(dot(dM,dM),tol,3); Piecewise<SBasis> length = integral(dMlength); length-=length.segs.front().at0(); return length; } Piecewise<SBasis> Geom::arcLengthSb(D2<SBasis> const &M, double tol){ return arcLengthSb(Piecewise<D2<SBasis> >(M), tol); } double Geom::length(D2<SBasis> const &M, double tol){ Piecewise<SBasis> length = arcLengthSb(M, tol); return length.segs.back().at1(); } double Geom::length(Piecewise<D2<SBasis> > const &M, double tol){ Piecewise<SBasis> length = arcLengthSb(M, tol); return length.segs.back().at1(); } // incomplete. Piecewise<SBasis> Geom::curvature(D2<SBasis> const &M, double tol) { D2<SBasis> dM=derivative(M); Piecewise<SBasis> result; Piecewise<D2<SBasis> > unitv = unitVector(dM,tol); Piecewise<SBasis> dMlength = dot(Piecewise<D2<SBasis> >(dM),unitv); Piecewise<SBasis> k = cross(derivative(unitv),unitv); k = divide(k,dMlength,tol,3); return(k); } Piecewise<SBasis> Geom::curvature(Piecewise<D2<SBasis> > const &V, double tol){ Piecewise<SBasis> result; Piecewise<D2<SBasis> > VV = cutAtRoots(V); result.cuts.push_back(VV.cuts.front()); for (unsigned i=0; i<VV.size(); i++){ Piecewise<SBasis> curv_seg; curv_seg = curvature(VV.segs[i],tol); curv_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1])); result.concat(curv_seg); } return result; } //================================================================= Piecewise<D2<SBasis> > Geom::arc_length_parametrization(D2<SBasis> const &M, unsigned order, double tol){ Piecewise<D2<SBasis> > u; u.push_cut(0); Piecewise<SBasis> s = arcLengthSb(Piecewise<D2<SBasis> >(M),tol); for (unsigned i=0; i < s.size();i++){ double t0=s.cuts[i],t1=s.cuts[i+1]; D2<SBasis> sub_M = compose(M,Linear(t0,t1)); D2<SBasis> sub_u; for (unsigned dim=0;dim<2;dim++){ SBasis sub_s = s.segs[i]; sub_s-=sub_s.at0(); sub_s/=sub_s.at1(); sub_u[dim]=compose_inverse(sub_M[dim],sub_s, order, tol); } u.push(sub_u,s(t1)); } return u; } Piecewise<D2<SBasis> > Geom::arc_length_parametrization(Piecewise<D2<SBasis> > const &M, unsigned order, double tol){ Piecewise<D2<SBasis> > result; for (unsigned i=0; i<M.size(); i++ ){ Piecewise<D2<SBasis> > uniform_seg=arc_length_parametrization(M[i],order,tol); result.concat(uniform_seg); } return(result); } /** centroid using sbasis integration. * This approach uses green's theorem to compute the area and centroid using integrals. For curved * shapes this is much faster than converting to polyline. * Returned values: 0 for normal execution; 2 if area is zero, meaning centroid is meaningless. * Copyright Nathan Hurst 2006 */ 00341 unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) { Point centroid_tmp(0,0); double atmp = 0; for(unsigned i = 0; i < p.size(); i++) { SBasis curl = dot(p[i], rot90(derivative(p[i]))); SBasis A = integral(curl); D2<SBasis> C = integral(multiply(curl, p[i])); atmp += A.at1() - A.at0(); centroid_tmp += C.at1()- C.at0(); // first moment. } // join ends centroid_tmp *= 2; Point final = p[p.size()-1].at1(), initial = p[0].at0(); const double ai = cross(final, initial); atmp += ai; centroid_tmp += (final + initial)*ai; // first moment. area = atmp / 2; if (atmp != 0) { centroid = centroid_tmp / (3 * atmp); return 0; } return 2; } //}; // namespace /* Local Variables: mode:c++ c-file-style:"stroustrup" c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +)) indent-tabs-mode:nil fill-column:99 End: */ // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :