/** Return a function which gives the angle of vect at each point.
\param vect a piecewise parameteric curve.
\param tol the maximum error allowed.
\param order the maximum degree to use for approximation
\relates Piecewise
*/
Piecewise<SBasis>
Geom::atan2(Piecewise<D2<SBasis> > const &vect, double tol, unsigned order){
Piecewise<SBasis> result;
Piecewise<D2<SBasis> > v = cutAtRoots(vect,tol);
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;
}
/**
* Compute the symmetric Hausdorf distance.
*/
double hausdorf(D2<SBasis>& A, D2<SBasis> const& B,
double m_precision,
double *a_t, double* b_t) {
double h_dist = hausdorfl(A, B, m_precision, a_t, b_t);
double dist = 0;
Point Bx = B.at0();
double t = Geom::nearest_point(Bx, A);
dist = Geom::distance(Bx, A(t));
if (dist > h_dist) {
if(a_t) *a_t = t;
if(b_t) *b_t = 0;
h_dist = dist;
}
Bx = B.at1();
t = Geom::nearest_point(Bx, A);
dist = Geom::distance(Bx, A(t));
if (dist > h_dist) {
if(a_t) *a_t = t;
if(b_t) *b_t = 1;
h_dist = dist;
}
return h_dist;
}
//TODO: this would work only for C1 pw<d2<sb>> input. Split the input at corners to work with pwd2sb...
//TODO: for more general purpose, return a path...
void toPoly(D2<SBasis> const &f, std::list<Point> &p, double tol, bool include_first=true){
D2<SBasis> df = derivative(f);
D2<SBasis> d2f = derivative(df);
double t=0;
if ( include_first ){ p.push_back( f.at0() );}
while (t<1){
Point v = unit_vector(df.valueAt(t));
//OptInterval bounds = bounds_local(df[X]*v[Y]-df[Y]*v[X], Interval(t,1));
OptInterval bounds = bounds_local(d2f[X]*v[Y]-d2f[Y]*v[X], Interval(t,1));
if (bounds) {
double bds_max = (-bounds->min()>bounds->max() ? -bounds->min() : bounds->max());
double dt;
//if (bds_max<tol) dt = 1;
//else dt = tol/bds_max;
if (bds_max<tol/4) dt = 1;
else dt = 2*std::sqrt( tol / bds_max );
t+=dt*5;
if (t>1) t = 1;
}else{
t = 1;
}
p.push_back( f.valueAt(t) );
}
return;
}
/** Make a path from a d2 sbasis.
\param p the d2 Symmetric basis polynomial
\returns a Path
If only_cubicbeziers is true, the resulting path may only contain CubicBezier curves.
*/
Path
path_from_sbasis(D2<SBasis> const &B, double tol, bool only_cubicbeziers) {
PathBuilder pb;
pb.moveTo(B.at0());
build_from_sbasis(pb, B, tol, only_cubicbeziers);
pb.flush();
return pb.peek().front();
}
/**
* Compute the Hausdorf distance from A to B only.
*/
double hausdorfl(D2<SBasis>& A, D2<SBasis> const& B,
double m_precision,
double *a_t, double* b_t) {
std::vector< std::pair<double, double> > xs;
std::vector<Point> Az, Bz;
sbasis_to_bezier (Az, A);
sbasis_to_bezier (Bz, B);
find_collinear_normal(xs, Az, Bz, m_precision);
double h_dist = 0, h_a_t = 0, h_b_t = 0;
double dist = 0;
Point Ax = A.at0();
double t = Geom::nearest_point(Ax, B);
dist = Geom::distance(Ax, B(t));
if (dist > h_dist) {
h_a_t = 0;
h_b_t = t;
h_dist = dist;
}
Ax = A.at1();
t = Geom::nearest_point(Ax, B);
dist = Geom::distance(Ax, B(t));
if (dist > h_dist) {
h_a_t = 1;
h_b_t = t;
h_dist = dist;
}
for (size_t i = 0; i < xs.size(); ++i)
{
Point At = A(xs[i].first);
Point Bu = B(xs[i].second);
double distAtBu = Geom::distance(At, Bu);
t = Geom::nearest_point(At, B);
dist = Geom::distance(At, B(t));
//FIXME: we might miss it due to floating point precision...
if (dist >= distAtBu-.1 && distAtBu > h_dist) {
h_a_t = xs[i].first;
h_b_t = xs[i].second;
h_dist = distAtBu;
}
}
if(a_t) *a_t = h_a_t;
if(b_t) *b_t = h_b_t;
return h_dist;
}
/** Make a path from a d2 sbasis.
\param p the d2 Symmetric basis polynomial
\returns a Path
If only_cubicbeziers is true, the resulting path may only contain CubicBezier curves.
*/
void build_from_sbasis(Geom::PathBuilder &pb, D2<SBasis> const &B, double tol, bool only_cubicbeziers) {
if (!B.isFinite()) {
THROW_EXCEPTION("assertion failed: B.isFinite()");
}
if(tail_error(B, 2) < tol || sbasis_size(B) == 2) { // nearly cubic enough
if( !only_cubicbeziers && (sbasis_size(B) <= 1) ) {
pb.lineTo(B.at1());
} else {
std::vector<Geom::Point> bez;
sbasis_to_bezier(bez, B, 4);
pb.curveTo(bez[1], bez[2], bez[3]);
}
} else {
build_from_sbasis(pb, compose(B, Linear(0, 0.5)), tol, only_cubicbeziers);
build_from_sbasis(pb, compose(B, Linear(0.5, 1)), tol, only_cubicbeziers);
}
}
/*
* This version works by inverting a reasonable upper bound on the error term after subdividing the
* curve at $a$. We keep biting off pieces until there is no more curve left.
*
* Derivation: The tail of the power series is $a_ks^k + a_{k+1}s^{k+1} + \ldots = e$. A
* subdivision at $a$ results in a tail error of $e*A^k, A = (1-a)a$. Let this be the desired
* tolerance tol $= e*A^k$ and invert getting $A = e^{1/k}$ and $a = 1/2 - \sqrt{1/4 - A}$
*/
void
subpath_from_sbasis_incremental(Geom::OldPathSetBuilder &pb, D2<SBasis> B, double tol, bool initial) {
const unsigned k = 2; // cubic bezier
double te = B.tail_error(k);
assert(B[0].IS_FINITE());
assert(B[1].IS_FINITE());
//std::cout << "tol = " << tol << std::endl;
while(1) {
double A = std::sqrt(tol/te); // pow(te, 1./k)
double a = A;
if(A < 1) {
A = std::min(A, 0.25);
a = 0.5 - std::sqrt(0.25 - A); // quadratic formula
if(a > 1) a = 1; // clamp to the end of the segment
} else
a = 1;
assert(a > 0);
//std::cout << "te = " << te << std::endl;
//std::cout << "A = " << A << "; a=" << a << std::endl;
D2<SBasis> Bs = compose(B, Linear(0, a));
assert(Bs.tail_error(k));
std::vector<Geom::Point> bez = sbasis_to_bezier(Bs, 2);
reverse(bez.begin(), bez.end());
if (initial) {
pb.start_subpath(bez[0]);
initial = false;
}
pb.push_cubic(bez[1], bez[2], bez[3]);
// move to next piece of curve
if(a >= 1) break;
B = compose(B, Linear(a, 1));
te = B.tail_error(k);
}
}
/**
* Centroid using sbasis integration.
\param p the Element.
\param centroid on return contains the centroid of the shape
\param area on return contains the signed area of the shape.
\relates Piecewise
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. Note that without an uncross operation the output is not the absolute area.
* Returned values:
0 for normal execution;
2 if area is zero, meaning centroid is meaningless.
*/
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;
}
void checkThatInheritedConstMethodDoesInvalidateMutableFields() {
D2 t;
t.mut = 1;
t.foo();
clang_analyzer_eval(t.mut); // expected-warning{{UNKNOWN}}
}
Geom::Path Gear::path() {
Geom::Path pb;
// angle covered by a full tooth and fillet
double tooth_rotation = 2.0 * tooth_thickness_angle();
// angle covered by an involute
double involute_advance = involute_intersect_angle(outer_radius()) - involute_intersect_angle(root_radius());
// angle covered by the tooth tip
double tip_advance = tooth_thickness_angle() - (2 * (involute_intersect_angle(outer_radius()) - involute_intersect_angle(pitch_radius())));
// angle covered by the toothe root
double root_advance = (tooth_rotation - tip_advance) - (2.0 * involute_advance);
// begin drawing the involute at t if the root circle is larger than the base circle
double involute_t = involute_swath_angle(root_radius())/involute_swath_angle(outer_radius());
//rewind angle to start drawing from the leading edge of the tooth
double first_tooth_angle = _angle - ((0.5 * tip_advance) + involute_advance);
Geom::Point prev;
for (int i=0; i < _number_of_teeth; i++)
{
double cursor = first_tooth_angle + (i * tooth_rotation);
D2<SBasis> leading_I = compose(_involute(cursor, cursor + involute_swath_angle(outer_radius())), Linear(involute_t,1));
if(i != 0) makeContinuous(leading_I, prev);
pb.append(SBasisCurve(leading_I));
cursor += involute_advance;
prev = leading_I.at1();
D2<SBasis> tip = _arc(cursor, cursor+tip_advance, outer_radius());
makeContinuous(tip, prev);
pb.append(SBasisCurve(tip));
cursor += tip_advance;
prev = tip.at1();
cursor += involute_advance;
D2<SBasis> trailing_I = compose(_involute(cursor, cursor - involute_swath_angle(outer_radius())), Linear(1,involute_t));
makeContinuous(trailing_I, prev);
pb.append(SBasisCurve(trailing_I));
prev = trailing_I.at1();
if (base_radius() > root_radius()) {
Geom::Point leading_start = trailing_I.at1();
Geom::Point leading_end = (root_radius() * unit_vector(leading_start - _centre)) + _centre;
prev = leading_end;
pb.appendNew<LineSegment>(leading_end);
}
D2<SBasis> root = _arc(cursor, cursor+root_advance, root_radius());
makeContinuous(root, prev);
pb.append(SBasisCurve(root));
//cursor += root_advance;
prev = root.at1();
if (base_radius() > root_radius()) {
Geom::Point trailing_start = root.at1();
Geom::Point trailing_end = (base_radius() * unit_vector(trailing_start - _centre)) + _centre;
pb.appendNew<LineSegment>(trailing_end);
prev = trailing_end;
}
}
return pb;
}
B::B(const D2 &mi) : fm(mi.GS()) {}
static int
D2__f_v( D2 & obj )
{
return obj.f_v();
}
static int
D2__f_d2( D2 & obj )
{
return obj.f_d2();
}
void draw( cairo_t *cr, std::ostringstream *notify,
int width, int height, bool save )
{
m_width = width;
m_height = height;
m_length = (m_width > m_height) ? m_width : m_height;
m_length *= 2;
cairo_set_line_width (cr, 0.3);
cairo_set_source_rgba (cr, 0.8, 0., 0, 1);
D2<SBasis> A = pshA.asBezier();
cairo_d2_sb(cr, A);
cairo_stroke(cr);
cairo_set_source_rgba (cr, 0.0, 0., 0, 1);
D2<SBasis> B = pshB.asBezier();
cairo_d2_sb(cr, B);
cairo_stroke(cr);
draw_text(cr, A.at0(), "A");
draw_text(cr, B.at0(), "B");
find_collinear_normal(xs, pshA.pts, pshB.pts, m_precision);
cairo_set_line_width (cr, 0.3);
cairo_set_source_rgba (cr, 0.0, 0.0, 0.7, 1);
for (size_t i = 0; i < xs.size(); ++i)
{
Point At = A(xs[i].first);
Point Bu = B(xs[i].second);
draw_axis(cr, At, Bu);
draw_handle(cr, At);
draw_handle(cr, Bu);
}
cairo_stroke(cr);
double h_a_t = 0, h_b_t = 0;
double h_dist = hausdorfl( A, B, m_precision, &h_a_t, &h_b_t);
{
Point At = A(h_a_t);
Point Bu = B(h_b_t);
cairo_move_to(cr, At);
cairo_line_to(cr, Bu);
draw_handle(cr, At);
draw_handle(cr, Bu);
cairo_save(cr);
cairo_set_line_width (cr, 1);
cairo_set_source_rgba (cr, 0.7, 0.0, 0.0, 1);
cairo_stroke(cr);
cairo_restore(cr);
}
/*h_dist = hausdorf( A, B, m_precision, &h_a_t, &h_b_t);
{
Point At = A(h_a_t);
Point Bu = B(h_b_t);
draw_axis(cr, At, Bu);
draw_handle(cr, At);
draw_handle(cr, Bu);
cairo_save(cr);
cairo_set_line_width (cr, 0.3);
cairo_set_source_rgba (cr, 0.0, 0.7, 0.0, 1);
cairo_stroke(cr);
cairo_restore(cr);
}*/
*notify << "Hausdorf distance = " << h_dist
<< "occuring at " << h_a_t
<< " B=" << h_b_t << std::endl;
Toy::draw(cr, notify, width, height, save);
}
/** Return a Piecewise<D2<SBasis> > which points in the same direction as V_in, but has unit_length.
\param V_in the original path.
\param tol the maximum error allowed.
\param order the maximum degree to use for approximation
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)
\relates Piecewise, D2
*/
Piecewise<D2<SBasis> >
Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){
//TODO: Handle vanishing vectors...
// -This approach is numerically bad. Find a stable way to rescale V_in to have non vanishing ends.
// -This done, unitVector will have jumps at zeros: fill the gaps with arcs of circles.
D2<SBasis> V = RescaleForNonVanishingEnds(V_in);
if (V[0].isZero(tol) && V[1].isZero(tol))
return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis()));
SBasis x = V[0], y = V[1];
SBasis r_eqn1, r_eqn2;
Point v0 = unit_vector(V.at0());
Point v1 = unit_vector(V.at1());
SBasis a = SBasis(order+1, Linear(0.));
a[0] = Linear(-v0[1],-v1[1]);
SBasis b = SBasis(order+1, Linear(0.));
b[0] = 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.at0())*v0[0]-rr0/2*v0[1];
b0 = r0/dot(v0,V.at0())*v0[1]+rr0/2*v0[0];
a1 = r1/dot(v1,V.at1())*v1[0]-rr1/2*v1[1];
b1 = r1/dot(v1,V.at1())*v1[1]+rr1/2*v1[0];
a[k] = Linear(a0,a1);
b[k] = 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;
}
}
static void intersect_polish_root (D2<SBasis> const &A, double &s,
D2<SBasis> const &B, double &t) {
#ifdef HAVE_GSL
const gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *sol;
int status;
size_t iter = 0;
#endif
std::vector<Point> as, bs;
as = A.valueAndDerivatives(s, 2);
bs = B.valueAndDerivatives(t, 2);
Point F = as[0] - bs[0];
double best = dot(F, F);
for(int i = 0; i < 4; i++) {
/**
we want to solve
J*(x1 - x0) = f(x0)
|dA(s)[0] -dB(t)[0]| (X1 - X0) = A(s) - B(t)
|dA(s)[1] -dB(t)[1]|
**/
// We're using the standard transformation matricies, which is numerically rather poor. Much better to solve the equation using elimination.
Affine jack(as[1][0], as[1][1],
-bs[1][0], -bs[1][1],
0, 0);
Point soln = (F)*jack.inverse();
double ns = s - soln[0];
double nt = t - soln[1];
as = A.valueAndDerivatives(ns, 2);
bs = B.valueAndDerivatives(nt, 2);
F = as[0] - bs[0];
double trial = dot(F, F);
if (trial > best*0.1) {// we have standards, you know
// At this point we could do a line search
break;
}
best = trial;
s = ns;
t = nt;
}
#ifdef HAVE_GSL
const size_t n = 2;
struct rparams p = {A, B};
gsl_multiroot_function f = {&intersect_polish_f, n, &p};
double x_init[2] = {s, t};
gsl_vector *x = gsl_vector_alloc (n);
gsl_vector_set (x, 0, x_init[0]);
gsl_vector_set (x, 1, x_init[1]);
T = gsl_multiroot_fsolver_hybrids;
sol = gsl_multiroot_fsolver_alloc (T, 2);
gsl_multiroot_fsolver_set (sol, &f, x);
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (sol);
if (status) /* check if solver is stuck */
break;
status =
gsl_multiroot_test_residual (sol->f, 1e-12);
}
while (status == GSL_CONTINUE && iter < 1000);
s = gsl_vector_get (sol->x, 0);
t = gsl_vector_get (sol->x, 1);
gsl_multiroot_fsolver_free (sol);
gsl_vector_free (x);
#endif
{
// This code does a neighbourhood search for minor improvements.
double best_v = L1(A(s) - B(t));
//std::cout << "------\n" << best_v << std::endl;
Point best(s,t);
while (true) {
Point trial = best;
double trial_v = best_v;
for(int nsi = -1; nsi < 2; nsi++) {
for(int nti = -1; nti < 2; nti++) {
Point n(EpsilonBy(best[0], nsi),
EpsilonBy(best[1], nti));
double c = L1(A(n[0]) - B(n[1]));
//std::cout << c << "; ";
if (c < trial_v) {
trial = n;
trial_v = c;
}
}
}
if(trial == best) {
//std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
//std::cout << t << " -> " << t - best[1] << std::endl;
//std::cout << best_v << std::endl;
s = best[0];
t = best[1];
return;
} else {
best = trial;
best_v = trial_v;
}
}
}
}
