MODULE PZCurve; (* Last edited on 1999-09-27 00:28:50 by hcgl *) IMPORT LR2, LR3, PZLR3Chain, PZLRChain, PZProc, PZSmooth, PZGeo; FROM PZTypes IMPORT LONG, LONGS, INT, NAT; FROM Math IMPORT sqrt; REVEAL T = Public BRANDED OBJECT s: REF PZLRChain.T; (* s[i] is est. arclength between "p[i-1]" and "p[i]". *) a: REF LONGS; (* Work vector for "SetToFiltered" *) OVERRIDES setSamples := SetSamples; setLabels := SetLabels; setTimes := SetTimes; uniformize := Uniformize; setToFiltered := SetToFiltered; eval := Eval; sample := Sample; diff := Diff; copy := Copy; END; (* The vectors "c.d" and "c.s" are used internally to speed up some operations. The vector "d[i]" is the estimated (unit-length) direction of the curve at point "p[i]", and "s[i]" is the estimated arc length between "p[i-1]" and "p[i]", for "i IN [0..m-1]". At present, if "t[i]" is the time currently associated with "p[i]", then between times "t[i-1]" and "t[i]" the curve is assumed to have constant speed "h[i] = s[i]/(t[i]-t[i-1])". Therefore the arc length is supposed to vary linearly with time between consecutive samples. The curve's position at a time "t'" is estimated from the arclength "s'" by Hermite interpolation between "0 -> (p[i-1], h[i]*d[i-1])" and "s[i+1] -> (p[i],h[i]*d[i])" or even by linear interpolation between "p[i-1]" and "p[i]", depending on the context. Thus the position estimates are not fully consistent with the constant-speed assumtion nor with the arclengths "s[i]". Hopefully the discrepancies will have negligible effect if the samples are sufficiently dense. The directions "d[i]" are automatically computed so as to minimize the velocity variation along the whole curve. The label function "c.r(t)" is defined by periodic linear interpolation of the values "c.r[i]". *) PROCEDURE New(m: NAT): T = BEGIN WITH c = NEW(T, m := m, p := NEW(REF PZLR3Chain.T, m), t := NEW(REF PZLRChain.T, m), r := NEW(REF PZLRChain.T, m), d := NEW(REF PZLR3Chain.T, m), s := NEW(REF LONGS, m), a := NEW(REF LONGS, m), tPeriod := 1.0d0, rPeriod := 1.0d0 ), p = c.p^, t = c.t^, r = c.r^, d = c.d^, s = c.s^ DO FOR i := 0 TO m-1 DO p[i] := LR3.T{0.0d0, 0.0d0, 0.0d0}; t[i] := FLOAT(i,LONG)/FLOAT(m,LONG); r[i] := t[i]; d[i] := LR3.T{0.0d0, 0.0d0, 0.0d0}; s[i] := 0.0d0; END; RETURN c END END New; PROCEDURE Copy(c: T): T = BEGIN WITH m = c.m, d = NEW(T, m := m, p := NEW(REF PZLR3Chain.T, m), t := NEW(REF LONGS, m), r := NEW(REF PZLRChain.T, m), d := NEW(REF PZLR3Chain.T, m), s := NEW(REF LONGS, m), a := NEW(REF LONGS, m), tPeriod := c.tPeriod, rPeriod := c.rPeriod ) DO d.p^ := c.p^; d.t^ := c.t^; d.r^ := c.r^; d.d^ := c.d^; d.s^ := c.s^; RETURN d END END Copy; PROCEDURE ComputeArcParameters( READONLY pa, da: LR3.T; READONLY pb, db: LR3.T; VAR ctr: LR2.T; VAR hab: LR2.T; VAR wa, wb: LR2.T; ) = (* Given the endpoints "pa,pb" of a spline arc, and the unit tangent vectors "da,db" at its endpoints, computes the arc parameters "ctr, hab, wa, wb" needed by "EvalArcPos" and "EvalArcVel". *) BEGIN <* ASSERT pa[2] = 0.0d0 *> <* ASSERT pb[2] = 0.0d0 *> ctr := LR2.T{0.5d0*(pa[0] + pb[0]), 0.5d0*(pa[1] + pb[1])}; hab := LR2.T{0.5d0*(pb[0] - pa[0]), 0.5d0*(pb[1] - pa[1])}; IF hab[0] = 0.0d0 AND hab[1] = 0.0d0 THEN wa := LR2.T{0.0d0, 0.0d0}; wb := LR2.T{0.0d0, 0.0d0}; ELSE WITH u = LR2.Dir(hab), (* Directions "dra,drb" are "da,db" relative to "u" *) dra = LR2.T{da[0]*u[0] + da[1]*u[1], da[1]*u[0] - da[0]*u[1]}, drb = LR2.T{db[0]*u[0] + db[1]*u[1], db[1]*u[0] - db[0]*u[1]}, sumx = dra[0] + drb[0], difx = dra[0] - drb[0], sumy = dra[1] + drb[1], dify = dra[1] - drb[1], ux2 = (sumx*sumx + sumy*sumy)/4.0d0, uy2 = (difx*difx + dify*dify)/4.0d0, mag = (2.0d0*ux2 + 3.0d0*uy2)/(ux2+uy2 + 1.0d-50) DO wa := LR2.Scale(mag, LR2.T{dra[0], dra[1]}); wb := LR2.Scale(mag, LR2.T{drb[0], drb[1]}) END END END ComputeArcParameters; PROCEDURE EvalArcPos( READONLY ctr, hab: LR2.T; READONLY wa, wb: LR2.T; s: LONG; ): LR3.T = (* Given the parameters "ctr, hab, wa,wb" of an arc parametrized in "[-1..+1]", computes its position at parameter value "s". *) BEGIN WITH sumx = wa[0]+wb[0], difx = wb[0]-wa[0], kx = 0.125d0 * ((((sumx - 4.0d0)*s + difx)*s - sumx + 12.0d0)*s - difx), sumy = wa[1]+wb[1], dify = wb[1]-wa[1], ky = 0.125d0 * (((sumy * s + dify)*s - sumy)*s - dify), xt = ctr[0] + kx*hab[0] - ky*hab[1], yt = ctr[1] + kx*hab[1] + ky*hab[0] DO RETURN LR3.T{xt, yt, 0.0d0} END END EvalArcPos; PROCEDURE EvalArcVel( <*UNUSED*> READONLY ctr: LR2.T; READONLY hab: LR2.T; READONLY wa, wb: LR2.T; s: LONG; ): LR3.T = (* Given the parameters "ctr, hab, wa,wb" of an arc parametrized in "[-1..+1]", computes the derivative "dp/ds" of its position "p(s)" rel to "s", at parameter value "s". *) BEGIN WITH sumx = wa[0]+wb[0], difx = wb[0]-wa[0], dkx = 0.125d0 * (((3.0d0*sumx - 12.0d0)*s + 2.0d0*difx)*s - sumx + 12.0d0), sumy = wa[1]+wb[1], dify = wb[1]-wa[1], dky = 0.125d0 * (((3.0d0*sumy)*s + 2.0d0*dify)*s - sumy), dxs = dkx*hab[0] - dky*hab[1], dys = dkx*hab[1] + dky*hab[0] DO RETURN LR3.T{dxs, dys, 0.0d0} END END EvalArcVel; <*UNUSED*> PROCEDURE EvalMidArcVel( <*UNUSED*> READONLY ctr: LR2.T; READONLY hab: LR2.T; READONLY wa, wb: LR2.T; ): LR3.T = (* Same as "EvalArcVel(ctr, hab, wa,wb, 0)" but a bit faster. *) BEGIN WITH sumx = wa[0]+wb[0], dkx = 0.125d0 * (12.0d0 - sumx), sumy = wa[1]+wb[1], dky = 0.125d0 * (- sumy), dxt = dkx*hab[0] - dky*hab[1], dyt = dkx*hab[1] + dky*hab[0] DO RETURN LR3.T{dxt, dyt, 0.0d0} END END EvalMidArcVel; PROCEDURE ComputeArcLength( <*UNUSED*> READONLY ctr: LR2.T; READONLY hab: LR2.T; READONLY wa, wb: LR2.T; ): LONG = (* Estimates the length of the Hermite curve with the given paramteres. *) (* Estimates the length of the arc with the given Arc parameters. *) BEGIN WITH (* Compute the derivative of the Hermite interpolant. *) (* Let the Hermite interpolant be "h(u) = h3*u^3 + ... + h0", where "u" ranges from -1 to +1. Note that "dh/du(-1) = wa/2", and "dh/du(1) = wb/2". The polynomial "h", relative to the "hab" frame, is then | h(u) = | | + (-1,0)*(+u^3 - 3*u + 2)/4 | + (+1,0)*(-u^3 + 3*u + 2)/4 | + wa*(u^3 - u^2 - u + 1)/8 | + wb*(u^3 + u^2 - u - 1)/8 | = | + (+ 1/8*(wa+wb) - (1/2,0))*u^3 | + (+ 1/8*(wb-wa) )*u^2 | + (- 1/8*(wa+wb) + (3/2,0))*u | + (- 1/8*(wb-wa) ) Its derivative with respect to "u" is then | dh/du(u) = | + (+ 3/8*(wa+wb) - (3/2,0)) * u^2 | + (+ 1/4*(wb-wa) ) * u | + (- 1/8*(wa+wb) + (3/2,0)) *) dxdu_2 = -1.5d0 + 0.375d0*(wa[0]+wb[0]), dydu_2 = + 0.375d0*(wa[1]+wb[1]), dxdu_1 = 0.250d0*(wb[0] - wa[0]), dydu_1 = 0.250d0*(wb[1] - wa[1]), dxdu_0 = +1.5d0 - 0.125d0*(wa[0]+wb[0]), dydu_0 = - 0.125d0*(wa[1]+wb[1]), (* Computes the speed squared "|dh/du|^2", as a function of "u". *) (* We only need the even powers: *) s_4 = dxdu_2*dxdu_2 + dydu_2*dydu_2, s_2 = 2.0d0 * (dxdu_2*dxdu_0 + dydu_2*dydu_0) + (dxdu_1*dxdu_1 + dydu_1*dydu_1), s_0 = dxdu_0*dxdu_0 + dydu_0*dydu_0, (* Computes the approximate square root of the polynomial. *) (* Assumes the constant term "s0" dominates, so *) (* "sqrt(U + s_0) ~ sqrt(s_0) + U/sqrt(s_0)/2" *) r_0 = sqrt(s_0 + 1.0d-50), r_2 = 0.5d0 * s_2/r_0, r_4 = 0.5d0 * s_4/r_0, (* Integrates the speed from -1 to +1 to get the length: *) length = 2.0d0*(r_4/5.0d0 + r_2/3.0d0 + r_0) DO RETURN LR2.Norm(hab)*length END END ComputeArcLength; PROCEDURE Diff(c: T): T = BEGIN WITH m = c.m, ct = c.t^, cp = c.p^, cr = c.r^, d = New(m), dt = d.t^, dp = d.p^, dr = d.r^ DO IF m <= 2 THEN FOR i := 0 TO m-1 DO dp[i] := LR3.T{0.0d0, 0.0d0, 0.0d0} END; ELSE FOR io := 0 TO m-1 DO WITH im = (io-1) MOD m, tm = ct[im] + FLOAT((io-1) DIV m, LONG) * c.tPeriod, to = ct[io], ip = (io+1) MOD m, tp = ct[ip] + FLOAT((io+1) DIV m, LONG) * c.tPeriod, vel = PZGeo.EstimateVelocityC(tm, cp[im], to, cp[io], tp, cp[ip]) DO dp[io] := LR3.T{vel[0], vel[1], 0.0d0}; <* ASSERT dp[io][0] + 1.0d0 > dp[io][0] *> END END; END; FOR i := 0 TO m-1 DO dt[i] := ct[i]; dr[i] := cr[i] END; d.tPeriod := c.tPeriod; d.rPeriod := c.rPeriod; RecomputeDirections(d); RecomputeLengths(d); RETURN d END END Diff; (* ----------------------------------- Old Diff PROCEDURE Diff(c: T): T = VAR ctr, hab, wa, wb: LR2.T; BEGIN WITH ct = c.t^, cp = c.p^, cr = c.r^, cd = c.d^, d = New(c.m), dt = d.t^, dp = d.p^, dr = d.r^ DO d.tPeriod := c.tPeriod; d.rPeriod := c.rPeriod; FOR i := 0 TO LAST(cp) DO WITH ia = (i - 1) MOD c.m, qa = FLOAT(ia DIV c.m, LONG), pa = cp[ia], da = cd[ia], ta = ct[ia] + c.tPeriod * qa, ra = cr[ia] + c.rPeriod * qa, ib = i, pb = cp[ib], db = cd[ib], tb = ct[ib], rb = cr[ib] DO ComputeArcParameters(pa, da, pb, db, ctr, hab, wa, wb); dt[i] := 0.5d0 * (ta + tb); dp[i] := EvalMidArcVel(ctr, hab, wa, wb); dr[i] := 0.5d0 * (ra + rb); END END; RecomputeDirections(d); RecomputeLengths(d); RETURN d END END Diff; -----------------------------------------------------------*) PROCEDURE SetSamples(c: T; READONLY p: PZLR3Chain.T) = BEGIN <* ASSERT NUMBER(p) = c.m *> c.p^ := p; RecomputeDirections(c); RecomputeLengths(c); END SetSamples; PROCEDURE SetTimes(c: T; READONLY t: PZLRChain.T; tPeriod: LONG) = BEGIN <* ASSERT NUMBER(t) = c.m *> c.t^ := t; c.tPeriod := tPeriod; END SetTimes; PROCEDURE SetLabels(c: T; READONLY r: PZLRChain.T; rPeriod: LONG) = BEGIN <* ASSERT NUMBER(r) = c.m *> c.r^ := r; c.rPeriod := rPeriod; END SetLabels; PROCEDURE RecomputeDirections(c: T) = (* Recomputes the directions "c.d[i]" from the samples "c.p[i]". Does not use the times "c.t", the lengths "c.s", or the labels "c.r". *) VAR im, ip: NAT; BEGIN WITH m = c.m, p = c.p^, d = c.d^ DO IF m <= 2 THEN FOR i := 0 TO m-1 DO d[i] := LR3.T{0.0d0, 0.0d0, 0.0d0} END; ELSE im := m-1; FOR io := 0 TO m-1 DO ip := (io+1) MOD m; WITH tm = LR3.Dist(p[im], p[io]) + 1.0d-50, tp = LR3.Dist(p[io], p[ip]) + 1.0d-50, vel = PZGeo.EstimateVelocityC(-tm, p[im], 0.0d0, p[io], +tp, p[ip]) DO IF vel[0] = 0.0d0 AND vel[1] = 0.0d0 THEN d[io] := LR3.T{0.0d0, 0.0d0, 0.0d0} ELSE d[io] := LR3.Dir(LR3.T{vel[0], vel[1], 0.0d0}); END; <* ASSERT d[io][0] + 1.0d0 > d[io][0] *> END; im := io; END; END END END RecomputeDirections; PROCEDURE RecomputeLengths(c: T) = (* Recomputes the estimated arclength "s[i]" between points "p[i-1]" and "p[i]". Uses the points "c.p" and the estimated direction "c.s"; does not use the times "c.t" or the labels "c.r". *) VAR ia: NAT; VAR ctr, hab, wa, wb: LR2.T; BEGIN WITH m = c.m, p = c.p^, d = c.d^, s = c.s^ DO IF m <= 1 THEN FOR i := 0 TO m-1 DO s[0] := 0.0d0 END; ELSE ia := m-1; FOR ib := 0 TO m-1 DO ComputeArcParameters(p[ia], d[ia], p[ib], d[ib], ctr, hab, wa, wb); WITH si = ComputeArcLength(ctr, hab, wa, wb) DO s[ib] := si; ia := ib END END END END END RecomputeLengths; PROCEDURE Uniformize(c: T) = VAR totLen: LONG; BEGIN WITH m = c.m, s = c.s^, t = c.t^ DO totLen := 0.0d0; FOR i := 0 TO m-1 DO t[i] := totLen; totLen := totLen + s[i]; END; c.tPeriod := totLen (* No need to recompute lengths since they don't depend on times. *) END END Uniformize; PROCEDURE SetToFiltered(c: T; d: T; sigma: LONG) = BEGIN WITH ct = c.t^, m = c.m, T = d.tPeriod, arg = c.a^ DO (* Temporary hack: filters the *linear* interpolation of the points "p[i]". *) PZSmooth.Polygonal(sigma, d.p^, d.t^, d.tPeriod, 0.0d0, c.p^); (* "PZSmooth" will resample at "m" uniform times in "[0..T]": *) FOR i := 0 TO m-1 DO ct[i] := T * FLOAT(i, LONG)/FLOAT(m, LONG) END; c.tPeriod := d.tPeriod; (* Recompute the velocities and lengths: *) RecomputeDirections(c); RecomputeLengths(c); (* Compute the labels of the sampled points: *) PZLRChain.ArgsFromValues(d.t^, d.tPeriod, ct, arg); PZLRChain.ValuesFromArgs(d.r^, d.rPeriod, arg, c.r^); c.rPeriod := d.rPeriod; END; END SetToFiltered; PROCEDURE Eval(c: T; t: LONG; VAR p, v: LR3.T; VAR r: LONG) = VAR ia, ib: NAT; k: INT; VAR ctr, hab, wa, wb: LR2.T; (* Computed for arc "c.p[ih-1]" .. "c.p[ih]" *) BEGIN WITH cm = c.m, cp = c.p^, cd = c.d^, ct = c.t^, cr = c.r^, tLo = ct[0], tHi = tLo + c.tPeriod DO <* ASSERT NUMBER(ct) = cm *> (* Find consecutive times "ct[ia]", "ct[ib]" that bracket "t" *) ia := 0; ib := 1; (* Invariant: "ia IN [0..cm-1]", "ib = ia+1" *) PZProc.ReduceToPeriod(t, tLo, tHi, k); (* Should do binary search ! *) WHILE t < ct[ia] DO ib := ia; DEC(ia) END; WHILE ib < cm AND t > ct[ib] DO ia := ib; INC(ib) END; WITH ta = ct[ia], tb = ct[ib MOD cm] + c.tPeriod * FLOAT(ib DIV cm, LONG), ra = cr[ia], rb = cr[ib MOD cm] + c.rPeriod * FLOAT(ib DIV cm, LONG) DO <* ASSERT ta <= t AND t <= tb *> ComputeArcParameters( cp[ia], cd[ia], cp[ib MOD cm], cd[ib MOD cm], ctr, hab, wa, wb ); WITH sh = (t - ta)/(tb - ta), s = 2.0d0*sh - 1.0d0 DO p := EvalArcPos(ctr, hab, wa, wb, s); v := EvalArcVel(ctr, hab, wa, wb, s); r := ra + sh * (rb - ra) + FLOAT(k,LONG) * c.rPeriod END; END; END END Eval; PROCEDURE Sample( c: T; READONLY t: PZLRChain.T; VAR p, d: PZLR3Chain.T; VAR r: PZLRChain.T; ) = VAR ia, ib, ih: NAT; k: INT; tau: LONG; VAR ctr, hab, wa, wb: LR2.T; (* Computed for arc "c.p[ih-1]" .. "c.p[ih]" *) BEGIN WITH cm = c.m, cp = c.p^, cd = c.d^, ct = c.t^, cr = c.r^, n = NUMBER(p), tLo = ct[0], tHi = tLo + c.tPeriod DO <* ASSERT NUMBER(ct) = cm *> <* ASSERT NUMBER(t) >= n *> ia := 0; ib := 1; ih := LAST(NAT); FOR j := 0 TO n-1 DO (* Invariant: "ia IN [0..cm-1]", "ib = ia+1" *) tau := t[j]; PZProc.ReduceToPeriod(tau, tLo, tHi, k); WHILE tau < ct[ia] DO ib := ia; DEC(ia) END; WHILE ib < cm AND tau > ct[ib] DO ia := ib; INC(ib) END; WITH ta = ct[ia], tb = ct[ib MOD cm] + c.tPeriod * FLOAT(ib DIV cm, LONG), ra = cr[ia], rb = cr[ib MOD cm] + c.rPeriod * FLOAT(ib DIV cm, LONG) DO <* ASSERT ta <= tau AND tau <= tb *> IF ib # ih THEN ComputeArcParameters( cp[ia], cd[ia], cp[ib MOD cm], cd[ib MOD cm], ctr, hab, wa, wb ); ih := ib END; WITH sh = (tau - ta)/(tb - ta), s = 2.0d0*sh - 1.0d0 DO p[j] := EvalArcPos(ctr, hab, wa, wb, s); d[j] := EvalArcVel(ctr, hab, wa, wb, s); r[j] := ra + sh * (rb - ra) + FLOAT(k,LONG) * c.rPeriod END; END; END; END END Sample; BEGIN END PZCurve. (* 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. *)