/* Last edited on 2024-12-04 22:35:09 by stolfi */ /* ---------------------------------------------------------------------- */ /* from rmxn.c */ int32_t i,j, t; if (olp == NULL) { olp = "(\n"; } if (osep == NULL) { osep = "\n"; } if (orp == NULL) { orp = "\n)"; } if (ilp == NULL) { ilp = " ("; } if (isep == NULL) { isep = " "; } if (irp == NULL) { irp = ")"; } if (fmt == NULL) { fmt = "%16.8e"; } fputs(olp, f); t = 0; for (i = 0; i < m; i++) { if (i > 0) { fputs(osep, f); } fputs(ilp, f); for (j = 0; j < n; j++) { if (j > 0) { fputs(isep, f); } fprintf(f, fmt, A[t]); t++; } fputs(irp, f); } fputs(orp, f); fflush(f); /* ---------------------------------------------------------------------- */ /* from r3_path.c */ { double tant = NAN; /* Previosu sampling parameter. */ r3_t qant; /* Previous point in path. */ r3_t qfin = path(t1); /* Final point in path. */ double t = t0; /* Current path parameter. */ while (t <= t1) { /* Get the candidate point {q} at the candidate time {t}: */ r3_t q = path(t); /* Are we close to the final point? */ double dspan = r3_dist(&qant, &q1); if (ns > 0) { r3_path_select_next_time(path, tant, &qant, t1, &q1, &t) /* Adjust {t} and recompute {q} so that the distance is between {step/2} and {step}: */ double dq = r3_dist(&q, qant); double dfin = r3_dist(&q, &q1); int32_t ntry = 0; while ((ntry < 100) && ((dq > step) || (dq < 0.5*step))) { /* Adjust {t}: */ double dt = t - tant; double r = (dq < 0.01*step ? (dq < 0.85 ? 0.65)*step/dq); double tx = tant + r*dt; } } r3_vec_expand(&spvec, ns); spvec.e[ns] = ; } r3_vec_t spvec = r3_vec_new(1000); } /* ---------------------------------------------------------------------- */ /* from rmxn_extra.c */ /* Checking: */ { fprintf(stderr, "rmxn_throw_ortho: checking k = %d\n", k); int i1,i2; for (i1 = 0; i1 <= k; i1++) { for (i2 = 0; i2 <= i1; i2++) { double d12 = rn_dot(k+1, &(M[i1*n]), &(M[i2*n])); double e12 = (i1 == i2 ? 1 : 0); assert(fabs(d12 - e12) < 1.0e-10); } } } /* ---------------------------------------------------------------------- */ /* from r2_test_class.c */ /* In any case, the procedure returns in {*dens} the approximate sampling probability density function of the returned point {*p}. The value of {*dens} includes the probability of selecting class {*cl} when {i} is uniformly chosen over a large interval. For bidimensional domains, the integral of {*dens} over the union of all domains will be 1. For unidimensional domains, the {*dens} is only an approximation of the linear density. */ void r2_test_class_sballs(problem_t *P, int i, int *classP, double attr[]) { assert(NA == 2); assert((NC == 2) || (NC == 8)); double r = 0.2; /* Radius of the seven disks. */ double R = 0.6; /* Radius of circle through centers of the 6 outer disks. */ /* H = area of hexagon = 2.5980762. */ /* D = area of 7 disks = 0.9076452. */ /* B = area of class 1 = 1.6904310. */ /* Ratio B/H = 0.6506472 ~ 2/3. */ r2_t p; /* The sample point. */ int cl; /* The point's class. */ int rem = i%21; if (rem >= 7) { /* In class 1 (2/3 chance). */ cl = 1; bool_t inCircles = TRUE; bool_t inHexagon = FALSE; while (inCircles || (! in Hexagon)) { /* Throw a point in {U^2} until in class 1. */ p = (r2_t){{ 2*drandom() - 1, 2*drandom() - 1 }}; r2_hexagon_reduce(&p); double dh = 0.8676872*p.c[0] + 0.5000000*p.c[1] - 0.8676872; inHexagon = (dh < 0); double da2 = r2_norm_sqr(&p); r2_t q = (r2_t){{ R, 0 }}; double db2 = r2_dist_sqr(&p,&q); inCircles = (da2 < r*r) || (db2 < r*r); } } else { /* In classes {2..8} (1/3 chance). */ cl = rem + 2; /* Get the center of the circle of class {cl}: */ r2_t q; if (cl == 2) { /* Central circle */ q = (r2_t) {{ 0,0 }}; } else { double t = (cl - 3)*M_PI/3; q = (r2_t) {{ R*cos(t), R*sin(t) }}; } r2_t v; r2_throw_ball(&v); r2_mix(1.0, &q, r, &v, &p); x = q-> (*classP) = cl; attr[0] = x; attr[1] = y; } void r2_test_class_snakes(problem_t *P, int i, int *classP, double attr[]) { /* Classes {2..NC} are N-shaped regions inside the {U^2} square. Class 1 is the background. */ assert(NA == 2); assert(NC >= 2); (*classP) = cl; attr[0] = x; attr[1] = y; } /* ---------------------------------------------------------------------- */ /* from elipse.c */ double ellipse_nearest_point(ellipse_t *ep, r2_t *p, r2_t *q) { bool_t debug = TRUE; /* !!! Delete excess debugging printouts. !!! */ /* Get the axis vectors and radii: */ r2_t u, v; double uR, vR; ellipse_vectors(ep, &u, &uR, &v, &vR); assert(uR >= 0); assert(vR >= 0); if (debug) { P(E, " u = ( %9.5f %9.5f ) uR = %9.5f\n", u.c[0], u.c[1], uR); } if (debug) { P(E, " v = ( %9.5f %9.5f ) vR = %9.5f\n", v.c[0], v.c[1], vR); } /* Handle special cases: */ if ((vR == 0) && (uR == 0)) { /* Ellipse is a point: */ (*q) = ep->ctr; return r2_dist(p, q); } else { /* Compute the coords {up,vp} of {p} in the system {ep.ctr,u,v}: */ r2_t hp; r2_sub(p, &(ep->ctr), &hp); double up = r2_dot(&hp, &u); double vp = r2_dot(&hp, &v); double fp = hypot(up/uR, vp/vR); /* Relative radial position of {p} */ if (debug) { P(E, " p = ( %9.5f %9.5f ) fp = %9.5f\n", up, vp, fp); } assert (fp >= 0); if (fp == 0) { /* Point {p} is at center; nearest pt is {ep.ctr+v*vR}. */ r2_mix(1.0, &(ep->ctr), vR, &v, q); return -vR; } else if (vR == 0) { /* Ellipse is a segment in the {u} direction: */ if (up > +uR) { up = +uR; } if (up < -uR) { up = -uR; } r2_mix(1.0, &(ep->ctr), up, &u, q); return r2_dist(p, q); } else if (up == 0) { /* Point {p} is on the minor axis: */ if (vp >= 0) { r2_add(&(ep->ctr), &v, q); } else { r2_sub(&(ep->ctr), &v, q); } return r2_dist(p, q); } else if ((vp == 0) && (up >= { /* Point {p} is on the major axis, beyond the focus: */ if } else { /* Ellipse has non-empty interior; */ assert(uR > 0); double uR2 = uR*uR; double vR2 = vR*vR; /* Get the coords {uq,vq} of {q} in the system {ep.ctr,u,v}: */ r2_t hq; r2_sub(q, &(ep->ctr), &hq); double uq = r2_dot(&hq, &u); double vq = r2_dot(&hq, &v); if (debug) { P(E, " q = ( %9.5f %9.5f )\n", uq, vq); } double tiny = 1.0e-7*uR; /* Stop when {q} change is less than this. */ int maxIts = 50; /* Max iterations. */ int nIts = 0; /* Num iterations. */ double ouq = +INF, ovq = +INF; /* previous values of {uq,vq}. */ while (TRUE) { if (debug) { P(E, " q raw = ( %9.5f %9.5f )\n", uq, vq); } /* Make sure that {uq,vq} are not too wild: */ if (isnan(uq) || ((uq == 0) && (up != 0)) || (fabs(uq) > fabs(uq)) || (uq*up < 0)) { uq = up/fp; } if (isnan(vq) || ((vq == 0) && (vp != 0)) || (fabs(vq) > fabs(vq)) || (vq*vp < 0)) { vq = vp/fp; } if (debug) { P(E, " q fix = ( %9.5f %9.5f )\n", uq, vq); } /* Pull it radially onto the ellipse: */ double fq = hypot(uq/uR, vq/vR); assert (fq > 0); uq /= fq; vq /= fq; if (debug) { P(E, " q prj = ( %9.5f %9.5f )\n", uq, vq); } /* Check for convergence: */ if (fabs(ouq - uq) + fabs(ovq - vq) < tiny) { break; } /* Check for loop: */ if (nIts >= maxIts) { P(E, "ellipse = "); ellipse_print(E, ep); P(E, "\n"); P(E, "p = "); r2_print(E, p); P(E, "\n"); demand(nIts < maxIts, "failed to converge"); } /* Basic Newton iteration. Equations: */ /* {F(q)==0, H(q)==0} where {H(q)==G(q)×(q-p)}, {G = grad F}. */ /* In terms of {uq,vq:} */ /* {F(q) == (uq/uR)^2 + (vq/vR)^2 - 1}; */ /* {G(q) == (2*uq/uR^2, 2*vq/vR^2)}; */ /* {H(q) == G(q)×(q-p) == 2*uq*(vq-vp)/uR^2 - 2*vq*(uq-up)/vR^2} */ /* { == 2*uq*vq/uR^2-2*uq*vp/uR^2 - 2*vq*uq-up)/vR^2}. */ r2x2_t A; r2_t b; A.c[0][0] = +2*uq/uR2; A.c[0][1] = +2*vq/vR2; A.c[1][0] = +2*(vq-vp)/uR2 - 2*vq/vR2; A.c[1][1] = +2*uq/uR2 - 2*(uq-up)/vR2; b.c[0] = - (uq*uq/uR2 + vq*vq/vR2 - 1); b.c[1] = - (2*uq*(vq-vp)/uR2 - 2*vq*(uq-up)/vR2); if (debug) { P(E, " system:\n"); P(E, " ( %+12.6f %+12.6f )", A.c[0][0], A.c[0][1]); P(E, " ( %+12.6f )\n", b.c[0]); P(E, " ( %+12.6f %+12.6f )", A.c[1][0], A.c[1][1]); P(E, " ( %+12.6f )\n", b.c[1]); } /* Solve the system {A d = b}: */ r2x2_t M; r2x2_inv(&A, &M); r2_t d; r2x2_map_col(&M, &b, &d); if (debug) { P(E, " q adj = ( %+9.5f %+9.5f )\n", d.c[0], d.c[1]); } /* Update {uq,vq}: */ ouq = uq; uq += d.c[0]; ovq = vq; vq += d.c[1]; /* One more iteration: */ nIts++; if (debug) { P(E, "\n"); } } if (debug) { P(E, "converged in %d iterations\n", nIts); } if (debug) { P(E, "\n"); } /* Compute the {q} point: */ r2_mix(uq, &u, vq, &v, q); r2_add(&(ep->ctr), q, q); /* Compute the coeffs of tangent {L} through {q}: */ double UL = 2*uq/uR2; double VL = 2*vq/vR2; double WL = - (UL*uq + VL*vq); /* Compute the signed distance from {p} to {L}: */ double Lp = UL*up + VL*vp + WL; return Lp/hypot(UL, VL); } } } /* ---------------------------------------------------------------------- */ void rmxn_rot2(double c, double s, double *x, double *y) { double X = c*(*x) - s*(*y); double Y = s*(*x) + c*(*y); (*x) = X; (*y) = Y; } void rmxn_spin_rows(int m, int n, double A[], double M[]) { /* Apply {n} two-axis rotations by random angles. */ /* This is almost certainly not a uniformly random rotation. */ if (M != A) { rmxn_copy(m, n, A, M); } int i, j0; for (j0 = 0; j0 < n; j0++) { int j1 = (j0+1)%n; double theta = 2*M_PI*drandom(); double c = cos(theta); double s = sin(theta); for (i = 0; i < m; i++) { rmxn_rot2(c, s, &(M[i*n + j0]), &(M[i*n + j1])); } } } void rmxn_spin_cols(int m, int n, double A[], double M[]) { /* Apply {m} two-axis rotations by random angles. */ /* This is almost certainly not a uniformly random rotation. */ if (M != A) { rmxn_copy(m, n, A, M); } int i0, j; for (i0 = 0; i0 < m; i0++) { int i1 = (i0+1)%n; double theta = 2*M_PI*drandom(); double c = cos(theta); double s = sin(theta); for (j = 0; j < n; j++) { rmxn_rot2(c, s, &(M[i0*n + j]), &(M[i1*n + j])); } } } /* ---------------------------------------------------------------------- */ hr2_pmap_t hr2_pmap_from_points(hr2_point_t *p, hr2_point_t *q, hr2_point_t *r, hr2_point_t *u) { bool_t debug = TRUE; auto void debug_point(char *head, hr2_point_t *p, char *tail); /* Prints the point {p} as the target of a point pair. */ auto void debug_matrix(char *head, r3x3_t *M, char *foot); /* Prints the matrix {M} and its effect on the cardinal simplex and its unit points. */ if (debug) { fprintf(stderr, " point pairs:\n"); debug_point("[ +1 00 00 ] -> p = ", p, "\n"); debug_point("[ 00 +1 00 ] -> q = ", q, "\n"); debug_point("[ 00 00 +1 ] -> r = ", r, "\n"); debug_point("[ +1 +1 +1 ] -> u = ", u, "\n"); fprintf(stderr, "\n"); } if (debug) { fprintf(stderr, " raw weights:"); fprintf(stderr, " p = %11.5f", w.c[0]); fprintf(stderr, " q = %11.5f", w.c[1]); fprintf(stderr, " r = %11.5f", w.c[2]); fprintf(stderr, "\n"); } if (debug) { debug_matrix("cardinal simplex:", P, ""); } void debug_point(char *head, hr2_point_t *p, char *tail) { if (head != NULL) { fprintf(stderr, " %s", head); } r3_t pp; r3_dir(&(p->c), &pp); rn_gen_print(stderr, NH, pp.c, "%+10.7f", "[ ", " ", " ]"); if (tail != NULL) { fprintf(stderr, " %s", tail); } } void debug_matrix(char *head, r3x3_t *M, char *foot) { if (head != NULL) { fprintf(stderr, " %s\n", head); } int i; for (i = 0; i < NH; i++) { r3_t p = (r3_t){{ 0.0, 0.0, 0.0 }}; p.c[i] = 1.0; r3_t q; r3x3_map_row(&p, M, &q); r3_t qq; r3_dir(&q, &qq); rn_gen_print(stderr, NH, p.c, "%+2.0f", " [ ", " ", " ]"); rn_gen_print(stderr, NH, qq.c, "%+10.7f", " -> [ ", " ", " ]"); fprintf(stderr, "\n"); } int w, x, y; for (y = -1; y <= +1; y += 2) for (x = -1; x <= +1; x += 2) for (w = -1; w <= +1; w += 2) { r3_t p = (r3_t){{ (double)w, (double)x, (double)y }}; r3_t q; r3x3_map_row(&p, M, &q); r3_t qq; r3_dir(&q, &qq); rn_gen_print(stderr, NH, p.c, "%+2.0f", " [ ", " ", " ]"); rn_gen_print(stderr, NH, qq.c, "%+10.7f", " -> [ ", " ", " ]"); fprintf(stderr, "\n"); } if (foot != NULL) { fprintf(stderr, " %s\n", foot); } } } void hr2_pmap_print(FILE *wr, char *indent, hr2_pmap_t *P); /* Prints the matrix {P} to {wr}, with each line prefixed by {indent}. */ void hr2_pmap_print(FILE *wr, char *indent, fitr_projmap_t *P) { fprintf(wr, "%s[ %10.4f %10.4f %10.4f ]\n", indent, P->WT, P->XT, P->YT); fprintf(wr, "%s[ %10.4f %10.4f %10.4f ]\n", indent, P->WX, P->XX, P->YX); fprintf(wr, "%s[ %10.4f %10.4f %10.4f ]\n", indent, P->WY, P->XY, P->YY); } double r2x2_cof (r2x2_t *m, int ix, int jx); /* Returns the cofactor of element "[ix,jx]" in matrix "m" */ double r2x2_cof (r2x2_t *m, int ix, int jx) { int ic = 1-ix, jc = 1-jx; double d = m->c[ic][jc]; if (ix != jx) { d = -d; } return d; } double r3x3_cof (r3x3_t *m, int ix, int jx); /* Returns the cofactor of element "[ix,jx]" in matrix "m" */ double r3x3_cof (r3x3_t *m, int ix, int jx) { int i0, i1, j0, j1; double d; /* Select columns of minor:*/ if (ix == 0) {i0 = 1; i1 = 2; } else if (ix == 1) {i0 = 0; i1 = 2; } else if (ix == 2) {i0 = 0; i1 = 1; } if (jx == 0) {j0 = 1; j1 = 2; } else if (jx == 1) {j0 = 0; j1 = 2; } else if (jx == 2) {j0 = 0; j1 = 1; } /* Compute determinant: */ d = m->c[i0][j0]*m->c[i1][j1] - m->c[i0][j1]*m->c[i1][j0]; if ((ix + jx) % 2) d = -d; return (d); } double r4x4_cof (r4x4_t *m, int ix, int jx); /* Returns the cofactor of element "[ix,jx]" in matrix "m" */ double r4x4_cof (r4x4_t *m, int ix, int jx) { r3x3_t t; int i, j, ii, jj; double d; /* Extract minor: */ ii = 0; for (i = 0; i < N; i++) if (i != ix) { jj = 0; for (j = 0; j < N; j++) if (j != jx) { t.c[ii][jj] = m->c[i][j]; jj++; } ii++; } /* Compute its determinant: */ d = r3x3_det(&t); if ((ix + jx) % 2) d = -d; return (d); } void r2x2_gen_print (FILE *f, r2x2_t *m, char *fmt, char *olp, char *osep, char *orp, char *ilp, char *isep, char *irp ) { int i,j; if (olp == NULL) { olp = "(\n"; } if (osep == NULL) { osep = "\n"; } if (orp == NULL) { orp = ")"; } if (ilp == NULL) { ilp = " ("; } if (isep == NULL) { isep = " "; } if (irp == NULL) { irp = ")"; } if (fmt == NULL) { fmt = "%16.8e"; } fputs(olp, f); for (i = 0; i < N; i++) { if (i > 0) { fputs(osep, f); } fputs(ilp, f); for (j = 0; j < N; j++) { if (j > 0) { fputs(isep, f); } fprintf(f, fmt, m->c[i][j]); } fputs(irp, f); } fputs(orp, f); fflush(f); } double r4_orthize (r4_t *a, r4_t *u, r4_t *res) { double u0 = u->c[0]; double u1 = u->c[1]; double u2 = u->c[2]; double u3 = u->c[3]; double sau = a->c[0]*u0 + a->c[1]*u1 + a->c[2]*u2 + a->c[3]*u3; if (sau == 0.0) { *res = *a; return 0.0; } else { double suu = u0*u0 + u1*u1 + u2*u2 + u3*u3; double c = sau / suu; res->c[0] = a->c[0] - c * u0; res->c[1] = a->c[1] - c * u1; res->c[2] = a->c[2] - c * u2; res->c[3] = a->c[3] - c * u3; return c; } } /* ---------------------------------------------------------------------- */ /* From {hr2.h}: */ hr2_pmap_t hr2_affmap_from_point_pairs(r2_vec_t *p1, r2_vec_t *p2); /* Returns an affine map {M} (with {M[1,0]=M[2,0]=0}) that best maps the points {p1} to the points {p2}, in the sense of minimizing the mean squared error. The lists {p1 and {p2} must have the same nonzero length {n}. If {n==1}, the result is a translation. If {n==2}, the result is an Euclidean similarity (translation, rotation and scaling). If {n} is 3 or more the result is a general affine map. */ hr2_pmap_t hr2_affmap_from_point_pairs(r2_vec_t *p1, r2_vec_t *p2) { int np = p1->ne; assert(np == p2->ne); int k; double debug = FALSE; if (debug) { fprintf(stderr, "--- computing the affine matrix ---\n"); } /* Compute the linear matrix {S} and the translation vector {v}: */ r2x2_t S; r2x2_ident(&S); r2_t v = (r2_t){{ 0,0 }}; if (np > 0) { /* Compute the barycenters of {p1} and {p2}: */ r2_t bar1; r2_vec_barycenter(p1, &bar1); if (debug) { r2_gen_print(stderr, &bar1, "%10.4f", " bar1 = ( ", " ", " )\n"); } demand(r2_is_finite(&bar1), "barycenter of {p1} is undefined"); r2_t bar2; r2_vec_barycenter(p2, &bar2); if (debug) { r2_gen_print(stderr, &bar2, "%10.4f", " bar2 = ( ", " ", " )\n"); } demand(r2_is_finite(&bar2), "barycenter of {p2} is undefined"); if (np > 1) { /* Compute mean 2×2 linear transformation {S} from {p1-bar1} to {p2-bar2}: */ if (np == 2) { /* Compute rotation & scale matrix: */ r2_t q1; r2_sub(&(p1->e[0]), &bar1, &q1); r2x2_t R1; r2x2_rot_and_scale(&q1, &R1); r2_t q2; r2_sub(&(p2->e[0]), &bar2, &q2); r2x2_t R2; r2x2_rot_and_scale(&q2, &R2); r2x2_inv(&R1, &R1); r2x2_mul(&R1, &R2, &S); } else { /* Compute general 2×2 linear transformation by least squares: */ r2x2_t A; r2x2_zero(&A); /* Moment matrix. */ r2x2_t B; r2x2_zero(&B); /* Projection matrix. */ for (k = 0; k < np; k++) { /* Reduce points relative to barycenter: */ r2_t q1k, q2k; r2_sub(&(p1->e[k]), &bar1, &q1k); r2_sub(&(p2->e[k]), &bar2, &q2k); /* Accumulate moments and projections: */ int i,j; for(i = 0; i < 2; i ++) { for (j = 0; j < 2; j++) { A.c[i][j] += q1k.c[i]*q1k.c[j]; B.c[i][j] += q1k.c[i]*q2k.c[j]; } } } r2x2_t Z; r2x2_inv(&A, &Z); r2x2_mul(&Z, &B, &S); } if (debug) { fprintf(stderr, " linear matrix:\n"); r2x2_gen_print(stderr, &S, "%13.6e", "", "\n", "\n", " [ ", " ", " ]"); } } r2x2_map_row(&bar1, &S, &v); r2_sub(&bar2, &v, &v); } /* Assemble the map {P.dir}: */ hr2_pmap_t P; P.dir.c[0][0] = 1; P.dir.c[1][0] = 0; P.dir.c[2][0] = 0; P.dir.c[0][1] = v.c[0]; P.dir.c[0][2] = v.c[1]; P.dir.c[1][1] = S.c[0][0]; P.dir.c[1][2] = S.c[0][1]; P.dir.c[2][1] = S.c[1][0]; P.dir.c[2][2] = S.c[1][1]; /* Compute the inverse map and return: */ r3x3_inv(&(P.dir), &(P.inv)); return P; } /* ---------------------------------------------------------------------- */ /* r2_aff_adjust.{h,c} */ /* Inefficient methods. */ void r2_aff_adjust_enum ( r2_aff_adjust_func_t *f2, /* Goal function to minimize. */ r2_aff_map_t *R, /* Max adjustment for {*A}. */ int32_t nSteps, /* Number of enum steps on each side of zero. */ int32_t nPasses, /* Number of passes. */ r2_aff_map_t *A, /* (IN/OUT) The affine map to adjust. */ double *f2P /* (OUT) Goal function value for the output {*A}. */ ); /* Adjusts an affine map {*A} so as to minimize the client-given mismatch function {f2(A)}. Uses coordinate-by-coordinate stepping. On input, {*A} must be a guess for the optimum affine map. On output, {*A} will be an improved affine map, which differs from the input {*A} by a fractional multiple of {*R}. More specifically, it varies each element of {A->mat} and {A->disp} by {nSteps} values on either side of its current value, not exceeding the initial value of that element plus or minus the corresponding element of {*R}. Then sets that element of {*A} to the value (possibly the original one) that gave the lowest mismatch. Performs {nPasses} of these passes over all elements of {*A}, with slowly decreasing step sizes. The elements of {*R} must be all positive or zero. If any element of {*R} is zero, the correponding element of {*A} is considered fixed and is not adjusted. In particular, if {R->mat} is zero, only {A->disp} is adjusted, and vice-versa. If {*R} is entirely zero, or {nSteps} is zero, no optimization is performed. The value of {f2(A)} for the final {*A} is returned on {*f2P}. */ void r2_aff_adjust_rand ( r2_aff_adjust_func_t *f2, /* Goal function to minimize. */ r2_aff_map_t *R, /* Max adjustment for {*A}. */ int32_t nTrials, /* Number of trials. */ r2_aff_map_t *A, /* (IN/OUT) The affine map to adjust. */ double *f2P /* (OUT) Goal function value for the output {*A}. */ ); /* Similar to {r2_aff_adjust_enum}, but uses random direction probing instead of element-by-element changes. More specifically, it selects a random increment map {D}, such that each element of {D} is the corresponding element of {*R}, times a random fraction in {[-1 _ +1]}. Then tries adding {D} to {*A}. If the change improves the function {f2}, replaces {*A} by it. Performs this random probing {nTrials} times. */ void r2_aff_adjust_enum ( r2_aff_adjust_func_t *f2, /* Goal function to minimize. */ r2_aff_map_t *R, /* Max adjustment for the affine map. */ int32_t nSteps, /* Number of enum steps on each side of zero. */ int32_t nPasses, /* Number of passes. */ r2_aff_map_t *A, /* (IN/OUT) The affine map to adjust. */ double *f2P /* (OUT) Goal function value for the output {*A}. */ ) { (*f2P) = f2(A); /* Current goal function on {A}. */ /* Identify the elements to optimize: */ int32_t nv; double *Ap[6]; /* Pointers to adjustable elements. */ double Re[6]; /* Max adjustment amount of each element. */ r2_aff_adjust_get_var_elems(A, R, Ap, Re, &nv); fprintf(stderr, "%d adjustable elements found\n", nv); /* Save initial elem values (center of search region): */ double Ce[nv]; for (int32_t iv = 0; iv < nv; iv++) { Ce[iv] = (*(Ap[iv])); } /* Perform the optimization passes: */ for (int32_t ip = 0; ip < nPasses; ip++) { for (int32_t iv = 0; iv < nv; iv++) { /* Try changing element {Ap[iv]}: */ double Aref = (*(Ap[iv])); /* Reference value of element for steps. */ /* Relative max step size, so that on last pass it can span the range: */ int32_t m = nPasses - ip; /* Passes remainng */ double rstep = pow(2.0, m-1)/(pow(2.0, m) - 1); for (int32_t t = 1; t <= +nSteps; t++) { for (int32_t dir = -1; dir <= +1; dir += 2) { /* Choose the max step based on {Aref} and {Ce[iv] ± Re[iv]}: */ double Reff = (dir < 0 ? Aref - (Ce[iv] - Re[iv]) : (Ce[iv] + Re[iv]) - Aref); /* Clip {Reff} to {Re[iv]}: */ if (Reff > Re[iv]) { Reff = Re[iv]; } /* Relative max step size, so that on last pass it can span the range: */ Reff = Reff * rstep; double Aold = (*(Ap[iv])); /* Save old value of element. */ (*(Ap[iv])) = Aref + dir*((double)t)/((double)nSteps)*Reff; double f2new = f2(A); if (f2new < (*f2P)) { /* Improved; update {(*f2P)}: */ (*f2P) = f2new; } else { /* Did not improve; restore {*A}, leave {*f2P} unchanged: */ (*(Ap[iv])) = Aold; } } } } } } void r2_aff_adjust_rand ( r2_aff_adjust_func_t *f2, /* Goal function to minimize. */ r2_aff_map_t *R, /* Max adjustment for {*A}. */ int32_t nTrials, /* Number of trials. */ r2_aff_map_t *A, /* (IN/OUT) The affine map to adjust. */ double *f2P /* (OUT) Goal function value for the output {*A}. */ ) { (*f2P) = f2(A); /* Current goal function on {A}. */ /* Identify the elements to optimize: */ int32_t nv; double *Ap[6]; /* Pointers to adjustable elements. */ double Re[6]; /* Max adjustment amount of each element. */ r2_aff_adjust_get_var_elems(A, R, Ap, Re, &nv); fprintf(stderr, "%d adjustable elements found\n", nv); if (nv == 0) { return; } /* Save initial elem values (center of search region): */ double Ce[nv]; for (int32_t iv = 0; iv < nv; iv++) { Ce[iv] = (*(Ap[iv])); } /* Perform the optimization trials: */ double Be[nv]; /* Saved values of adjusted elements. */ for (int32_t it = 0; it < nTrials; it++) { for (int32_t iv = 0; iv < nv; iv++) { /* Save the current values of adjustable elem: */ Be[iv] = (*(Ap[iv])); /* Pick a random number in {[-1 _ +1]} with triangular distribution: */ double frac = dabrandom(-0.5, +0.5) + dabrandom(-0.5, +0.5); if (frac > 0) { /* Use {frac} between {*Ap} and {Ce + Re}: */ (*(Ap[iv])) = (1-frac)*Be[iv] + frac*(Ce[iv] + Re[iv]); } else { /* Use (-frac} between {*Ap} and {Ce - Re}: */ (*(Ap[iv])) = (1 + frac)*Be[iv] - frac*(Ce[iv] - Re[iv]); } } /* Evaluate the goal function on {*A}: */ double f2new = f2(A); if (f2new < (*f2P)) { /* Update {*f2P}: */ (*f2P) = f2new; } else { /* Restore {*A}: */ for (int32_t iv = 0; iv < nv; iv++) { (*(Ap[iv])) = Be[iv]; } } } } /* ---------------------------------------------------------------------- */ /* r2_aff_adjust_test.c */ /* IMPLEMENTATIONS */ double r2aat_compute_mismatch_sqr ( /* Number of images being compared. */ int32_t hwx, /* Half-width of comparison window. */ double dx, /* X sampling step. */ double wx[], /* Horizontal weight table. */ int32_t hwy, /* Half-height of comparison window. */ double dy, /* Y sampling step. */ double wy[], /* Vertical weight table. */ r2_aff_map_t *A /* Sampling grid center for each image. */ ) { double sum_wht_var = 0; double sum_wht = 0; for (int32_t jy = -hwy; jy <= +hwy; jy++) { double vy = jy*dy; /* Vert offset of sample map. */ for (int32_t jx = -hwx; jx <= +hwx; jx++) { double vx = jx*dx; /* Horiz offset of sample map. */ /* Sample the images at this grid sampling map: */ double u[ni]; for (int32_t i = 0; i < ni; i++) { /* Get samples of image {i}: */ u = eval(i, scale, *A.c[0]+vx, *A.c[1]+vy); } /* Compute mean and variance, and sampling map weight {wht}: */ double avg, var; r2aat_compute_avg_var(u, &avg, &var); double wht = wx[jx+hwx]*wy[jy+hwy]; sum_wht_var += wht*var; sum_wht += wht; } } return (sum_wht == 0 ? 0.0 : sum_wht_var / sum_wht); } void r2aat_compute_avg_var(double u[], double *avgP, double *varP) { /* Compute the average {avg}: */ double sum_u = 0; /* Sum of {u[ki]} */ for (int32_t i = 0; i < ni; i++) { sum_u += u; } double avg = sum_u/ni; /* Compute the sample variance: */ double sum_du2 = 0; for (int32_t i = 0; i < ni; i++) { double dui = u - avg; sum_du2 += dui*dui; } double var = sum_du2/ni; (*avgP) = avg; (*varP) = var; } /* ---------------------------------------------------------------------- */ /* test_align_enum.c */ FILE *wr = NULL; int32_t nv = r2_align_count_variable_coords(ni, arad); /* Non-fixed coords */ int32_t nd = nv - 2; /* Independent search directions. */ double v[nv]; /* Non-fixed coordinates. */ fprintf(stderr, "nv = %d\n", nv); int32_t rx = -1, ry = -1; /* Indices of the non-fixed coordinates to plot. */ if (nd == 2) { wr = open_write("out/f.dat", TRUE); /* Flatten out the non-zero coordinates of {arad}: */ r2_align_points_to_vars(ni, arad, arad, NULL, nv, v); /* Find the two largest coordinates {v[rx],v[ry]} of {arad}: */ for (int32_t r = 0; r < nv; r++) { if ((rx == -1) || (v[r] > v[rx])) { ry = rx; rx = r; } else if ((ry == -1) || (v[r] > v[ry])) { ry = r; } } fprintf(stderr, "plotting v[%d] = %12.6f v[%d] = %12.6f\n", rx, v[rx], ry, v[ry]); assert((rx >= 0) && (ry >= 0) && (rx != ry)); } r2_t dmax = (r2_t){{ 0.0, 0.0 }}; for (int32_t k = 0; k < nu; k++) { r2_t* Uk = &(U[k*ni]); double sum2 = 0.0; for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double rij = arad[i].c[j]; if (rij != 0) { demand(rij > 0, "invalid arad"); double Ukij = Uk[i].c[j]; double skij = Ukij/rij; sum2 += skij*skij; } } } assert(sum2 > 0); dmax.c[k] = 1.0/sqrt(sum2); if (debug) { fprintf(stderr, "dmax[%d] = %8.5f\n", k, dmax.c[k]); } } /* Find index {ip[j]} of alignment coord to plot from each axis, or {-1} if none: */ int32_t ip[2] = {-1,-1}; /* Indices of the non-fixed coordinates to plot. */ if (nd == 2) { /* Find the two largest coordinates {v[rx],v[ry]} of {arad}: */ for (int32_t j = 0; j < 2; j++) { for (int32_t i = 0; i < ni; i++) { double rij = arad[i].c[j]; if (rij > 0) { if ((ip[j] < 0) || (rij > arad[ip[j]].c[j])) { ip[j] = i; } } } } if ((ip[0] < 0) || (ip[1] < 0)) { ip[0] = -1; ip[1] = -1; } else { for (int32_t j = 0; j < 2; j++) { assert((ip[j] >= 0) && (ip[j] < ni)); double rij = arad[ip[j]].c[j]; fprintf(stderr, "plotting {p[%d].c[%d]} range [%+12.6f _ %+12.6f]\n", ip[j], j, -rij, rij); } wr = open_write("out/f.dat", TRUE); } } double frac (double x); /* Returns the fractional part of {x}, with the sign of {x}. Note that {frac(-2.3)} is {-0.3}, not {0.7}. */ double frac (double x) { int32_t i = (int32_t)x; double f = x - (double)i; return f; } /* ---------------------------------------------------------------------- */ typedef struct r2_aff_map_t { r2x2_t mat; r2_t disp; } r2_aff_map_t; /* An affine map from {\RR^2} to {\RR^2}, specifically that maps a point {p} to the point {p*mat + disp}. The map is degenerate (non-invertible) iff {mat} is singular. */ void r2_aff_map_compose(r3x3_t *A, r3x3_t *B, r3x3_t *C); /* Stores into {*C} the composition of the maps {A} and {B}, applied in that order. That is, if {A} maps {p} to {q}, and {B} maps {q} to {r}, then {C} maps {p} to {r}. */ /* --> hr2_pmap_compose or r3x3_mul */ r3x3_t r2_aff_map_translation(r2_t *vec); /* Returns a projective map that performs a translation by the Cartesian vector {vec}. */ /* --> hr2_pmap_translation */ r3x3_t r2_aff_map_rot_scale(double ang, double scale); /* Returns a projective map that performs a rotaton by {ang} radians combined with uniform scaling by {scale}, both about the origin */ /* --> hr2_pmap_rotation_and_scaling */ void r2_aff_map_apply(r2_t *p, r3x3_t *A, r2_t *q); /* Stores into {*q} the image of {p} by the map {A}. */ /* --> hr2_pmap_r2_point */ void r2_aff_map_invert(r3x3_t *A, r3x3_t *B); /* Stores into {*B} the inverse of the affine map {*A}. The matrix {f.mat} had better be invertible. */ /* hr2_pmap_inv, hr2_pmap_aff_inv */ void r2_aff_map_print (FILE *f, r3x3_t *A) { r2_aff_map_gen_print(f, A, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL); } /* Prints {A} on file {f}, with some default format. */ /* hr2_pmap_print */ void r2_aff_map_gen_print ( FILE *f, r3x3_t *A, char *mfmt, char *dfmt, char *olp, char *osep, char *orp, /* Outer delimiters. */ char *ilp, char *isep, char *irp, /* Inner delimiters. */ char *dsep /* Matrix to displacement separator. */ ); /* Prints the affine map {A} to file {f}, using {mfmt} for each matrix element and {dfmt} for each displacement element. The matrix {A.mat} is bounded by {olp} and {orp}, and its rows are separated by {osep}. Each row of {A.mat} as well as the vector {A.disp} is bounded by {ilp} and {irp}, and elements are separated by {isep}. The matrix and displacement are separated by {dsep}. Defaults are provided for any of these strings which are NULL. */ /* hr2_pmap_gen_print */ void r2_aff_map_gen_print ( FILE *f, r3x3_t *A, char *mfmt, char *dfmt, char *olp, char *osep, char *orp, char *ilp, char *isep, char *irp, char *dsep ) { if (dsep == NULL) { dsep = "\n"; } r2x2_gen_print(f, &(A->mat), mfmt, olp, osep, orp, ilp, isep, irp); fputs(dsep, f); r2_gen_print(f, &(A->disp), dfmt, ilp, isep, irp); } void r2_aff_map_apply(r2_t *p, r3x3_t *A, r2_t *q) { hr2_pmap_aff_norm_check(A); r3_t ph = (r3_t){{ 1.0, p->c[0], p->c[1] }}; r3_t qh; r3x3_map_row(&ph, &(A), &qh); q->c[0] = qh.c[1]/qh.c[0]; q->c[1] = qh.c[2]/qh.c[0]; } void r2_aff_map_invert(r3x3_t *A, r3x3_t *B) { hr2_pmap_aff_norm_check(A); r3x3_inv(A, B); } void r2_aff_map_compose(r3x3_t *A, r3x3_t *B, r3x3_t *C) { hr2_pmap_aff_norm_check(A); hr2_pmap_aff_norm_check(B); r3x3_mul(A, B, C); } r3x3_t r2_aff_map_translation(r2_t *vec) { r3x3_t A; r3x3_ident(&A); A.c[0][1] = vec->c[0]; A.c[0][2] = vec->c[1]; return A; } if (drandom() < 0.100) { M = (hr2_pmap_t){ .mat = (r2x2_t){{ { 1, 2 }, { 3, 4 } }}, .disp = (r2_t){{ 5, 6 }} }; N = (hr2_pmap_t){ .mat = (r2x2_t){{ { 4, 5 }, { 6, 7 } }}, .disp = (r2_t){{ 8, 9 }} }; R = (hr2_pmap_t){ .mat = (r2x2_t){{ { 3, 3 }, { 3, 3 } }}, .disp = (r2_t){{ 3, 3 }} }; } else { throw_aff_map(&M); throw_aff_map(&N); throw_aff_map(&R); } if (i == 0) { M = (hr2_pmap_t){ .mat = (r2x2_t){{ { 10, 0 }, { 0, 10 } }}, .disp = (r2_t){{ 5, 6 }} }; N = (hr2_pmap_t){ .mat = (r2x2_t){{ { 20, 0 }, { 0, 20 } }}, .disp = (r2_t){{ 5, 6 }} }; } else if (i == 1) { M = (hr2_pmap_t){ .mat = (r2x2_t){{ { +10, 0 }, { 0, +10 } }}, .disp = (r2_t){{ 5, 6 }} }; N = (hr2_pmap_t){ .mat = (r2x2_t){{ { 0, +10 }, { -10, 0 } }}, .disp = (r2_t){{ 5, 6 }} }; } else if (i == 2) { M = (hr2_pmap_t){ .mat = (r2x2_t){{ { 10, 0 }, { 0, 0 } }}, .disp = (r2_t){{ 5, 6 }} }; N = (hr2_pmap_t){ .mat = (r2x2_t){{ { 10, 0 }, { 0, 10 } }}, .disp = (r2_t){{ 5, 6 }} }; } else { throw_aff_map(&M); throw_aff_map(&N); } /* ---------------------------------------------------------------------- */ /* rmxn_extra.c */ void rmxn_canonical_simplex(int32_t d, int32_t n, double V[]) { demand(0 <= d, "bad dimension {d}"); demand(d < n, "space dimension {n} is too small for {d}"); int32_t i, j; for (i = 0; i <= d; i++) { for (j = 0; j < n; j++) { V[i*n + j] = (i == j ? 1 : 0); } } } double rmxn_canonical_simplex_radius(int32_t d) { double D = (double)d; return sqrt(D/(D+1)); } double rmxn_canonical_simplex_subradius(int32_t d, int32_t k) { double D = (double)d; double K = (double)k; return sqrt((D-K)/((D+1)*(K+1))); } double rmxn_canonical_simplex_edge(int32_t d) { return M_SQRT2; } double rmxn_canonical_simplex_height(int32_t d) { double D = (double)d; return sqrt((D+1)/D); } double rmxn_canonical_simplex_measure(int32_t d) { double D = (double)d; return sqrt(D+1)*exp(-lgamma(D+1)); } void rmxn_regular_simplex(int32_t n, double V[]) { double N = (double)n; double SN1 = sqrt(N+1); double c = (SN1 - 1)/N; double d = 1 + (N-1)*c; int32_t i, j; /* Set the matrix {p}: */ for (i = 0; i <= n; i++) { int32_t ni = i*n; if (i == 0) { /* Set the first row to {(-1,-1,..-1)}: */ for (j = 0; j < n; j++) { V[ni + j] = -1; } } else { /* Set row {i} to {(1+d+c)*u_{i-1} - (c,c,..c)}: */ for (j = 0; j < n; j++) { V[ni + j] = (i == j+1 ? d : -c); } } } } double rmxn_regular_simplex_radius(int32_t n) { double N = (double)n; return sqrt(N); } double rmxn_regular_simplex_subradius(int32_t n, int32_t k) { double N = (double)n; double K = (double)k; return sqrt((N-K)/(K+1)); } double rmxn_regular_simplex_edge(int32_t n) { double N = (double)n; return sqrt(2*(N+1)); } double rmxn_regular_simplex_height(int32_t n) { double N = (double)n; return (N+1)/sqrt(N); } double rmxn_regular_simplex_measure(int32_t n) { double N = (double)n; return exp((N+1)*log(N+1)/2 - lgamma(N+1)); } void rmxn_canonical_simplex_throw(int32_t d, double x[]) { /* Generate a random point in the unit cube {[0_1]^d}: */ int32_t i; for (i = 0; i < d; i++) { x[i] = drandom(); } /* Sort {x} by increasing value: */ auto int32_t cmp(const void *a, const void *b); int32_t cmp(const void *a, const void *b) { return cmp_double((double *)a, (double *)b); } qsort(x, d, sizeof(double), &cmp); /* Now map the ordered {d}-simplex to the canonical {d}-simplex: */ x[d] = 1; for (i = d; i > 0; i--) { x[i] = x[i] - x[i-1]; } } void rmxn_canonical_simplex_ball_throw(int32_t d, double x[]) { /* Generate a random point in the unit {(d+1)}-dimensional ball: */ rn_throw_ball(d+1, x); /* Compute projection {s} of {x} on {u = (1,1,..1)/sqrt(d+1)}: */ double sum = 0.0; int32_t i; for (i = 0; i <= d; i++) { sum += x[i]; } double s = sum/sqrt(d+1); /* Ensure projection is in {[-1_+1]}: */ if (s > +1.0) { s = +1.0; } if (s < -1.0) { s = -1.0; } /* Compute radius of ball slice at coordinate {s}: */ double rx = sqrt(1.0 - s*s); /* Expand unit ball to cylinder with axis {u} and project normal to {u}: */ if (rx == 0) { rx = 1; } double q = sum/(d+1); for (i = 0; i <= d; i++) { x[i] = (x[i] - q)/rx; } /* Scale to the radius of the canonical {d}-simplex: */ double rc = rmxn_canonical_simplex_radius(d); rn_scale(d+1, rc, x, x); /* Shift to center in {(1,1,..1)/(d+1)}: */ rn_shift(d+1, 1.0/(d+1), x, x); } /* ---------------------------------------------------------------------- */ /* hr2.c */ bool_t hr2_pmap_is_identity(hr2_pmap_t *M, sign_t sgnx, sign_t sgny, double tol) { for (int32_t dir = 0; dir <= 1; dir++) { r3x3_t *A = (dir == 0 ? &(M->dir) : &(M->inv)); double A00 = A->c[0][0]; if (A00 < 1.0e-200) { return FALSE; } double Atol = A00*tol; double E11 = (sgnx == 0 ? fabs(A->c[1][1]) - fabs(A00) : A->c[1][1] - sgnx*A00); double E22 = (sgny == 0 ? fabs(A->c[2][2]) - fabs(A00) : A->c[2][2] - sgny*A00); if (fabs(A->c[0][1]) > Atol) { return FALSE; } if (fabs(A->c[0][2]) > Atol) { return FALSE; } if (fabs(A->c[1][0]) > Atol) { return FALSE; } if (fabs(E11) > Atol) { return FALSE; } if (fabs(A->c[1][2]) > Atol) { return FALSE; } if (fabs(A->c[2][0]) > Atol) { return FALSE; } if (fabs(A->c[2][1]) > Atol) { return FALSE; } if (fabs(E22) > Atol) { return FALSE; } } return TRUE; } /* The {sgn} parameter must be {-1} or {+1}, and specifies the handedness of the resultimg map. If {sgn} is {-1}, the result is the same as obtained with {sgn=+1}, except that the last row of {M.dir} and last column of {M.inv} is negated. This has the effect of flipping the sign of the {Y} coordinate of the point before applying the positive map {M}. */ demand((sgn == +1) || (sgn == -1), "invalid {sgn}"); if (sgn == -1) { /* Combine with a {XY} swap: */ hr2_pmap_t S = hr2_pmap_xy_swap(); (*M) = hr2_pmap_compose(&S, M); } /* The {GENERIC} type is special in that the encoding {enc} does not yield a point of {\RR^8} but a point {y[0..8]} of {\RR^9}, specifically on the unit sphere {\RS^8} embedded in {\RR^9}. The decoding map {dec} is defined on the whole of {\RR^9} minus the origin, and may yield a positive map, a negative map, or an invalid map with zero determinant. See {hr2_pmap_generic_encode} for details. */ /* Note that in the {GENERIC} case the result is 1 more that the degrees of freedom in the map. Therefore, when optimizing a function of the map, an extra constraint or penalty term must be added to keep the optimum unique. */ void test_hr2_pmap_generic_encode__hr2_pmap_generic_decode(bool_t verbose) { if (verbose) { fprintf(stderr, "> --- %s ---\n", __FUNCTION__); } double eqtol = 1.0e-13; /* Tolerance for type matching. */ /* Generate a projective map {M} with positive handedness: */ hr2_pmap_t M; M.dir = hpedt_throw_r3x3(10.0); double det = r3x3_det(&(M.dir)); if (det < 0) { for (int32_t j = 0; j < 3; j++) { M.dir.c[2][j] = - M.dir.c[2][j]; } } r3x3_inv(&(M.dir), &(M.inv)); if (verbose) { hpedt_print_map("reference", &M); } hr2_test_check_pmap("M", &M, eqtol, "bug"); demand(hr2_pmap_is_type(&M, hr2_pmap_type_GENERIC, +1, eqtol), "wrong map type or sign"); /* Get the encoding {y[0..8]} of {M}: */ int32_t ny = 9; double y[ny]; hr2_pmap_generic_encode(&M, y); if (verbose) { hpedt_print_encoding(ny, y); } double mag = r3x3_norm(&(M.dir)); int32_t kv = 0; for (int32_t i = 0; i < 3; i++) { for (int32_t j = 0; j < 3; j++) { double Aij = M.dir.c[i][j]; demand(fabs(y[kv] - Aij/mag) <= eqtol, "{hr2_pmap_generic_encode} failed (1)"); kv++; } } /* Test decoding: */ hr2_pmap_t N = hr2_pmap_identity(); hr2_pmap_generic_decode(y, &N); if (verbose) { hpedt_print_map("decoded", &N); } demand(hr2_pmap_is_type(&N, hr2_pmap_type_GENERIC, +1, eqtol), "wrong map type or sign"); hpedt_check_same_map(&M, &N); if (verbose) { fprintf(stderr, "< --- %s ---\n", __FUNCTION__); } } /* ---------------------------------------------------------------------- */ /* From r3_extra.{h,c}, test_r3.c */ double r3_pick_ortho (r3_t *u, r3_t *r); /* Sets {r} to an arbitrary vector orthogonal to {u}, with the same L-infinity norm as {u} (which is the result of the call). In particular, if {u} is {(0,0,0)}, sets {r} to {(0,0,0)} and returns 0. Note that {r} is not a continuous function of {u}. */ double r3_pick_ortho (r3_t *u, r3_t *r) { if (u != r) { (*r) = (*u); } double m = r3_L_inf_norm(u); if (m != 0) { int32_t i; if (m == fabs(u->c[0])) { i = 0; } else if (m == fabs(u->c[1])) { i = 1; } else if (m == fabs(u->c[2])) { i = 2; } else { assert(FALSE); } int32_t j = (i + 1) % N; int32_t k = (i + 2) % N; double t = r->c[i]; r->c[i] = -(r->c[j]); r->c[j] = t; r->c[k] = 0; } return m; } void test_r3_pick_ortho(bool_t verbose) { if (verbose) { fprintf(stderr, "--- r3_pick_ortho ---\n"); } r3_t a, d; /* Test on basis vectors: */ for (int32_t i = 0; i < N; i++) { int32_t i0 = (i + 0) % N; r3_axis(i0, &a); double ma = r3_L_inf_norm(&a); double mo = r3_pick_ortho(&a, &d); rn_check_eq(ma, mo, NO, NO, "r3_pick_ortho error (0)"); double r = r3_dot(&a, &d); rn_check_eps(r, 0.0, 0.00000001*ma, NO, NO, "r3_pick_ortho error (1)"); } /* Test on random vectors: */ { r3_throw_cube(&a); double ma = r3_L_inf_norm(&a); double mo = r3_pick_ortho(&a,&d); rn_check_eq(ma, mo, NO, NO, "r3_pick_ortho error (2)"); double r = r3_dot(&a, &d); rn_check_eps(r, 0.0, 0.00000001*ma, NO, NO, "r3_pick_ortho error (3)"); } } /* ---------------------------------------------------------------------- */ /* From MISC/sphere-splines/progs/SPBasic.c */ void r3_pick_ortho(r3_t *u, r3_t *v) { int i, imax = 0, imin = 0; for (i = 0; i < 3; i++) { double ui = fabs(u->c[i]); if (ui > fabs(u->c[imax])) { imax = i; } if (ui <= fabs(u->c[imin])) { imin = i; } } affirm(imax != imin, "huh?"); (*v) = (r3_t){{0.0, 0.0, 0.0}}; v->c[imax] = -(u->c[imin]); v->c[imin] = u->c[imax]; r3_dir(v, v); } void r3_pick_ortho_pair(r3_t *u, r3_t *v, r3_t *w) { r3_pick_ortho(u, v); r3_cross(u, v, w); r3_dir(w, w); } /* ---------------------------------------------------------------------- */ /* From r4_extra.{h,c} */ void r4_pick_ortho (r4_t *u, r4_t *r); /* Sets {r} to an arbitrary vector orthogonal to {u}, with the same L-infinity and Euclidean norms as {u}. In particular, if {u} is {(0,0,0,0)}, sets {r} to {(0,0,0,0)}. The result {r} is a continuous function of {u}. */ void r4_pick_ortho (r4_t *u, r4_t *r) { double t0 = u->c[0]; r->c[0] = -(u->c[1]); r->c[1] = t0; double t2 = u->c[2]; r->c[2] = -(u->c[3]); r->c[3] = t2; } /* ---------------------------------------------------------------------- */ /* From r6_extra.{h,c} */ void r6_pick_ortho (r6_t *u, r6_t *r); /* Sets {r} to an arbitrary vector orthogonal to {u}, with the same L-infinity and Euclidean norms as {u}. In particular, if {u} is {(0,0,0,0,0,0)}, sets {r} to {(0,0,0,0,0,0)}. The result {r} is a continuous function of {u}. */ void r6_pick_ortho (r6_t *u, r6_t *r) { double t0 = u->c[0]; r->c[0] = -(u->c[1]); r->c[1] = t0; double t2 = u->c[2]; r->c[2] = -(u->c[3]); r->c[3] = t2; double t4 = u->c[4]; r->c[4] = -(u->c[5]); r->c[5] = t4; } /* ---------------------------------------------------------------------- */ /* From rmxn.h */ double rmxn_cof (int32_t n, double *A, int32_t ix, int32_t jx); /* Returns the cofactor of element {[ix,jx]} in the {n x n} matrix {A}. */ void rmxn_adj (int32_t n, double *A, double *M); /* Sets {M} to the adjoint of the {n x n} matrix {A}. The matrix {M} may be the same as {A}. */ /* ---------------------------------------------------------------------- */ /* From hr2_pmap_test_tools.c */ hr2_pmap_t hr2_pmap_test_tools_throw_near_singular(double detMax) { bool_t debug = TRUE; if (debug) { Pr(Er, " > --- %s ---\n", __FUNCTION__); } demand(detMax > 1.0e-13, "{detMax} is too small"); double detMin = detMax/10; r3x3_t A1, A2; /* Candidate matrix and its inverse. */ bool_t Aok; int32_t iter = 0; do { if (debug) { Pr(Er, " iteration %d\n", iter); } /* Fill {A1} with a singular array with unit RMS elem: */ rmxn_throw_singular(NH, &(A1.c[0][0])); /* Tests show that if a singular {3×3} matrix with unit element RMS is perturbed by adding to each element a random number in the range {[-eps _ +eps]}, for small {eps}, the determinant of the perturbed matrix will have zero mean and deviation {~2*eps}. Therefore, by setting {eps=detMax/4} we have a good chance that the determinant will be between {detMax/4} and {detMax}: */ double eps = detMax/4; if (debug) { Pr(Er, " eps = %24.16e\n", eps); } for (int32_t i = 0; i < NH; i++) { for (int32_t j = 0; j < NH; j++) { A1.c[i][j] += eps*(2*drandom() - 1); } } Aok = TRUE; double A1enorm; if (Aok) { if (debug) { Pr(Er, " checking if {A1} is far enough from zero ...\n"); } A1enorm = r3x3_norm(&A1)/3; /* RMS of {A1} elems. */ if (debug) { Pr(Er, " norm(A1)/3 = %24.16e\n", A1enorm); } if ((! isfinite(A1enorm)) || (fabs(A1enorm) < 1.0e-100)) { Aok = FALSE; } } double detA1; if (Aok) { if (debug) { Pr(Er, " normalizing {A1} to unit RMS elem ...\n"); } r3x3_scale(1/A1enorm, &(A1), &(A1)); detA1 = r3x3_det(&A1); if (debug) { Pr(Er, " det(A1) = %24.16e\n", detA1); } if (debug) { Pr(Er, " ensuring {det(A1) >= detMin} ...\n"); } if (fabs(detA1) < detMin) { /* Too small: */ Aok = FALSE; } if (debug) { Pr(Er, " ensuring {det(A1) <= detMax} ...\n"); } if (fabs(detA1) > detMax) { /* Too big: */ Aok = FALSE; } } /* Now get the inverse {A2} of {A1} and make sure it is OK: */ double A2enorm; if (Aok) { if (debug) { Pr(Er, " computing {A2} the adjoint of {A1} ...\n"); } r3x3_adj(&A1, &A2); A2enorm = r3x3_norm(&A2)/3; if (debug) { Pr(Er, " norm(A2)/3 = %24.16e\n", A2enorm); } if ((! isfinite(A2enorm)) || (fabs(A2enorm) < 1.0e-100)) { Aok = FALSE; } } double detA2; if (Aok) { if (debug) { Pr(Er, " normalizing {A2} to unit RMS elem ...\n"); } r3x3_scale(1/A2enorm, &(A2), &(A2)); detA2 = r3x3_det(&A2); if (debug) { Pr(Er, " det(A2) = %24.16e\n", detA2); } if (debug) { Pr(Er, " ensuring {det(A2) >= detMin} ...\n"); } if (fabs(detA2) < detMin) { Aok = FALSE; } } iter++; demand (iter < 10, "exceed max iterations"); if (debug) { Pr(Er, "\n"); } } while (! Aok); /* Package the map: */ hr2_pmap_t M; if (drandom() < 0.5) { M.dir = A1; M.inv = A2; } else { M.dir = A2; M.inv = A1; } if (debug) { Pr(Er, " < --- %s ---\n", __FUNCTION__); } return M; } /* ---------------------------------------------------------------------- */ /* rmxn_inv */ gauss_elim_triangularize(n, nC, C, TRUE, 0.0); double det = 1.0; for (uint32_t i = 0; i < n; i++) { det *= C[nC*i + i]; } if (det == 0.0) { /* Singular matrix: */ double *Mij = M; for (uint32_t i = 0; i < n; i++) { for (uint32_t j = 0; j < n; j++) { (*Mij) = NAN; Mij++; } } } else { gauss_elim_diagonalize(n, nC, C); gauss_elim_normalize(n, nC, C); for (uint32_t i = 0; i < n; i++) { double *Cij = &(C[nC*i + n]); double *Mij = &(M[n*i]); for (uint32_t j = 0; j < n; j++) { (*Mij) = (*Cij); Mij++; Cij++; } } } /* ---------------------------------------------------------------------- */ /* rn_cross before gausol_solve: */ void rn_cross (uint32_t n, double *a[], double r[]) { uint32_t nn1 = (n-1)*n; double *C = rmxn_alloc(n-1, n); double *Cij = C; for (uint32_t i = 0; i < n-1; i++) { double *ai = a[i]; for (uint32_t j = 0; j < n; j++) { (*Cij) = ai[j]; Cij++; } } gauss_elim_triangularize(n-1, n, C, TRUE, 0.0); gauss_elim_diagonalize(n-1, n, C); /* If {det(C)} is not zero, set {d = det(C)}, {izer = -1}. Else set {izer} to the first row {i} such that {C[i,i]} is zero, and {d} to the determinant of {C} excluding column {izer}. */ double d = 1.0; int32_t izer = -1; { uint32_t t = 0; for (uint32_t i = 0; i < n-1; i++) { if (C[t] == 0.0) { if (izer < 0) { izer = (int32_t)i; t++; } else { d = 0.0; break; } } d *= C[t]; t += n+1; } } if (izer < 0) { /* The main diagonal of {C} is non-zero, and possibly column {n-1}. */ r[n-1] = d; /* For {i=0..n-2}, set {r[i] = -d*(C[i,n-1]/C[i,i])}: */ uint32_t s = nn1-1; uint32_t t = nn1-2; for (int32_t i = (int32_t)n-2; i >= 0; i--) { r[i] = -d*C[s]/C[t]; s -= n; t -= n+1; } } else { /* Set {r[izer] = (-1)^(n-1-izer)*d}, all other elems to 0; */ assert((izer >= 0) && (izer < n-1)); for (uint32_t i = 0; i < n; i++) { r[i] = 0.0; } r[izer] = ((n - 1 - (uint32_t)izer) % 2 == 0 ? d : -d); } free(C); } /* STUPID replacement: */ void rn_cross (uint32_t n, double *a[], double r[]) { double *C = rmxn_alloc(n-1, n); double *b = rn_alloc(n); for (uint32_t j = 0; j < n; j++) { b[j] = 0; } double *Cij = C; for (uint32_t i = 0; i < n-1; i++) { double *ai = a[i]; for (uint32_t j = 0; j < n; j++) { (*Cij) = ai[j]; Cij++; } } /* !!! Using full pivoting. Would partial pivoting be better? !!! */ bool_t pivot_rows = TRUE, pivot_cols = TRUE; double tiny = 1.0e-180; double det; uint32_t rank; gausol_solve(n-1, n, C, 1, b, r, pivot_rows, pivot_cols, tiny, &rank, &det); free(C); free(b); } double rmxn_inv_full (uint32_t n, double *A, double *M); /* Sets {M} to the inverse of the {n x n} matrix {A}. The matrix {M} may be the same as {A}. Returns the determinant of {A}. If the returned determinant is zero, the contents of {M} should be considered garbage. Note that if {A} is singular or nearly so the procedure may still return a nonzero value, but the contents of {M} will probably be garbage all the same. Uses full pivoting. */ double rmxn_inv_full (uint32_t n, double *A, double *M) { /* Copy {A} into {M}: */ int32_t t = 0; for (uint32_t i = 0; i < n; i++) { for (uint32_t j = 0; j < n; j++) { M[t] = A[t]; t++; } } double det = 1.0; /* Accumulates the determinant. */ uint32_t prow[n], pcol[n]; uint32_t k = 0; while (k < n) { /* Process the remaining {n-k}x{n-k} submatrix starting at {A[k][k]}. */ /* Find the element with largest abs value in the submatrix. */ /* Store its value in {biga}, its indices in {prow[k],pcol[k]}: */ prow[k] = k; pcol[k] = k; double biga = M[k*n+k]; for (uint32_t i = k; i < n; i++) { for (uint32_t j = k; j < n; j++) { if (fabs (M[i*n+j]) > fabs (biga)) { biga = M[i*n+j]; prow[k] = i; pcol[k] = j; } } } /* Accumulate the determinant: */ det *= biga; /* Product of pivots */ /* If the matrix is all zeros, we break here: */ if (biga == 0.0) { break; } /* Swap rows {k} and {prow[k]}: */ { uint32_t i = prow[k]; if (i > k) { for (uint32_t j = 0; j < n; j++) { double hold = -M[k*n+j]; M[k*n+j] = M[i*n+j]; M[i*n+j] = hold; } } } /* Swap columns {k} and {pcol[k]}: */ { uint32_t j = pcol[k]; if (j > k) { for (uint32_t i = 0; i < n; i++) { double hold = -M[i*n+k]; M[i*n+k] = M[i*n+j]; M[i*n+j] = hold; } } } /* Negate column {k} and divide by minus the pivot {biga}. */ for (uint32_t i = 0; i < n; i++) { if (i != k) { M[i*n+k] /= -biga; } } /* Reduce the matrix: */ for (uint32_t i = 0; i < n; i++) { if (i != k) { double hold = M[i*n+k]; for (uint32_t j = 0; j < n; j++) { if (j != k) { M[i*n+j] += hold * M[k*n+j]; } } } } /* Divide row {k} by pivot {biga}: */ for (uint32_t j = 0; j < n; j++) { if (j != k) { M[k*n+j] /= biga; } } /* Set the diagonal element to its reciprocal: */ M[k*n+k] = 1.0 / biga; /* Shrink: */ k++; } /* Undo the row and column interchanges, transposed: */ while(k > 0) { /* Back up in {k}: */ k--; /* Safe here since {k > 0}. */ /* Swap column {k} with column {prow[k]}: */ { uint32_t i = prow[k]; if (i > k) { for (uint32_t j = 0; j < n; j++) { double hold = M[j*n+k]; M[j*n+k] = -M[j*n+i]; M[j*n+i] = hold; } } } /* Swap row {k} with row {pcol[k]}: */ { uint32_t j = pcol[k]; if (j > k) { for (uint32_t i = 0; i < n; i++) { double hold = M[k*n+i]; M[k*n+i] = -M[j*n+i]; M[j*n+i] = hold; } } } } /* Return the determinant: */ return det; } void test_rmxn_inv_full(uint32_t m, bool_t verbose); test_rmxn_inv_full(m, verbose); void test_rmxn_inv_full(uint32_t m, bool_t verbose) { verbose = TRUE; double *Qmm = rmxn_alloc(m, m); double *Rmm = rmxn_alloc(m, m); double *Smm = rmxn_alloc(m, m); /* TEST: double rmxn_inv_full (uint32_t n, double *A, double *M); */ if (verbose) { fprintf(stderr, "--- rmxn_inv_full ---\n"); } double s = rmxn_inv_full(m, Qmm, Rmm); rmxn_mul(m, m, m, Qmm, Rmm, Smm); for (uint32_t i = 0; i < m; i++) { for (uint32_t j = 0; j < m; j++) { double val = (i == j ? 1.0 : 0.0); rn_test_tools_check_eps(Smm[m*i + j],val,0.000000001, NO, NO, "rmxn_inv_full error"); } } double r = rmxn_det(m, Qmm); rn_test_tools_check_eps(r,s,0.000000001, NO, NO, "rmxn_inv-full/rmxn_det error"); free(Qmm); free(Rmm); free(Smm); } /* ---------------------------------------------------------------------- */ /* rmxn_ellipsoid.c */ { /* Convert {M} to tridiag with diagonal {e[0..d-1]} and subdiagonal {s[1..d-1]}: */ double s[d]; /* Sub-diagonal elements of temporary tridiagonal matrix. */ sym_eigen_tridiagonalize(d, M, e, s, Q); /* Compute eigenvalues and eigenvectors from {Q} and tridiag matrix: */ uint32_t a; /* Number of eigenvalues computed. */ uint32_t absrt = 0; /* Sort eigenvalues by signed value. */ sym_eigen_trid_eigen(d, e, s, Q, &a, absrt); /* Check that all eigenvalues are positive: */ demand(a == d, "failed to determine eigenvalues of {M}"); } /* ---------------------------------------------------------------------- */ /* rmxn_transform_quadratic.c */ /* Convert {M} to tridiag with diagonal {d[0..m-1]} and subdiagonal {t[1..m-1]}: */ double d[m]; /* Diagonal elements of tridiagonal matrix. */ double t[m]; /* Sub-diagonal elements of temporary tridiagonal matrix. */ sym_eigen_tridiagonalize(m, M, d, t, F); /* Compute eigenvalues and eigenvectors from {F} and tridiag matrix: */ uint32_t p; /* Number of eigenvalues computed. */ uint32_t absrt = 0; /* Sort eigenvalues by signed value. */ sym_eigen_trid_eigen(m, d, t, F, &p, absrt);