/* Last edited on 2025-01-04 23:55:56 by stolfi */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include pz_r3_chain_t pz_r3_chain_from_int_chain( pz_i2_chain_t *c ) { int n = c->nel; pz_r3_chain_t p = r3_vec_new(n); int i; for (i = 0; i < n; i++) { i2_t *ci = &(c->el[i]); r3_t *pi = &(p.el[i]); pi->c[0] = ((double)ci->c[0]); pi->c[1] = ((double)ci->c[1]); pi->c[2] = 0.0; } return p; } pz_i2_chain_t pz_r3_chain_to_int_chain( pz_r3_chain_t *p ) { int n = p->nel; pz_i2_chain_t c = i2_vec_new(n); int i; for (i = 0; i < n ; i++) { i2_t *ci = &(c.el[i]); r3_t *pi = &(p->el[i]); ci->c[0] = pz_round(pi->c[0]); ci->c[1] = pz_round(pi->c[1]); } return c; } pz_r3_chain_t pz_r3_chain_from_line_segment ( r3_t *p, r3_t *q, unsigned n ) { r3_t delta; r3_sub(q, p, &delta); r3_scale(1.0/((double)n-1), &delta, &delta); pz_r3_chain_t c = r3_vec_new(n); int i; for (i = 0; i <= n-1 ; i++) { double f = (double)i; r3_t *ci = &(c.el[i]); ci->c[0] = delta.c[0] * f + p->c[0]; ci->c[1] = delta.c[1] * f + p->c[1]; ci->c[2] = delta.c[2] * f + p->c[2]; } return c; } pz_r3_chain_t pz_r3_chain_cut ( pz_r3_chain_t *c, unsigned start, unsigned length ) { pz_r3_chain_t x = r3_vec_new(length); pz_r3_chain_do_cut(c, start, length, &x); return x; } void pz_r3_chain_do_cut ( pz_r3_chain_t *c, unsigned start, unsigned length, pz_r3_chain_t *x ) { int n = c->nel; int ini = (start % n); int i, j; for (i= 0, j = ini; i < length; i++, j++) { if (j > n) { j -= n; } x->el[i] = c->el[j]; } } void pz_r3_chain_reverse ( pz_r3_chain_t *c ) { r3_t aux; int M = c->nel/2, i, j; for (i = 0, j = c->nel-1; i < M; i++, j--) { aux = c->el[i]; c->el[i] = c->el[j]; c->el[j] = aux; } } pz_r3_chain_t pz_r3_chain_extract_segment ( pz_r3_chain_t *c, pz_segment_t s ) { affirm(c->nel == s.tot, "wrong tot size"); pz_r3_chain_t t = r3_vec_new(s.ns); pz_r3_chain_do_extract_segment(c, s, &t); return t; } void pz_r3_chain_do_extract_segment ( pz_r3_chain_t *c, pz_segment_t s, pz_r3_chain_t *t ) { affirm(c->nel == s.tot, "wrong tot size"); pz_r3_chain_do_cut(c, s.ini, s.ns, t); if (s.rev) { pz_r3_chain_reverse(t); } } pz_window_t pz_r3_chain_get_window ( pz_r3_chain_t *c ) { int n = c->nel; int i, k; pz_window_t w; for (k = 0; k <= 2 ; k++) { w.r[k].end[0] = +INFINITY; w.r[k].end[1] = -INFINITY; } for (i = 0; i < n; i++) { r3_t *ci = &(c->el[i]); for (k = 0; k <= 2 ; k++) { double cik = ci->c[k]; double *lo = &(w.r[k].end[0]), *hi = &(w.r[k].end[1]); if (cik < *lo) { *lo = cik; } if (cik > *hi) { *hi = cik; } } } return w; } r3_t pz_r3_chain_sample_barycenter ( pz_r3_chain_t *c ) { int n = c->nel; r3_t p = (r3_t){{0.0, 0.0, 0.0}}; int i; for (i = 0; i < n ; i++) { r3_t *ci = &(c->el[i]); p.c[0] += ci->c[0]; p.c[1] += ci->c[1]; p.c[2] += ci->c[2]; } double fn = n; p.c[0] /= fn; p.c[1] /= fn; p.c[2] /= fn; return p; } r3_t pz_r3_chain_area_barycenter ( pz_r3_chain_t *c ) { int n = c->nel; double totArea = 0.0; /* Twice the area, actually. */ unsigned iPrev = n-1; r3_t p = (r3_t){{0.0, 0.0, 0.0}}; int i; for (i = 0; i < n ; i++) { r3_t *ai = &(c->el[iPrev]); r3_t *bi = &(c->el[i]); double area = ai->c[0]*bi->c[1] - ai->c[1]*bi->c[0]; /* 2× triang area. */ p.c[0] += area*(ai->c[0] + bi->c[0]); /* Postpone the "/ 3". */ p.c[1] += area*(ai->c[1] + bi->c[1]); /* Postpone the "/ 3". */ totArea += area; iPrev = i; } if (totArea == 0.0) { p = pz_r3_chain_sample_barycenter(c); } else { totArea *= 3; p.c[0] /= totArea; p.c[1] /= totArea; } p.c[2] = 0.0; return p; } r3_t pz_r3_chain_center ( pz_r3_chain_t *c, pz_ctr_t ctrType ) { switch(ctrType) { case pz_ctr_NONE: return (r3_t){{0,0,0}}; case pz_ctr_AREA: return pz_r3_chain_area_barycenter(c); case pz_ctr_SMPS: return pz_r3_chain_sample_barycenter(c); case pz_ctr_ENDS: { r3_t *p = &(c->el[0]); r3_t *q = &(c->el[c->nel-1]); r3_t r; int i; for (i = 0; i < 3; i++) { r.c[i] = (p->c[i] + q->c[i])/2; } return r; } default: affirm(FALSE, "bad center type"); return (r3_t){{0,0,0}}; } } void pz_r3_chain_recenter ( pz_r3_chain_t *c, pz_ctr_t ctrType ) { if (ctrType != pz_ctr_NONE) { r3_t bar = pz_r3_chain_center(c, ctrType); r3_neg(&bar, &bar); pz_r3_chain_do_translate(c, bar); } } pz_r3_chain_t pz_r3_chain_translate ( pz_r3_chain_t *c, r3_t t ) { int n = c->nel; pz_r3_chain_t d = r3_vec_new(n); int i; for (i = 0; i < n ; i++) { d.el[i] = c->el[i]; } pz_r3_chain_do_translate(&d, t); return d; } void pz_r3_chain_do_translate ( pz_r3_chain_t *c, r3_t t ) { int n = c->nel; int i; for (i = 0; i < n ; i++) { r3_t *ci = &(c->el[i]); ci->c[0] += t.c[0]; ci->c[1] += t.c[1]; ci->c[2] += t.c[2]; } } pz_r3_chain_t pz_r3_chain_map ( pz_r3_chain_t *c, hr3_pmap_t *M ) { int n = c->nel; pz_r3_chain_t d = r3_vec_new(n); int i; for (i = 0; i < n ; i++) { d.el[i] = c->el[i]; } pz_r3_chain_do_map(&d, M); return d; } void pz_r3_chain_do_map ( pz_r3_chain_t *c, hr3_pmap_t *M ) { int n = c->nel; int i; for (i = 0; i < n ; i++) { r3_t *ci = &(c->el[i]); r4_t u = (r4_t){{1.0, ci->c[0], ci->c[1], ci->c[2]}}; r4_t v; r4x4_map_row(&u, &(M->dir), &v); *ci = (r3_t){{v.c[1]/v.c[0], v.c[2]/v.c[0], v.c[3]/v.c[0]}}; } } r3_t pz_r3_chain_general_dir( pz_r3_chain_t *c, double pos ) { int nc = c->nel; int ci = (int)(pos*nc + 0.5); int cf = (int)((1-pos)*nc + 0.5); int mc = (nc - 1)/2; if (ci == cf) { ci = mc-1; cf = mc+1; } ci = imax(0, imin(ci, nc - 1)); cf = imax(0, imin(cf, nc - 1)); r3_t *p = &(c->el[ci]); r3_t *q = &(c->el[cf]); r3_t u = pz_seg_dir(p, q); return u; } hr3_pmap_t pz_r3_chain_alignment_matrix ( pz_r3_chain_t *a, pz_r3_chain_t *b, pz_ctr_t ctrType, double pos ) { r3_t aDir = pz_r3_chain_general_dir(a, pos); r3_t aCtr = pz_r3_chain_center(a, ctrType); r3_t bDir = pz_r3_chain_general_dir(b, pos); r3_t bCtr = pz_r3_chain_center(b, ctrType); r3_t t; r3_sub(&bCtr, &aCtr, &t); hr3_pmap_t T = hr3_pmap_translation(&t); hr3_pmap_t R = hr3_pmap_u_to_v_rotation(&aDir, &bDir); hr3_pmap_t M = hr3_pmap_compose(&T, &R); return M; } void pz_r3_chain_untilt ( pz_r3_chain_t *c, pz_ctr_t ctrType, double pos ) { r3_t trn = pz_r3_chain_center(c, ctrType); r3_neg(&trn, &trn); r3_t dir = pz_r3_chain_general_dir(c, pos); r3_t ref = (r3_t){{1,0,0}}; hr3_pmap_t T = hr3_pmap_translation(&trn); hr3_pmap_t R = hr3_pmap_u_to_v_rotation(&dir, &ref); hr3_pmap_t M = hr3_pmap_compose(&T, &R); pz_r3_chain_do_map(c, &M); } double pz_r3_chain_linear_lengths ( pz_r3_chain_t *p, /* {p[i]} is chain position at time {t[i]}. */ pz_double_chain_t *s /* {s[j]} is the length from {p[0]} to {p[j]}. */ ) { double length = 0.0; int n = p->nel; int i; affirm((s->nel) == n, "wrong size"); s->el[0] = 0.0; for (i = 1; i < n; i++) { length += r3_dist(&(p->el[i]), &(p->el[i-1])); s->el[i] = length; } return length + r3_dist(&(p->el[n-1]), &(p->el[0])); } void pz_r3_chain_linear_uniform_sample ( pz_double_chain_t *t, double tPeriod, pz_r3_chain_t *p, double tStart, pz_r3_chain_t *q ) { unsigned ib; double ta, tb; int k; int np = p->nel; int nq = q->nel; double tLo = t->el[0], tHi = t->el[0] + tPeriod; affirm(t->nel == np, "wrong size"); pz_reduce_to_period(&tStart, tLo, tHi, &k); ta = t->el[0]; ib = 1; tb = t->el[1]; int j; for (j = 0; j < nq; j++) { double tau = tStart + tPeriod * (((double)j) / ((double)nq)); affirm(tau >= t->el[ib-1], "interp bug"); affirm(tau < tHi, "interp bug"); while ((ib < np) && (tau > t->el[ib])) { ta = tb; ib = ib+1; tb = (ib == np ? tHi : t->el[ib]); } q->el[j] = pz_linear_interpolate( tau, ta, &(p->el[ib-1]), tb, &(p->el[ib % np]) ); } } void pz_r3_chain_linear_equidistant_sample ( pz_r3_chain_t *p, double tStart, pz_r3_chain_t *q ) { int np = p->nel; pz_double_chain_t t = double_vec_new(np); double length = pz_r3_chain_linear_lengths(p, &t); pz_r3_chain_linear_uniform_sample(&t, length, p, tStart*length, q); } void pz_r3_chain_linear_time_sample ( pz_double_chain_t *t, double tPeriod, pz_r3_chain_t *p, pz_double_chain_t *r, pz_r3_chain_t *q ) { unsigned ia, ib; int i, k; double ta, tb; int np = p->nel; int nq = q->nel; double tLo = t->el[0]; double tHi = t->el[0] + tPeriod; affirm(t->nel == np, "wrong size"); affirm(r->nel == nq, "wrong size"); ia = 0; ta = t->el[0]; ib = 1; tb = t->el[1]; for (i = 0; i <= nq-1 ; i++) { /* Invariant: {ia IN [0..np-1]}, {ib == ia+1} */ double tau = r->el[i]; pz_reduce_to_period(&tau, tLo, tHi, &k); affirm((tLo <= tau) && (tau < tHi), "bug"); while (tau < t->el[ia]){ ib = ia; ia--; } while ((ib < np) && (tau > t->el[ib])) { ia = ib; ib++; } double ta = t->el[ia]; double tb = t->el[ib % np] + tPeriod * ((double)ib / np); q->el[i] = pz_linear_interpolate(tau, ta, &(p->el[ia % np]), tb, &(p->el[ib % np])); } } void pz_r3_chain_hermite_uniform_sample ( pz_double_chain_t *t, double tPeriod, pz_r3_chain_t *p, pz_r3_chain_t *dp, double tStart, pz_r3_chain_t *q, pz_r3_chain_t *dq ) { unsigned ib; double ta, tb; int i, k; int np = p->nel; int nq = q->nel; double tLo = t->el[0]; double tHi = tLo + tPeriod; affirm(dp->nel == np, "dp wrong size"); affirm(t->nel == np, "t wrong size"); pz_reduce_to_period(&tStart, tLo, tHi, &k); ta = t->el[0]; ib = 1; tb = t->el[1]; for (i = 0; i < nq; i++) { double tau = tStart + tPeriod * (((double)i) / ((double)nq)); affirm(tau >= t->el[ib-1], "bug"); affirm(tau < tHi, "bug"); while ((ib < np) && (tau > t->el[ib])) { ta = tb; ib = ib + 1; if (ib == np) { tb = tHi; } else { tb = t->el[ib]; } } affirm((tau >= ta) && (tau <= tb), "bug"); pz_hermite_interpolate( tau, ta, &(p->el[ib-1]), &(dp->el[ib-1]), tb, &(p->el[ib % np]), &(dp->el[ib % np]), &(q->el[i]), &(dq->el[i]) ); } } void pz_r3_chain_hermite_equidistant_sample ( pz_r3_chain_t *p, double tStart, pz_r3_chain_t *q, pz_r3_chain_t *dq ) { int np = p->nel; int i; if (np <= 1) { for (i = 0; i < q->nel; i++) { q->el[i] = p->el[0]; dq->el[i] = (r3_t){{0.0, 0.0, 0.0}}; } return; } pz_double_chain_t t = double_vec_new(np); double length = pz_r3_chain_linear_lengths(p, &t); pz_r3_chain_t dp = r3_vec_new(np); pz_r3_chain_estimate_velocities(&t, length, p, &dp, pz_estimate_velocity_L); pz_r3_chain_hermite_uniform_sample(&t, length, p, &dp, tStart*length, q, dq); } void pz_r3_chain_hermite_time_sample ( pz_double_chain_t *t, double tPeriod, pz_r3_chain_t *p, pz_r3_chain_t *dp, pz_double_chain_t *r, pz_r3_chain_t *q, pz_r3_chain_t *dq ) { unsigned ia, ib; int i, k; int np = p->nel; int nq = q->nel; double tLo = t->el[0]; double tHi = tLo + tPeriod; affirm(t->nel == np, "t wrong size"); affirm(r->nel >= nq, "r wrong size"); ia = 0; ib = 1; for (i = 0; i < nq; i++) { /* Invariant: {ia IN [0..np-1]}, {ib == ia+1} */ double tau = r->el[i]; pz_reduce_to_period(&tau, tLo, tHi, &k); while (tau < t->el[ia]) { ib = ia; ia--; } while ((ib < np) && (tau > t->el[ib])) { ia = ib; ib++; } double ta = t->el[ia]; double tb = t->el[ib % np] + tPeriod * ((double)ib / np); affirm((ta <= tau) && (tau <= tb), "bug"); pz_hermite_interpolate( tau, ta, &(p->el[ia]), &(dp->el[ia]), tb, &(p->el[(ia+1) % np]), &(dp->el[(ia+1) % np]), &(q->el[i]), &(dq->el[i]) ); } } void pz_r3_chain_estimate_velocities ( pz_double_chain_t *t, double tPeriod, pz_r3_chain_t *p, pz_r3_chain_t *dp, pz_r3_chain_vel_estimator_t est ) { int np = p->nel; affirm(dp->nel == np, "wrong size"); affirm(t->nel == np, "wrong size"); int i; if (np <= 2) { for (i = 0; i < np; i++) { dp->el[i] = (r3_t){{0.0, 0.0, 0.0}}; } } else { /* Estimate the velocities by {L} method: */ unsigned ka = np-2, kb = np-1; double sa = t->el[np-2] - tPeriod, sb = t->el[np-1] - tPeriod; int i; for (i = 0; i <= np-1 ; i++) { dp->el[kb] = est(sa, &(p->el[ka]), sb, &(p->el[kb]), t->el[i], &(p->el[i])); ka = kb; kb = i; sa = sb; sb = t->el[i]; } } } double pz_r3_chain_adjust_unit ( double givenUnit, pz_r3_chain_t *c ) { double maxBig = 0.0, minDev = INFINITY; int n = c->nel; int i, k; for (k = 0; k < 3; k++) { double avg = 0.0, dev = 0.0, big = 0.0; /* Compute average and max-abs value: */ if (n > 0) { for (i = 0; i < n; i++) { r3_t *ci = &(c->el[i]); avg = avg + ci->c[k]; big = fmax(big, fabs(ci->c[k])); } avg = avg/(double)n; } maxBig = fmax(maxBig, big); /* Compute standard deviation: */ if (n > 1) { for (i = 0; i < n ; i++) { double d = c->el[i].c[k] - avg; dev += d*d; } dev = sqrt(dev/((double)(n-1))); if (dev > 1.0e-10 * big){ minDev = fmin(minDev, dev); } } } if (minDev > maxBig){ minDev = maxBig; } return pz_adjust_unit(givenUnit, minDev, maxBig); } #define pz_r3_chain_file_version "2004-04-30" void pz_r3_chain_write ( FILE *wr, char *cmt, pz_r3_chain_t *c, double unit ) { auto int to_int ( double x ); int to_int ( double x ) { affirm(x > (double)MININT, "overflow"); affirm(x < (double)MAXINT, "overflow"); return pz_round(x); } int n = c->nel; filefmt_write_header(wr, "pz_r3_chain_t", pz_r3_chain_file_version); int ind = 0; /* Comment indentation. */ filefmt_write_comment(wr, cmt, ind, '|'); fprintf(wr, "unit = %g\n", unit); fprintf(wr, "samples = %d\n", n); int i; for (i = 0; i < n; i++) { r3_t *ci = &(c->el[i]); fprintf(wr, "%d ", to_int(ci->c[0] / unit)); fprintf(wr, "%d ", to_int(ci->c[1] / unit)); fprintf(wr, "%d\n", to_int(ci->c[2] / unit)); } filefmt_write_footer(wr, "pz_r3_chain_t"); fflush(wr); } pz_r3_chain_read_data_t pz_r3_chain_read ( FILE *rd, bool_t header_only, pz_ctr_t recenter ) { auto double pz_r3_chain_to_double ( int n ); double pz_r3_chain_to_double ( int n ) { affirm(n > MININT, "overflow"); affirm(n < MAXINT, "overflow"); return (double)n; } pz_r3_chain_read_data_t d; filefmt_read_header(rd, "pz_r3_chain_t", pz_r3_chain_file_version); d.cmt = filefmt_read_comment(rd, '|'); d.unit = nget_double(rd, "unit"); fget_skip_formatting_chars(rd); d.samples = nget_int32(rd, "samples"); int n = d.samples; double unit = d.unit; if (header_only) { d.c = r3_vec_new(0); } else { d.c = r3_vec_new(n); int i; for (i = 0; i < n; i++) { r3_t *ci = &(d.c.el[i]); fget_skip_formatting_chars(rd); ci->c[0] = pz_r3_chain_to_double(fget_int32(rd)) * unit; fget_skip_spaces(rd); ci->c[1] = pz_r3_chain_to_double(fget_int32(rd)) * unit; fget_skip_spaces(rd); ci->c[2] = pz_r3_chain_to_double(fget_int32(rd)) * unit;; } pz_r3_chain_recenter(&(d.c), recenter); filefmt_read_footer(rd, "pz_r3_chain_t"); } return d; } #define NoData (pz_r3_chain_read_data_t){ \ /*cmt*/ "NOT READ", \ /*samples*/ 0, \ /*c*/ (r3_vec_t){0, NULL}, \ /*unit*/ 0 \ } pz_r3_chain_read_all_data_t pz_r3_chain_read_all ( char *prefix, unsigned band, char *extension, bool_vec_t *sel, bool_t header_only, pz_ctr_t recenter, char *dir ) { double unitMin = +INFINITY; double unitMax = -INFINITY; char *cmt = ""; fprintf(stderr, "reading geometric curves:\n"); int nch = sel->nel; pz_r3_chain_read_data_t *dd = malloc(nch * sizeof(pz_r3_chain_read_data_t)); int k; for (k = 0; k < nch; k++) { pz_r3_chain_read_data_t *ddk = &(dd[k]); if (sel->el[k]) { fprintf(stderr, " %04d: ", k); char *full_name; char *full_name = jsprintf("%s/%04d/%s%03d%s", dir, k, prefix, band, extension); FILE *rd = open_read(full_name, TRUE); if (rd != NULL) { *ddk = pz_r3_chain_read(rd, header_only, recenter); fprintf(stderr, "read from %s\n", full_name ); affirm(ddk->unit > 0.0, "bad unit"); if (unitMin > unitMax) { cmt = ddk->cmt; }; unitMin = fmin(unitMin, ddk->unit); unitMax = fmax(unitMax, ddk->unit);; fclose(rd); } else { *ddk = NoData; fprintf(stderr, "file %s not found.\n", full_name ); } } else { *ddk = NoData; } } char *buf = jsprintf ( "Curves read from %s/NNNN/%sBBB%s\nLast entry:\n%s", dir, prefix, extension, cmt ); return (pz_r3_chain_read_all_data_t) { /* chData */ dd, /* unitMin */ unitMin, /* unitMax */ unitMax, /* cmt */ buf }; } void pz_r3_chain_fourier_transform ( pz_r3_chain_t *c, pz_r3_chain_t *f ) { int nc = (c->nel); affirm(f->nel == nc, "wrong size"); pz_double_chain_t cr = double_vec_new(nc); pz_double_chain_t fr = double_vec_new(nc); int i, k; for (k = 0; k < 3; k++) { for (i = 0; i < nc; i++) { cr.el[i] = c->el[i].c[k]; } pz_fourier_transform(&cr, &fr); for (i = 0; i < nc; i++) { f->el[i].c[k] = fr.el[i]; } } } void pz_r3_chain_inv_fourier_transform ( pz_r3_chain_t *f, pz_r3_chain_t *c ) { int nc = (c->nel); affirm(f->nel == nc, "wrong size"); pz_double_chain_t cr = double_vec_new(nc); pz_double_chain_t fr = double_vec_new(nc); int i, k; for (k = 0; k < 3; k++) { for (i = 0; i < nc; i++) { fr.el[i] = f->el[i].c[k]; } pz_fourier_transform(&fr, &cr); for (i = 0; i < nc; i++) { c->el[i].c[k] = cr.el[i]; } } } /* 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. */