#include /* Last edited on 2015-01-20 16:48:08 by stolfilocal */ #include #include #include #include #include #include /*DEBUG #include GUBED*/ CONST /* Sqrt2 == 1.4142135623730950488e0; */ SqrtHalf == 0.7071067811865475244e0; /* SqrtPi == 1.77245385090551603e0; */ SqrtTwoPi == 2.50662827463100050e0; void pz_smooth_polygonal( double sigma, pz_r3_chain_t *p, pz_double_chain_t *t, double tPeriod, double tStart, pz_r3_chain_t *u, /* VAR v: pz_r3_chain_t; */ /* VAR w: pz_r3_chain_t; */ ) VAR R: double; { { /* with */ ??? m = (p.ne); ??? n = (u.ne); ??? tStep = tPeriod / ((double)n); /* do */ affirm((p.ne) == (t.ne) ); /* affirm((v.ne) == (u.ne) ); */ /* affirm((w.ne) == (u.ne) ); */ if (( tPeriod == 0.0e0 ) || ( m == 1 )){ { /* with */ ??? b = pz_r3_chain_sample_barycenter(p); /* do */ for (i = 0; i < (u.ne ) ; i++){ u[i] = b; /* v[i] = r3_t{0.0e0, ..}; */ /* w[i] = r3_t{0.0e0, ..}; */; }; }; return ; }; /*DEBUG fprintf(stderr, "delta:%s\n", FLR(delta,8,4) ); GUBED*/ /* Computes length of polynomial: */ R = r3_dist(p[m-1], p[0]); for (i = 1; i < (p.ne ) ; i++){ R = R + r3_dist(p[i], p[i-1]) ;}; /* Clear sample arrays: */ for (i = 0; i <= n-1 ; i++){ u[i] = p[0]; /* v[i] = r3_t{0.0e0, 0.0e0, 0.0e0}; */ /* w[i] = r3_t{0.0e0, 0.0e0, 0.0e0}; */; }; /* Filter and sample: */ SplatCircus( sigma, ta = t[0], pa = p[0], tz = t[0] + tPeriod, pz = p[0], p = SUBARRAY(p, 1, m-1), t = SUBARRAY(t, 1, m-1), R = R, tStart = tStart, tStep = tStep, u = u /* , v = v, w = w */ );; } } TYPE ValDiff = struct ??? { v /*, dv, ddv*/: double; } ???; void SplatCircus( double sigma, double ta, r3_t *pa, double tz, r3_t *pz, pz_r3_chain_t *p, pz_double_chain_t *t, double R, double tStart, double tStep, pz_r3_chain_t *u, /* VAR v: pz_r3_chain_t; */ /* VAR w: pz_r3_chain_t; */ ) /* Filters a piecewise linear function "p(t)" that is zero for "t <= ta" or "t >= tz", and whose graph has vertices "(ta,0)", "(t[0],d[0])", "(t[1],d[1])", ... "(t[N-1],d[N-1])", "(tz,0)", where "N == (t.ne) == (p.ne)", and "d[i]" is the difference between "p[i]" and the linear interpolation of "(ta,pa)" and "(tz,pz)". Evaluates the filtered function and its derivatives at equally spaced times "tStart + k*tStep", and adds those values to the vectors "u[k]", "v[k]", "w[k]". The procedure assumes that "d[i]" is at most "R". */ VAR dtMin: double; void DoSplatCircus( double ta, r3_t *pa, double tz, r3_t *pz, unsigned lo, unsigned hi ) /* Performs "SplatCircus" for the points "t[lo..hi], p[lo..hi]" */ { if (( (lo > hi) ) || ( (tz - ta < dtMin) )){ return ;}; { /* with */ ??? md = (lo+hi) DIV 2; ??? tm = t[md]; ??? pm = p[md]; ??? rm = pz_geo.LinearInterpolate(tm, ta, pa, tz, pz); ??? dm = r3_sub(pm, rm); /* do */ SplatTent(dm, ta, tm, tz, tStart, tStep, sigma, u /*, v, w */); if (( lo < md )){ DoSplatCircus(ta, pa, tm, pm, lo, md-1) ;}; if (( md < hi )){ DoSplatCircus(tm, pm, tz, pz, md+1, hi) ;};; } } /* DoSplatCircus */ { affirm((p.ne) == (t.ne) ); dtMin = 1.0e-4 * SqrtTwoPi * Math.sqrt(sigma)/R; DoSplatCircus(ta, pa, tz, pz, 0, (t.ne - 1)) } /* SplatCircus */ void SplatTent( r3_t *pi, double tm, double ti, double tp, double tStart, double tStep, double sigma, pz_r3_chain_t *u, /* VAR v: pz_r3_chain_t; */ /* VAR w: pz_r3_chain_t; */ ) { { /* with */ ??? fiveSigma = 5.0e0 * sigma; ??? n = (u.ne); ??? jMin = CEILING((tm - tStart - fiveSigma)/tStep); ??? jMax = FLOOR((tp - tStart + fiveSigma)/tStep); /* do */ for (j = jMin; j <= jMax ; j++){ { /* with */ ??? t = tStart + tStep * FLOAT(j,double); ??? ci = EvalSmoothedTent(t-ti, tm-ti, 0.0e0, tp-ti, sigma); ??? jm = j MOD n; uj = u[jm] /* , vj = v[jm], */ ; /* wj = w[jm] */ /* do */ for (k = 0; k <= 2 ; k++){ affirm(pi[k] + 1.0e0 > pi[k] ); uj[k] = uj[k] + pi[k] * ci.v; /* vj[k] = vj[k] + pi[k] * ci.dv; */ /* wj[k] = wj[k] + pi[k] * ci.ddv; */; };; };; }; } } /* SplatTent */ ValDiff EvalSmoothedTent( double t, double tm, double ti, double tp, double sigma ) /* Evaluates the tent function with graph "(tm,0) (ti,1) (tp,0)", filtered by a Gaussian of duration "sigma", and its derivatives, at time "t". */ { if (( tp <= tm )){ return ValDiff{0.0e0 /*, 0.0e0, 0.0e0 */} ;}; { /* with */ ??? w = tp - tm; /* Fix "ti" so that it does not coincide with "tm" or "tp": */ ??? eps = 1.0e-5 * w; ??? tiFix = MIN(MAX(ti, tm + eps), tp-eps); /* Now evaluate the filtered tent: */ ??? a = tiFix-tm; ??? b = tp-tiFix; ??? A = 1.0e0/a; ??? B = 1.0e0/b; ??? AB = A+B; ??? r0 = EvalSmoothedRamp(t-tm,sigma); ??? r1 = EvalSmoothedRamp(t-tiFix,sigma); ??? r2 = EvalSmoothedRamp(t-tp,sigma); v = A * r0.v - AB * r1.v + B * r2.v /* , */ /* dv = A * r0.dv - AB * r1.dv + B * r2.dv, */ ; /* ddv = A * r0.ddv - AB * r1.ddv + B * r2.ddv */ /* do */ affirm(v + 1.0e0 > v ); return ValDiff{v /*, dv, ddv */}; } } /* EvalSmoothedTent */ ValDiff EvalSmoothedRamp( double t, double sigma ) /* Evaluates the unit ramp function "max(0,x)", filtered by a Gaussian of duration "sigma", and its derivatives, at time "t". */ { { /* with */ ??? ts = t/sigma; ??? ts2 = ts*ts; ??? er1 = 1.0e0 + Math.erf(SqrtHalf*ts); ??? ga = Math.exp(-0.5e0*ts2); v = 0.5e0 * t * er1 + (sigma/SqrtTwoPi) * ga /* , dv = 0.5e0 * er1, */ ; /* ddv = ga/(sigma*SqrtTwoPi) */ /* do */ affirm(v + 1.0e0 > v ); return ValDiff{v /*, dv, ddv */}; } } /* EvalSmoothedRamp */ /*DEBUG char *FLR( double x, unsigned w, unsigned p ) { return Fmt.Pad(Fmt.LongReal(x, Fmt.Style.Fix, prec = p), w) } /* FLR */ GUBED*/ { /* Copyright © 2001 Universidade Estadual de Campinas (UNICAMP). Authors: Helena C. G. Leitão and Jorge Stolfi. This file can be freely distributed, used, and modified, provided that this copyright and authorship notice is preserved, and that any modified versions are clearly marked as such. This software has NO WARRANTY of correctness or applicability for any purpose. Neither the authors nor their employers chall be held responsible for any losses or damages that may result from its use. */