src/gromacs/linearalgebra/nrjac.cpp

   1 /*
   2  * This file is part of the GROMACS molecular simulation package.
   3  *
   4  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
   5  * Copyright (c) 2001-2004, The GROMACS development team.
   6  * Copyright (c) 2012,2014,2015,2017,2018 by the GROMACS development team.
   7  * Copyright (c) 2019,2020, by the GROMACS development team, led by
   8  * Mark Abraham, David van der Spoel, Berk Hess, and Erik Lindahl,
   9  * and including many others, as listed in the AUTHORS file in the
  10  * top-level source directory and at http://www.gromacs.org.
  11  *
  12  * GROMACS is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU Lesser General Public License
  14  * as published by the Free Software Foundation; either version 2.1
  15  * of the License, or (at your option) any later version.
  16  *
  17  * GROMACS is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  20  * Lesser General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU Lesser General Public
  23  * License along with GROMACS; if not, see
  24  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
  25  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
  26  *
  27  * If you want to redistribute modifications to GROMACS, please
  28  * consider that scientific software is very special. Version
  29  * control is crucial - bugs must be traceable. We will be happy to
  30  * consider code for inclusion in the official distribution, but
  31  * derived work must not be called official GROMACS. Details are found
  32  * in the README & COPYING files - if they are missing, get the
  33  * official version at http://www.gromacs.org.
  34  *
  35  * To help us fund GROMACS development, we humbly ask that you cite
  36  * the research papers on the package. Check out http://www.gromacs.org.
  37  */
  38 /* This file is completely threadsafe - keep it that way! */
  39 #include "gmxpre.h"
  40
  41 #include "nrjac.h"
  42
  43 #include <cmath>
  44
  45 #include "gromacs/utility/fatalerror.h"
  46 #include "gromacs/utility/smalloc.h"
  47
  48 static inline void do_rotate(double** a, int i, int j, int k, int l, double tau, double s)
  49 {
  50     double g, h;
  51     g       = a[i][j];
  52     h       = a[k][l];
  53     a[i][j] = g - s * (h + g * tau);
  54     a[k][l] = h + s * (g - h * tau);
  55 }
  56
  57 void jacobi(double** a, int n, double d[], double** v, int* nrot)
  58 {
  59     int    j, i;
  60     int    iq, ip;
  61     double tresh, theta, tau, t, sm, s, h, g, c, *b, *z;
  62
  63     snew(b, n);
  64     snew(z, n);
  65     for (ip = 0; ip < n; ip++)
  66     {
  67         for (iq = 0; iq < n; iq++)
  68         {
  69             v[ip][iq] = 0.0;
  70         }
  71         v[ip][ip] = 1.0;
  72     }
  73     for (ip = 0; ip < n; ip++)
  74     {
  75         b[ip] = d[ip] = a[ip][ip];
  76         z[ip]         = 0.0;
  77     }
  78     *nrot = 0;
  79     for (i = 1; i <= 50; i++)
  80     {
  81         sm = 0.0;
  82         for (ip = 0; ip < n - 1; ip++)
  83         {
  84             for (iq = ip + 1; iq < n; iq++)
  85             {
  86                 sm += std::abs(a[ip][iq]);
  87             }
  88         }
  89         if (sm == 0.0)
  90         {
  91             sfree(z);
  92             sfree(b);
  93             return;
  94         }
  95         if (i < 4)
  96         {
  97             tresh = 0.2 * sm / (n * n);
  98         }
  99         else
 100         {
 101             tresh = 0.0;
 102         }
 103         for (ip = 0; ip < n - 1; ip++)
 104         {
 105             for (iq = ip + 1; iq < n; iq++)
 106             {
 107                 g = 100.0 * std::abs(a[ip][iq]);
 108                 if (i > 4 && std::abs(d[ip]) + g == std::abs(d[ip])
 109                     && std::abs(d[iq]) + g == std::abs(d[iq]))
 110                 {
 111                     a[ip][iq] = 0.0;
 112                 }
 113                 else if (std::abs(a[ip][iq]) > tresh)
 114                 {
 115                     h = d[iq] - d[ip];
 116                     if (std::abs(h) + g == std::abs(h))
 117                     {
 118                         t = (a[ip][iq]) / h;
 119                     }
 120                     else
 121                     {
 122                         theta = 0.5 * h / (a[ip][iq]);
 123                         t     = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
 124                         if (theta < 0.0)
 125                         {
 126                             t = -t;
 127                         }
 128                     }
 129                     c   = 1.0 / std::sqrt(1 + t * t);
 130                     s   = t * c;
 131                     tau = s / (1.0 + c);
 132                     h   = t * a[ip][iq];
 133                     z[ip] -= h;
 134                     z[iq] += h;
 135                     d[ip] -= h;
 136                     d[iq] += h;
 137                     a[ip][iq] = 0.0;
 138                     for (j = 0; j < ip; j++)
 139                     {
 140                         do_rotate(a, j, ip, j, iq, tau, s);
 141                     }
 142                     for (j = ip + 1; j < iq; j++)
 143                     {
 144                         do_rotate(a, ip, j, j, iq, tau, s);
 145                     }
 146                     for (j = iq + 1; j < n; j++)
 147                     {
 148                         do_rotate(a, ip, j, iq, j, tau, s);
 149                     }
 150                     for (j = 0; j < n; j++)
 151                     {
 152                         do_rotate(v, j, ip, j, iq, tau, s);
 153                     }
 154                     ++(*nrot);
 155                 }
 156             }
 157         }
 158         for (ip = 0; ip < n; ip++)
 159         {
 160             b[ip] += z[ip];
 161             d[ip] = b[ip];
 162             z[ip] = 0.0;
 163         }
 164     }
 165     gmx_fatal(FARGS, "Error: Too many iterations in routine JACOBI\n");
 166 }
 167
 168 int m_inv_gen(real* m, int n, real* minv)
 169 {
 170     double **md, **v, *eig, tol, s;
 171     int      nzero, i, j, k, nrot;
 172
 173     snew(md, n);
 174     for (i = 0; i < n; i++)
 175     {
 176         snew(md[i], n);
 177     }
 178     snew(v, n);
 179     for (i = 0; i < n; i++)
 180     {
 181         snew(v[i], n);
 182     }
 183     snew(eig, n);
 184     for (i = 0; i < n; i++)
 185     {
 186         for (j = 0; j < n; j++)
 187         {
 188             md[i][j] = m[i * n + j];
 189         }
 190     }
 191
 192     tol = 0;
 193     for (i = 0; i < n; i++)
 194     {
 195         tol += std::abs(md[i][i]);
 196     }
 197     tol = 1e-6 * tol / n;
 198
 199     jacobi(md, n, eig, v, &nrot);
 200
 201     nzero = 0;
 202     for (i = 0; i < n; i++)
 203     {
 204         if (std::abs(eig[i]) < tol)
 205         {
 206             eig[i] = 0;
 207             nzero++;
 208         }
 209         else
 210         {
 211             eig[i] = 1.0 / eig[i];
 212         }
 213     }
 214
 215     for (i = 0; i < n; i++)
 216     {
 217         for (j = 0; j < n; j++)
 218         {
 219             s = 0;
 220             for (k = 0; k < n; k++)
 221             {
 222                 s += eig[k] * v[i][k] * v[j][k];
 223             }
 224             minv[i * n + j] = s;
 225         }
 226     }
 227
 228     sfree(eig);
 229     for (i = 0; i < n; i++)
 230     {
 231         sfree(v[i]);
 232     }
 233     sfree(v);
 234     for (i = 0; i < n; i++)
 235     {
 236         sfree(md[i]);
 237     }
 238     sfree(md);
 239
 240     return nzero;
 241 }