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