/* find the closest local minimizer (lm) to p with a smaller function value;
this function is called when p is first added to our tree */
static void find_closest_lm(int n, rb_tree *lms, pt *p)
{
rb_node *node = rb_tree_find_lt(lms, &p->f);
double closest_d = HUGE_VAL;
while (node) {
double d = distance2(n, p->x, node->k+1);
if (d < closest_d) closest_d = d;
node = rb_tree_pred(node);
}
p->closest_lm_d = closest_d;
}
/* find the closest pt to p with a smaller function value;
this function is called when p is first added to our tree */
static void find_closest_pt(int n, rb_tree *pts, pt *p)
{
rb_node *node = rb_tree_find_lt(pts, (rb_key) p);
double closest_d = HUGE_VAL;
while (node) {
double d = distance2(n, p->x, ((pt *) node->k)->x);
if (d < closest_d) closest_d = d;
node = rb_tree_pred(node);
}
p->closest_pt_d = closest_d;
}
static nlopt_result divide_good_rects(params *p)
{
const int n = p->n;
double **hull;
int nhull, i, xtol_reached = 1, divided_some = 0;
double magic_eps = p->magic_eps;
if (p->hull_len < p->rtree.N) {
p->hull_len += p->rtree.N;
p->hull = (double **) realloc(p->hull, sizeof(double*)*p->hull_len);
if (!p->hull) return NLOPT_OUT_OF_MEMORY;
}
nhull = convex_hull(&p->rtree, hull = p->hull, p->which_opt != 1);
divisions:
for (i = 0; i < nhull; ++i) {
double K1 = -HUGE_VAL, K2 = -HUGE_VAL, K;
int im, ip;
/* find unequal points before (im) and after (ip) to get slope */
for (im = i-1; im >= 0 && hull[im][0] == hull[i][0]; --im) ;
for (ip = i+1; ip < nhull && hull[ip][0] == hull[i][0]; ++ip) ;
if (im >= 0)
K1 = (hull[i][1] - hull[im][1]) / (hull[i][0] - hull[im][0]);
if (ip < nhull)
K2 = (hull[i][1] - hull[ip][1]) / (hull[i][0] - hull[ip][0]);
K = MAX(K1, K2);
if (hull[i][1] - K * hull[i][0]
<= p->minf - magic_eps * fabs(p->minf) || ip == nhull) {
/* "potentially optimal" rectangle, so subdivide */
nlopt_result ret = divide_rect(hull[i], p);
divided_some = 1;
if (ret != NLOPT_SUCCESS) return ret;
xtol_reached = xtol_reached && small(hull[i] + 3+n, p);
}
/* for the DIRECT-L variant, we only divide one rectangle out
of all points with equal diameter and function values
... note that for p->which_opt == 1, i == ip-1 should be a no-op
anyway, since we set allow_dups=0 in convex_hull above */
if (p->which_opt == 1)
i = ip - 1; /* skip to next unequal point for next iteration */
else if (p->which_opt == 2) /* like DIRECT-L but randomized */
i += nlopt_iurand(ip - i); /* possibly do another equal pt */
}
if (!divided_some) {
if (magic_eps != 0) {
magic_eps = 0;
goto divisions; /* try again */
}
else { /* WTF? divide largest rectangle with smallest f */
/* (note that this code actually gets called from time
to time, and the heuristic here seems to work well,
but I don't recall this situation being discussed in
the references?) */
rb_node *max = rb_tree_max(&p->rtree);
rb_node *pred = max;
double wmax = max->k[0];
do { /* note: this loop is O(N) worst-case time */
max = pred;
pred = rb_tree_pred(max);
} while (pred && pred->k[0] == wmax);
return divide_rect(max->k, p);
}
}
return xtol_reached ? NLOPT_XTOL_REACHED : NLOPT_SUCCESS;
}
/* Find the lower convex hull of a set of points (x,y) stored in a rb-tree
of pointers to {x,y} arrays sorted in lexographic order by (x,y).
Unlike standard convex hulls, we allow redundant points on the hull,
and even allow duplicate points if allow_dups is nonzero.
The return value is the number of points in the hull, with pointers
stored in hull[i] (should be an array of length >= t->N).
*/
static int convex_hull(rb_tree *t, double **hull, int allow_dups)
{
int nhull = 0;
double minslope;
double xmin, xmax, yminmin, ymaxmin;
rb_node *n, *nmax;
/* Monotone chain algorithm [Andrew, 1979]. */
n = rb_tree_min(t);
if (!n) return 0;
nmax = rb_tree_max(t);
xmin = n->k[0];
yminmin = n->k[1];
xmax = nmax->k[0];
if (allow_dups)
do { /* include any duplicate points at (xmin,yminmin) */
hull[nhull++] = n->k;
n = rb_tree_succ(n);
} while (n && n->k[0] == xmin && n->k[1] == yminmin);
else
hull[nhull++] = n->k;
if (xmin == xmax) return nhull;
/* set nmax = min mode with x == xmax */
#if 0
while (nmax->k[0] == xmax)
nmax = rb_tree_pred(nmax); /* non-NULL since xmin != xmax */
nmax = rb_tree_succ(nmax);
#else
/* performance hack (see also below) */
{
double kshift[2];
kshift[0] = xmax * (1 - 1e-13);
kshift[1] = -HUGE_VAL;
nmax = rb_tree_find_gt(t, kshift); /* non-NULL since xmin != xmax */
}
#endif
ymaxmin = nmax->k[1];
minslope = (ymaxmin - yminmin) / (xmax - xmin);
/* set n = first node with x != xmin */
#if 0
while (n->k[0] == xmin)
n = rb_tree_succ(n); /* non-NULL since xmin != xmax */
#else
/* performance hack (see also below) */
{
double kshift[2];
kshift[0] = xmin * (1 + 1e-13);
kshift[1] = -HUGE_VAL;
n = rb_tree_find_gt(t, kshift); /* non-NULL since xmin != xmax */
}
#endif
for (; n != nmax; n = rb_tree_succ(n)) {
double *k = n->k;
if (k[1] > yminmin + (k[0] - xmin) * minslope)
continue;
/* performance hack: most of the points in DIRECT lie along
vertical lines at a few x values, and we can exploit this */
if (nhull && k[0] == hull[nhull - 1][0]) { /* x == previous x */
if (k[1] > hull[nhull - 1][1]) {
double kshift[2];
/* because of the round to float in rect_diameter, above,
it shouldn't be possible for two diameters (x values)
to have a fractional difference < 1e-13. Note
that k[0] > 0 always in DIRECT */
kshift[0] = k[0] * (1 + 1e-13);
kshift[1] = -HUGE_VAL;
n = rb_tree_pred(rb_tree_find_gt(t, kshift));
continue;
}
else { /* equal y values, add to hull */
if (allow_dups)
hull[nhull++] = k;
continue;
}
}
/* remove points until we are making a "left turn" to k */
while (nhull > 1) {
double *t1 = hull[nhull - 1], *t2;
/* because we allow equal points in our hull, we have
to modify the standard convex-hull algorithm slightly:
we need to look backwards in the hull list until we
find a point t2 != t1 */
int it2 = nhull - 2;
do {
t2 = hull[it2--];
} while (it2 >= 0 && t2[0] == t1[0] && t2[1] == t1[1]);
if (it2 < 0) break;
/* cross product (t1-t2) x (k-t2) > 0 for a left turn: */
if ((t1[0]-t2[0]) * (k[1]-t2[1])
- (t1[1]-t2[1]) * (k[0]-t2[0]) >= 0)
break;
--nhull;
}
hull[nhull++] = k;
}
if (allow_dups)
do { /* include any duplicate points at (xmax,ymaxmin) */
hull[nhull++] = nmax->k;
nmax = rb_tree_succ(nmax);
} while (nmax && nmax->k[0] == xmax && nmax->k[1] == ymaxmin);
else
hull[nhull++] = nmax->k;
return nhull;
}
/* Internal version of nldrmd_minimize, intended to be used as
a subroutine for the subplex method. Three differences compared
to nldrmd_minimize:
*minf should contain the value of f(x) (so that we don't have to
re-evaluate f at the starting x).
if psi > 0, then it *replaces* xtol and ftol in stop with the condition
that the simplex diameter |xl - xh| must be reduced by a factor of psi
... this is for when nldrmd is used within the subplex method; for
ordinary termination tests, set psi = 0.
scratch should contain an array of length >= (n+1)*(n+1) + 2*n,
used as scratch workspace.
On output, *fdiff will contain the difference between the high
and low function values of the last simplex. */
nlopt_result nldrmd_minimize_(int n, nlopt_func f, void *f_data,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
const double *xstep, /* initial step sizes */
nlopt_stopping *stop,
double psi, double *scratch,
double *fdiff)
{
double *pts; /* (n+1) x (n+1) array of n+1 points plus function val [0] */
double *c; /* centroid * n */
double *xcur; /* current point */
rb_tree t; /* red-black tree of simplex, sorted by f(x) */
int i, j;
double ninv = 1.0 / n;
nlopt_result ret = NLOPT_SUCCESS;
double init_diam = 0;
pts = scratch;
c = scratch + (n+1)*(n+1);
xcur = c + n;
rb_tree_init(&t, simplex_compare);
*fdiff = HUGE_VAL;
/* initialize the simplex based on the starting xstep */
memcpy(pts+1, x, sizeof(double)*n);
pts[0] = *minf;
if (*minf < stop->minf_max) { ret=NLOPT_MINF_MAX_REACHED; goto done; }
for (i = 0; i < n; ++i) {
double *pt = pts + (i+1)*(n+1);
memcpy(pt+1, x, sizeof(double)*n);
pt[1+i] += xstep[i];
if (pt[1+i] > ub[i]) {
if (ub[i] - x[i] > fabs(xstep[i]) * 0.1)
pt[1+i] = ub[i];
else /* ub is too close to pt, go in other direction */
pt[1+i] = x[i] - fabs(xstep[i]);
}
if (pt[1+i] < lb[i]) {
if (x[i] - lb[i] > fabs(xstep[i]) * 0.1)
pt[1+i] = lb[i];
else {/* lb is too close to pt, go in other direction */
pt[1+i] = x[i] + fabs(xstep[i]);
if (pt[1+i] > ub[i]) /* go towards further of lb, ub */
pt[1+i] = 0.5 * ((ub[i] - x[i] > x[i] - lb[i] ?
ub[i] : lb[i]) + x[i]);
}
}
if (close(pt[1+i], x[i])) { ret=NLOPT_FAILURE; goto done; }
pt[0] = f(n, pt+1, NULL, f_data);
CHECK_EVAL(pt+1, pt[0]);
}
restart:
for (i = 0; i < n + 1; ++i)
if (!rb_tree_insert(&t, pts + i*(n+1))) {
ret = NLOPT_OUT_OF_MEMORY;
goto done;
}
while (1) {
rb_node *low = rb_tree_min(&t);
rb_node *high = rb_tree_max(&t);
double fl = low->k[0], *xl = low->k + 1;
double fh = high->k[0], *xh = high->k + 1;
double fr;
*fdiff = fh - fl;
if (init_diam == 0) /* initialize diam. for psi convergence test */
for (i = 0; i < n; ++i) init_diam += fabs(xl[i] - xh[i]);
if (psi <= 0 && nlopt_stop_ftol(stop, fl, fh)) {
ret = NLOPT_FTOL_REACHED;
goto done;
}
/* compute centroid ... if we cared about the perfomance of this,
we could do it iteratively by updating the centroid on
each step, but then we would have to be more careful about
accumulation of rounding errors... anyway n is unlikely to
be very large for Nelder-Mead in practical cases */
memset(c, 0, sizeof(double)*n);
for (i = 0; i < n + 1; ++i) {
double *xi = pts + i*(n+1) + 1;
if (xi != xh)
for (j = 0; j < n; ++j)
c[j] += xi[j];
}
for (i = 0; i < n; ++i) c[i] *= ninv;
/* x convergence check: find xcur = max radius from centroid */
memset(xcur, 0, sizeof(double)*n);
for (i = 0; i < n + 1; ++i) {
double *xi = pts + i*(n+1) + 1;
for (j = 0; j < n; ++j) {
double dx = fabs(xi[j] - c[j]);
if (dx > xcur[j]) xcur[j] = dx;
}
}
for (i = 0; i < n; ++i) xcur[i] += c[i];
if (psi > 0) {
double diam = 0;
for (i = 0; i < n; ++i) diam += fabs(xl[i] - xh[i]);
if (diam < psi * init_diam) {
ret = NLOPT_XTOL_REACHED;
goto done;
}
}
else if (nlopt_stop_x(stop, c, xcur)) {
ret = NLOPT_XTOL_REACHED;
goto done;
}
/* reflection */
if (!reflectpt(n, xcur, c, alpha, xh, lb, ub)) {
ret=NLOPT_XTOL_REACHED; goto done;
}
fr = f(n, xcur, NULL, f_data);
CHECK_EVAL(xcur, fr);
if (fr < fl) { /* new best point, expand simplex */
if (!reflectpt(n, xh, c, gamm, xh, lb, ub)) {
ret=NLOPT_XTOL_REACHED; goto done;
}
fh = f(n, xh, NULL, f_data);
CHECK_EVAL(xh, fh);
if (fh >= fr) { /* expanding didn't improve */
fh = fr;
memcpy(xh, xcur, sizeof(double)*n);
}
}
else if (fr < rb_tree_pred(high)->k[0]) { /* accept new point */
memcpy(xh, xcur, sizeof(double)*n);
fh = fr;
}
else { /* new worst point, contract */
double fc;
if (!reflectpt(n,xcur,c, fh <= fr ? -beta : beta, xh, lb,ub)) {
ret=NLOPT_XTOL_REACHED; goto done;
}
fc = f(n, xcur, NULL, f_data);
CHECK_EVAL(xcur, fc);
if (fc < fr && fc < fh) { /* successful contraction */
memcpy(xh, xcur, sizeof(double)*n);
fh = fc;
}
else { /* failed contraction, shrink simplex */
rb_tree_destroy(&t);
rb_tree_init(&t, simplex_compare);
for (i = 0; i < n+1; ++i) {
double *pt = pts + i * (n+1);
if (pt+1 != xl) {
if (!reflectpt(n,pt+1, xl,-delta,pt+1, lb,ub)) {
ret = NLOPT_XTOL_REACHED;
goto done;
}
pt[0] = f(n, pt+1, NULL, f_data);
CHECK_EVAL(pt+1, pt[0]);
}
}
goto restart;
}
}
high->k[0] = fh;
rb_tree_resort(&t, high);
}
done:
rb_tree_destroy(&t);
return ret;
}
int main(int argc, char **argv)
{
int N, M;
int *k;
double kd;
rb_tree t;
rb_node *n;
int i, j;
if (argc < 2) {
fprintf(stderr, "Usage: redblack_test Ntest [rand seed]\n");
return 1;
}
N = atoi(argv[1]);
k = (int *) malloc(N * sizeof(int));
rb_tree_init(&t, comp);
srand((unsigned) (argc > 2 ? atoi(argv[2]) : time(NULL)));
for (i = 0; i < N; ++i) {
double *newk = (double *) malloc(sizeof(double));
*newk = (k[i] = rand() % N);
if (!rb_tree_insert(&t, newk)) {
fprintf(stderr, "error in rb_tree_insert\n");
return 1;
}
if (!rb_tree_check(&t)) {
fprintf(stderr, "rb_tree_check_failed after insert!\n");
return 1;
}
}
if (t.N != N) {
fprintf(stderr, "incorrect N (%d) in tree (vs. %d)\n", t.N, N);
return 1;
}
for (i = 0; i < N; ++i) {
kd = k[i];
if (!rb_tree_find(&t, &kd)) {
fprintf(stderr, "rb_tree_find lost %d!\n", k[i]);
return 1;
}
}
n = rb_tree_min(&t);
for (i = 0; i < N; ++i) {
if (!n) {
fprintf(stderr, "not enough successors %d\n!", i);
return 1;
}
printf("%d: %g\n", i, n->k[0]);
n = rb_tree_succ(n);
}
if (n) {
fprintf(stderr, "too many successors!\n");
return 1;
}
n = rb_tree_max(&t);
for (i = 0; i < N; ++i) {
if (!n) {
fprintf(stderr, "not enough predecessors %d\n!", i);
return 1;
}
printf("%d: %g\n", i, n->k[0]);
n = rb_tree_pred(n);
}
if (n) {
fprintf(stderr, "too many predecessors!\n");
return 1;
}
for (M = N; M > 0; --M) {
int knew = rand() % N; /* random new key */
j = rand() % M; /* random original key to replace */
for (i = 0; i < N; ++i)
if (k[i] >= 0)
if (j-- == 0)
break;
if (i >= N)
abort();
kd = k[i];
if (!(n = rb_tree_find(&t, &kd))) {
fprintf(stderr, "rb_tree_find lost %d!\n", k[i]);
return 1;
}
n->k[0] = knew;
if (!rb_tree_resort(&t, n)) {
fprintf(stderr, "error in rb_tree_resort\n");
return 1;
}
if (!rb_tree_check(&t)) {
fprintf(stderr, "rb_tree_check_failed after change %d!\n", N - M + 1);
return 1;
}
k[i] = -1 - knew;
}
if (t.N != N) {
fprintf(stderr, "incorrect N (%d) in tree (vs. %d)\n", t.N, N);
return 1;
}
for (i = 0; i < N; ++i)
k[i] = -1 - k[i]; /* undo negation above */
for (i = 0; i < N; ++i) {
rb_node *le, *gt;
double lek, gtk;
kd = 0.01 * (rand() % (N * 150) - N * 25);
le = rb_tree_find_le(&t, &kd);
gt = rb_tree_find_gt(&t, &kd);
n = rb_tree_min(&t);
lek = le ? le->k[0] : -HUGE_VAL;
gtk = gt ? gt->k[0] : +HUGE_VAL;
printf("%g <= %g < %g\n", lek, kd, gtk);
if (n->k[0] > kd) {
if (le) {
fprintf(stderr, "found invalid le %g for %g\n", lek, kd);
return 1;
}
if (gt != n) {
fprintf(stderr, "gt is not first node for k=%g\n", kd);
return 1;
}
} else {
rb_node *succ = n;
do {
n = succ;
succ = rb_tree_succ(n);
} while (succ && succ->k[0] <= kd);
if (n != le) {
fprintf(stderr, "rb_tree_find_le gave wrong result for k=%g\n", kd);
return 1;
}
if (succ != gt) {
fprintf(stderr, "rb_tree_find_gt gave wrong result for k=%g\n", kd);
return 1;
}
}
}
for (M = N; M > 0; --M) {
j = rand() % M;
for (i = 0; i < N; ++i)
if (k[i] >= 0)
if (j-- == 0)
break;
if (i >= N)
abort();
kd = k[i];
if (!(n = rb_tree_find(&t, &kd))) {
fprintf(stderr, "rb_tree_find lost %d!\n", k[i]);
return 1;
}
n = rb_tree_remove(&t, n);
free(n->k);
free(n);
if (!rb_tree_check(&t)) {
fprintf(stderr, "rb_tree_check_failed after remove!\n");
return 1;
}
k[i] = -1 - k[i];
}
if (t.N != 0) {
fprintf(stderr, "nonzero N (%d) in tree at end\n", t.N);
return 1;
}
rb_tree_destroy(&t);
free(k);
printf("SUCCESS.\n");
return 0;
}