EXODUS/packages/seacas/libraries/chaco/optimize/opt3d.c

/*
 * Copyright(C) 1999-2020, 2023 National Technology & Engineering Solutions
 * of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
 * NTESS, the U.S. Government retains certain rights in this software.
 *
 * See packages/seacas/LICENSE for details
 */

#include "defs.h"
#include "structs.h"
#include <math.h>
#include <stdio.h>

void opt3d(struct vtx_data **graph,   /* data structure containing vertex weights */
           double          **yvecs,   /* eigenvectors */
           int               nvtxs,   /* total number of vertices */
           int               nmyvtxs, /* number of vertices I own */
           double           *vwsqrt,  /* square root of vertex weights */
           double *ptheta, double *pphi, double *pgamma, /* return optimal angles */
           int using_vwgts                               /* are vertex weights being used? */
)

/* Compute rotation angle to minimize distance to discrete points. */
{
  extern int DEBUG_OPTIMIZE;     /* debug flag for optimization */
  extern int OPT3D_NTRIES;       /* number of local opts to find global min */
  double    *aptr, *bptr, *cptr; /* loop through yvecs */
  double    *wsptr;              /* loops through vwsqrt */
  double     coeffs[25];         /* various products of yvecs */
  double     vars[3];            /* angular variables */
  double     best[3];            /* best minimizer found so far */
  double     grad[3];            /* gradiant of the function */
  double     gradc[3];           /* gradiant of the constraint */
  double     hess[3][3];         /* hessian of the function */
  double     hessc[3][3];        /* hessian of the constraint */
  double     step[3];            /* Newton step in optimization */
  double     grad_norm;          /* norm of the gradient */
  double     grad_min;           /* acceptable gradient for convergence */
  double     a, b, c;            /* temporary values */
  double     funcf = 0.0, funcc; /* values of function to be minimized */
  double     step_size;          /* norm of step */
  double     step_max;           /* maximum allowed step */
  double     step_min;           /* minimum step => convergence */
  double     early_step_min;     /* min step for early convergence stages */
  double     final_step_min;     /* min step for final convergence */
  double     hess_min;           /* value for hessian if < 0 */
  double     hess_tol;           /* smallest possible positive hess_min */
  double     hfact;              /* scales minimum tolerated hessian */
  double     w, ws = 0;          /* vertex weight squared or to the 1.5 */
  double     mult;               /* multiplier for constraint violation */
  double     max_constraint;     /* maximum allowed value for constraint */
  double     eval;               /* smallest eigenvalue of Hessian */
  double     pdtol;              /* eval < tol considered to be 0 */
  double     mfactor;            /* scaling for constraint growth */
  double     mstart;             /* starting value for constraint scaling */
  double     bestf;              /* value of best minimizer so far */
  double     res;                /* returned eigen-residual */
  int        pdflag;             /* converging to non-minimum? */
  int        inner;              /* number of iterations at each stage */
  int        inner1;
  int        total;            /* total number of iterations */
  int        ntries, maxtries; /* number of local minimizations */
  int        i, j;             /* loop counter */

  /* Set parameters. */
  best[0] = best[1] = best[2] = 0.0;
  a                           = sqrt((double)nvtxs);
  step_max                    = PI / 4;
  early_step_min              = 2.0e-4;
  final_step_min              = early_step_min / 10;
  grad_min                    = 1.0e-7;
  hfact                       = 2;
  hess_tol                    = 1.0e-6;
  pdtol                       = 1.0e-7;
  max_constraint              = 1.0e-12 * a;
  mfactor                     = 20.0;
  mstart                      = 5.0 * a;

  for (i = 0; i < 25; i++) {
    coeffs[i] = 0;
  }

  aptr  = yvecs[1] + 1;
  bptr  = yvecs[2] + 1;
  cptr  = yvecs[3] + 1;
  wsptr = vwsqrt + 1;
  for (i = 1; i <= nmyvtxs; i++) {
    a = *aptr++;
    b = *bptr++;
    c = *cptr++;
    w = graph[i]->vwgt;
    if (using_vwgts) {
      ws = *wsptr++;
    }
    if (w == 1) {
      coeffs[0] += a * a * a * a;
      coeffs[1] += b * b * b * b;
      coeffs[2] += c * c * c * c;
      coeffs[3] += a * a * a * b;
      coeffs[4] += a * a * b * b;
      coeffs[5] += a * b * b * b;
      coeffs[6] += a * a * a * c;
      coeffs[7] += a * a * c * c;
      coeffs[8] += a * c * c * c;
      coeffs[9] += b * b * b * c;
      coeffs[10] += b * b * c * c;
      coeffs[11] += b * c * c * c;
      coeffs[12] += a * a * b * c;
      coeffs[13] += a * b * b * c;
      coeffs[14] += a * b * c * c;

      coeffs[15] += a * a * a;
      coeffs[16] += b * b * b;
      coeffs[17] += c * c * c;
      coeffs[18] += a * a * b;
      coeffs[19] += a * a * c;
      coeffs[20] += a * b * b;
      coeffs[21] += b * b * c;
      coeffs[22] += a * c * c;
      coeffs[23] += b * c * c;
      coeffs[24] += a * b * c;
    }
    else {
      w  = 1 / (w * w);
      ws = 1 / ws;
      coeffs[0] += a * a * a * a * w;
      coeffs[1] += b * b * b * b * w;
      coeffs[2] += c * c * c * c * w;
      coeffs[3] += a * a * a * b * w;
      coeffs[4] += a * a * b * b * w;
      coeffs[5] += a * b * b * b * w;
      coeffs[6] += a * a * a * c * w;
      coeffs[7] += a * a * c * c * w;
      coeffs[8] += a * c * c * c * w;
      coeffs[9] += b * b * b * c * w;
      coeffs[10] += b * b * c * c * w;
      coeffs[11] += b * c * c * c * w;
      coeffs[12] += a * a * b * c * w;
      coeffs[13] += a * b * b * c * w;
      coeffs[14] += a * b * c * c * w;

      coeffs[15] += a * a * a * ws;
      coeffs[16] += b * b * b * ws;
      coeffs[17] += c * c * c * ws;
      coeffs[18] += a * a * b * ws;
      coeffs[19] += a * a * c * ws;
      coeffs[20] += a * b * b * ws;
      coeffs[21] += b * b * c * ws;
      coeffs[22] += a * c * c * ws;
      coeffs[23] += b * c * c * ws;
      coeffs[24] += a * b * c * ws;
    }
  }

  /* Adjust for normalization of eigenvectors. */
  /* This should make convergence criteria insensitive to problem size. */
  /* Note that the relative sizes of funcf and funcc depend on normalization of
     eigenvectors, and I'm assuming them normalized to 1. */
  for (i = 0; i < 15; i++) {
    coeffs[i] *= nvtxs;
  }
  a = sqrt((double)nvtxs);
  for (i = 15; i < 25; i++) {
    coeffs[i] *= a;
  }

  bestf    = 0;
  maxtries = OPT3D_NTRIES;
  for (ntries = 1; ntries <= maxtries; ntries++) {
    /* Initialize the starting guess randomly. */
    vars[0] = TWOPI * (drandom() - .5);
    vars[1] = acos(2.0 * drandom() - 1.0) - HALFPI;
    vars[2] = TWOPI * (drandom() - .5);

    inner1   = 0;
    total    = 0;
    mult     = mstart;
    step_min = early_step_min;
    funcc    = max_constraint;
    while (funcc >= max_constraint && total < 70) {
      inner     = 0;
      step_size = step_min;
      pdflag    = FALSE;
      grad_norm = 0;
      while (step_size >= step_min && (!pdflag || grad_norm > grad_min) && inner < 15) {
        funcf = func3d(coeffs, vars[0], vars[1], vars[2]);
        grad3d(coeffs, grad, vars[0], vars[1], vars[2]);
        hess3d(coeffs, hess);

        /* Compute contribution of constraint term. */
        funcc = constraint(&coeffs[15]);
        /* func = funcf + mult*funcc; */
        gradcon(&coeffs[15], gradc);
        hesscon(&coeffs[15], hessc);

        /* If in final pass, tighten convergence criterion. */
        if (funcc < max_constraint) {
          step_min = final_step_min;
        }

        for (i = 0; i < 3; i++) {
          /* Note: I'm taking negative of gradient here. */
          grad[i] = -grad[i] - mult * gradc[i];
          for (j = 0; j < 3; j++) {
            hess[i][j] += mult * hessc[i][j];
          }
        }

        grad_norm = fabs(grad[0]) + fabs(grad[1]) + fabs(grad[2]);
        hess_min  = hfact * grad_norm / step_max;
        if (hess_min < hess_tol) {
          hess_min = hess_tol;
        }

        /* Find smallest eigenvalue of hess. */
        ch_evals3(hess, &eval, &res, &res);

        /* If eval < 0, add to diagonal to make pos def. */
        if (eval < -pdtol) {
          pdflag = FALSE;
        }
        else {
          pdflag = TRUE;
        }

        if (eval < hess_min) {
          for (i = 0; i < 3; i++) {
            hess[i][i] += hess_min - eval;
          }
        }

        /* Now solve linear system for step sizes. */
        kramer3(hess, grad, step);

        /* Scale step down if too big. */
        step_size = fabs(step[0]) + fabs(step[1]) + fabs(step[2]);
        if (step_size > step_max) {
          a = step_max / step_size;
          for (i = 0; i < 3; i++) {
            step[i] *= a;
          }
        }

        if ((step_size < step_min || grad_norm < grad_min) && !pdflag) {
          /* Convergence to non-min. */
          for (i = 0; i < 3; i++) {
            hess[i][i] -= hess_min - eval;
          }
          ch_eigenvec3(hess, eval, step, &res);
          step_size = fabs(step[0]) + fabs(step[1]) + fabs(step[2]);
          a         = step_min / step_size;
          for (i = 0; i < 3; i++) {
            step[i] *= a;
          }
          step_size = step_min;
        }
        for (i = 0; i < 3; i++) {
          vars[i] += step[i];
        }
        inner++;
      }
      if (inner1 == 0) {
        inner1 = inner;
      }
      total += inner;
      mult *= mfactor;
    }

    if (DEBUG_OPTIMIZE > 0) {
      printf("On try %d, After %d (%d) passes, funcf=%e, funcc=%e (%f, %f, %f)\n", ntries, total,
             inner1, funcf, funcc, vars[0], vars[1], vars[2]);
    }

    if (ntries == 1 || funcf < bestf) {
      bestf = funcf;
      for (i = 0; i < 3; i++) {
        best[i] = vars[i];
      }
    }
  }
  *ptheta = best[0];
  *pphi   = best[1];
  *pgamma = best[2];
}