src/gromacs/math/vec.h

   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) 2013,2014, 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 #ifndef GMX_MATH_VEC_H
  38 #define GMX_MATH_VEC_H
  39
  40 /*
  41    collection of in-line ready operations:
  42
  43    lookup-table optimized scalar operations:
  44    real gmx_invsqrt(real x)
  45    real sqr(real x)
  46    double dsqr(double x)
  47
  48    vector operations:
  49    void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  50    void dvec_add(const dvec a,const dvec b,dvec c)  c = a + b
  51    void ivec_add(const ivec a,const ivec b,ivec c)  c = a + b
  52    void rvec_inc(rvec a,const rvec b)               a += b
  53    void dvec_inc(dvec a,const dvec b)               a += b
  54    void ivec_inc(ivec a,const ivec b)               a += b
  55    void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  56    void dvec_sub(const dvec a,const dvec b,dvec c)  c = a - b
  57    void rvec_dec(rvec a,rvec b)                     a -= b
  58    void copy_rvec(const rvec a,rvec b)              b = a (reals)
  59    void copy_dvec(const dvec a,dvec b)              b = a (reals)
  60    void copy_ivec(const ivec a,ivec b)              b = a (integers)
  61    void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  62    void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  63    void dsvmul(double a,dvec v1,dvec v2)            v2 = a * v1
  64    void clear_rvec(rvec a)                          a = 0
  65    void clear_dvec(dvec a)                          a = 0
  66    void clear_ivec(rvec a)                          a = 0
  67    void clear_rvecs(int n,rvec v[])
  68    real iprod(rvec a,rvec b)                        = a . b (inner product)
  69    double diprod(dvec a,dvec b)                     = a . b (inner product)
  70    real iiprod(ivec a,ivec b)                       = a . b (integers)
  71    real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  72    double dnorm2(dvec a)                            = | a |^2 ( = x*y*z )
  73    real norm(rvec a)                                = | a |
  74    double dnorm(dvec a)                             = | a |
  75    void cprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  76    void dprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  77    void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  78    real cos_angle(rvec a,rvec b)
  79    real cos_angle_no_table(rvec a,rvec b)
  80    real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  81    void unitv(rvec src,rvec dest)                   dest = src / |src|
  82    void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
  83
  84    matrix (3x3) operations:
  85     ! indicates that dest should not be the same as a, b or src
  86     the _ur0 varieties work on matrices that have only zeros
  87     in the upper right part, such as box matrices, these varieties
  88     could produce less rounding errors, not due to the operations themselves,
  89     but because the compiler can easier recombine the operations
  90    void copy_mat(matrix a,matrix b)                 b = a
  91    void clear_mat(matrix a)                         a = 0
  92    void mmul(matrix a,matrix b,matrix dest)      !  dest = a . b
  93    void mmul_ur0(matrix a,matrix b,matrix dest)     dest = a . b
  94    void transpose(matrix src,matrix dest)        !  dest = src*
  95    void tmmul(matrix a,matrix b,matrix dest)     !  dest = a* . b
  96    void mtmul(matrix a,matrix b,matrix dest)     !  dest = a . b*
  97    real det(matrix a)                               = det(a)
  98    void m_add(matrix a,matrix b,matrix dest)        dest = a + b
  99    void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
 100    void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
 101    void m_inv_ur0(matrix src,matrix dest)           dest = src^-1
 102    void m_inv(matrix src,matrix dest)            !  dest = src^-1
 103    void mvmul(matrix a,rvec src,rvec dest)       !  dest = a . src
 104    void mvmul_ur0(matrix a,rvec src,rvec dest)      dest = a . src
 105    void tmvmul_ur0(matrix a,rvec src,rvec dest)     dest = a* . src
 106    real trace(matrix m)                             = trace(m)
 107  */
 108
 109 /* The file only depends on config.h for GMX_SOFTWARE_INVSQRT and
 110    HAVE_*SQRT*. This is no problem with public headers because
 111    it is OK if user code uses a different rsqrt implementation */
 112 #ifdef HAVE_CONFIG_H
 113 #include <config.h>
 114 #endif
 115
 116 #include <math.h>
 117
 118 #include "../legacyheaders/physics.h"
 119
 120 #include "utilities.h"
 121 #include "vectypes.h"
 122
 123 #include "../utility/basedefinitions.h"
 124 #include "../utility/fatalerror.h"
 125 #include "../utility/real.h"
 126
 127 #ifdef __cplusplus
 128 extern "C" {
 129 #elif 0
 130 } /* avoid screwing up indentation */
 131 #endif
 132
 133 #ifdef GMX_SOFTWARE_INVSQRT
 134 #define EXP_LSB         0x00800000
 135 #define EXP_MASK        0x7f800000
 136 #define EXP_SHIFT       23
 137 #define FRACT_MASK      0x007fffff
 138 #define FRACT_SIZE      11              /* significant part of fraction */
 139 #define FRACT_SHIFT     (EXP_SHIFT-FRACT_SIZE)
 140 #define EXP_ADDR(val)   (((val)&EXP_MASK)>>EXP_SHIFT)
 141 #define FRACT_ADDR(val) (((val)&(FRACT_MASK|EXP_LSB))>>FRACT_SHIFT)
 142
 143 extern const unsigned int *gmx_invsqrt_exptab;
 144 extern const unsigned int *gmx_invsqrt_fracttab;
 145
 146 typedef union
 147 {
 148     unsigned int bval;
 149     float        fval;
 150 } t_convert;
 151
 152 static gmx_inline real gmx_software_invsqrt(real x)
 153 {
 154     const real   half  = 0.5;
 155     const real   three = 3.0;
 156     t_convert    result, bit_pattern;
 157     unsigned int exp, fract;
 158     real         lu;
 159     real         y;
 160 #ifdef GMX_DOUBLE
 161     real         y2;
 162 #endif
 163
 164     bit_pattern.fval = x;
 165     exp              = EXP_ADDR(bit_pattern.bval);
 166     fract            = FRACT_ADDR(bit_pattern.bval);
 167     result.bval      = gmx_invsqrt_exptab[exp] | gmx_invsqrt_fracttab[fract];
 168     lu               = result.fval;
 169
 170     y = (half*lu*(three-((x*lu)*lu)));
 171 #ifdef GMX_DOUBLE
 172     y2 = (half*y*(three-((x*y)*y)));
 173
 174     return y2;                  /* 10 Flops */
 175 #else
 176     return y;                   /* 5  Flops */
 177 #endif
 178 }
 179 #define gmx_invsqrt(x) gmx_software_invsqrt(x)
 180 #define INVSQRT_DONE
 181 #endif /* gmx_invsqrt */
 182
 183 #ifndef INVSQRT_DONE
 184 #    ifdef GMX_DOUBLE
 185 #        ifdef HAVE_RSQRT
 186 #            define gmx_invsqrt(x)     rsqrt(x)
 187 #        else
 188 #            define gmx_invsqrt(x)     (1.0/sqrt(x))
 189 #        endif
 190 #    else /* single */
 191 #        ifdef HAVE_RSQRTF
 192 #            define gmx_invsqrt(x)     rsqrtf(x)
 193 #        elif defined HAVE_RSQRT
 194 #            define gmx_invsqrt(x)     rsqrt(x)
 195 #        elif defined HAVE_SQRTF
 196 #            define gmx_invsqrt(x)     (1.0/sqrtf(x))
 197 #        else
 198 #            define gmx_invsqrt(x)     (1.0/sqrt(x))
 199 #        endif
 200 #    endif
 201 #endif
 202
 203
 204 static gmx_inline real sqr(real x)
 205 {
 206     return (x*x);
 207 }
 208
 209 static gmx_inline double dsqr(double x)
 210 {
 211     return (x*x);
 212 }
 213
 214 /* Maclaurin series for sinh(x)/x, useful for NH chains and MTTK pressure control
 215    Here, we compute it to 10th order, which might be overkill, 8th is probably enough,
 216    but it's not very much more expensive. */
 217
 218 static gmx_inline real series_sinhx(real x)
 219 {
 220     real x2 = x*x;
 221     return (1 + (x2/6.0)*(1 + (x2/20.0)*(1 + (x2/42.0)*(1 + (x2/72.0)*(1 + (x2/110.0))))));
 222 }
 223
 224 static gmx_inline void rvec_add(const rvec a, const rvec b, rvec c)
 225 {
 226     real x, y, z;
 227
 228     x = a[XX]+b[XX];
 229     y = a[YY]+b[YY];
 230     z = a[ZZ]+b[ZZ];
 231
 232     c[XX] = x;
 233     c[YY] = y;
 234     c[ZZ] = z;
 235 }
 236
 237 static gmx_inline void dvec_add(const dvec a, const dvec b, dvec c)
 238 {
 239     double x, y, z;
 240
 241     x = a[XX]+b[XX];
 242     y = a[YY]+b[YY];
 243     z = a[ZZ]+b[ZZ];
 244
 245     c[XX] = x;
 246     c[YY] = y;
 247     c[ZZ] = z;
 248 }
 249
 250 static gmx_inline void ivec_add(const ivec a, const ivec b, ivec c)
 251 {
 252     int x, y, z;
 253
 254     x = a[XX]+b[XX];
 255     y = a[YY]+b[YY];
 256     z = a[ZZ]+b[ZZ];
 257
 258     c[XX] = x;
 259     c[YY] = y;
 260     c[ZZ] = z;
 261 }
 262
 263 static gmx_inline void rvec_inc(rvec a, const rvec b)
 264 {
 265     real x, y, z;
 266
 267     x = a[XX]+b[XX];
 268     y = a[YY]+b[YY];
 269     z = a[ZZ]+b[ZZ];
 270
 271     a[XX] = x;
 272     a[YY] = y;
 273     a[ZZ] = z;
 274 }
 275
 276 static gmx_inline void dvec_inc(dvec a, const dvec b)
 277 {
 278     double x, y, z;
 279
 280     x = a[XX]+b[XX];
 281     y = a[YY]+b[YY];
 282     z = a[ZZ]+b[ZZ];
 283
 284     a[XX] = x;
 285     a[YY] = y;
 286     a[ZZ] = z;
 287 }
 288
 289 static gmx_inline void rvec_sub(const rvec a, const rvec b, rvec c)
 290 {
 291     real x, y, z;
 292
 293     x = a[XX]-b[XX];
 294     y = a[YY]-b[YY];
 295     z = a[ZZ]-b[ZZ];
 296
 297     c[XX] = x;
 298     c[YY] = y;
 299     c[ZZ] = z;
 300 }
 301
 302 static gmx_inline void dvec_sub(const dvec a, const dvec b, dvec c)
 303 {
 304     double x, y, z;
 305
 306     x = a[XX]-b[XX];
 307     y = a[YY]-b[YY];
 308     z = a[ZZ]-b[ZZ];
 309
 310     c[XX] = x;
 311     c[YY] = y;
 312     c[ZZ] = z;
 313 }
 314
 315 static gmx_inline void rvec_dec(rvec a, const rvec b)
 316 {
 317     real x, y, z;
 318
 319     x = a[XX]-b[XX];
 320     y = a[YY]-b[YY];
 321     z = a[ZZ]-b[ZZ];
 322
 323     a[XX] = x;
 324     a[YY] = y;
 325     a[ZZ] = z;
 326 }
 327
 328 static gmx_inline void copy_rvec(const rvec a, rvec b)
 329 {
 330     b[XX] = a[XX];
 331     b[YY] = a[YY];
 332     b[ZZ] = a[ZZ];
 333 }
 334
 335 static gmx_inline void copy_rvecn(rvec *a, rvec *b, int startn, int endn)
 336 {
 337     int i;
 338     for (i = startn; i < endn; i++)
 339     {
 340         b[i][XX] = a[i][XX];
 341         b[i][YY] = a[i][YY];
 342         b[i][ZZ] = a[i][ZZ];
 343     }
 344 }
 345
 346 static gmx_inline void copy_dvec(const dvec a, dvec b)
 347 {
 348     b[XX] = a[XX];
 349     b[YY] = a[YY];
 350     b[ZZ] = a[ZZ];
 351 }
 352
 353 static gmx_inline void copy_ivec(const ivec a, ivec b)
 354 {
 355     b[XX] = a[XX];
 356     b[YY] = a[YY];
 357     b[ZZ] = a[ZZ];
 358 }
 359
 360 static gmx_inline void ivec_sub(const ivec a, const ivec b, ivec c)
 361 {
 362     int x, y, z;
 363
 364     x = a[XX]-b[XX];
 365     y = a[YY]-b[YY];
 366     z = a[ZZ]-b[ZZ];
 367
 368     c[XX] = x;
 369     c[YY] = y;
 370     c[ZZ] = z;
 371 }
 372
 373 static gmx_inline void copy_mat(matrix a, matrix b)
 374 {
 375     copy_rvec(a[XX], b[XX]);
 376     copy_rvec(a[YY], b[YY]);
 377     copy_rvec(a[ZZ], b[ZZ]);
 378 }
 379
 380 static gmx_inline void svmul(real a, const rvec v1, rvec v2)
 381 {
 382     v2[XX] = a*v1[XX];
 383     v2[YY] = a*v1[YY];
 384     v2[ZZ] = a*v1[ZZ];
 385 }
 386
 387 static gmx_inline void dsvmul(double a, const dvec v1, dvec v2)
 388 {
 389     v2[XX] = a*v1[XX];
 390     v2[YY] = a*v1[YY];
 391     v2[ZZ] = a*v1[ZZ];
 392 }
 393
 394 static gmx_inline real distance2(const rvec v1, const rvec v2)
 395 {
 396     return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 397 }
 398
 399 static gmx_inline void clear_rvec(rvec a)
 400 {
 401     /* The ibm compiler has problems with inlining this
 402      * when we use a const real variable
 403      */
 404     a[XX] = 0.0;
 405     a[YY] = 0.0;
 406     a[ZZ] = 0.0;
 407 }
 408
 409 static gmx_inline void clear_dvec(dvec a)
 410 {
 411     /* The ibm compiler has problems with inlining this
 412      * when we use a const real variable
 413      */
 414     a[XX] = 0.0;
 415     a[YY] = 0.0;
 416     a[ZZ] = 0.0;
 417 }
 418
 419 static gmx_inline void clear_ivec(ivec a)
 420 {
 421     a[XX] = 0;
 422     a[YY] = 0;
 423     a[ZZ] = 0;
 424 }
 425
 426 static gmx_inline void clear_rvecs(int n, rvec v[])
 427 {
 428     int i;
 429
 430     for (i = 0; (i < n); i++)
 431     {
 432         clear_rvec(v[i]);
 433     }
 434 }
 435
 436 static gmx_inline void clear_mat(matrix a)
 437 {
 438     const real nul = 0.0;
 439
 440     a[XX][XX] = a[XX][YY] = a[XX][ZZ] = nul;
 441     a[YY][XX] = a[YY][YY] = a[YY][ZZ] = nul;
 442     a[ZZ][XX] = a[ZZ][YY] = a[ZZ][ZZ] = nul;
 443 }
 444
 445 static gmx_inline real iprod(const rvec a, const rvec b)
 446 {
 447     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 448 }
 449
 450 static gmx_inline double diprod(const dvec a, const dvec b)
 451 {
 452     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 453 }
 454
 455 static gmx_inline int iiprod(const ivec a, const ivec b)
 456 {
 457     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 458 }
 459
 460 static gmx_inline real norm2(const rvec a)
 461 {
 462     return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 463 }
 464
 465 static gmx_inline double dnorm2(const dvec a)
 466 {
 467     return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 468 }
 469
 470 /* WARNING:
 471  * As dnorm() uses sqrt() (which is slow) _only_ use it if you are sure you
 472  * don't need 1/dnorm(), otherwise use dnorm2()*dinvnorm(). */
 473 static gmx_inline double dnorm(const dvec a)
 474 {
 475     return sqrt(diprod(a, a));
 476 }
 477
 478 /* WARNING:
 479  * As norm() uses sqrtf() (which is slow) _only_ use it if you are sure you
 480  * don't need 1/norm(), otherwise use norm2()*invnorm(). */
 481 static gmx_inline real norm(const rvec a)
 482 {
 483     /* This is ugly, but we deliberately do not define gmx_sqrt() and handle the
 484      * float/double case here instead to avoid gmx_sqrt() being accidentally used. */
 485 #ifdef GMX_DOUBLE
 486     return dnorm(a);
 487 #elif defined HAVE_SQRTF
 488     return sqrtf(iprod(a, a));
 489 #else
 490     return sqrt(iprod(a, a));
 491 #endif
 492 }
 493
 494 static gmx_inline real invnorm(const rvec a)
 495 {
 496     return gmx_invsqrt(norm2(a));
 497 }
 498
 499 static gmx_inline real dinvnorm(const dvec a)
 500 {
 501     return gmx_invsqrt(dnorm2(a));
 502 }
 503
 504 /* WARNING:
 505  * Do _not_ use these routines to calculate the angle between two vectors
 506  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 507  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 508  * Instead, use the new gmx_angle() function directly.
 509  */
 510 static gmx_inline real
 511 cos_angle(const rvec a, const rvec b)
 512 {
 513     /*
 514      *                  ax*bx + ay*by + az*bz
 515      * cos-vec (a,b) =  ---------------------
 516      *                      ||a|| * ||b||
 517      */
 518     real   cosval;
 519     int    m;
 520     double aa, bb, ip, ipa, ipb, ipab; /* For accuracy these must be double! */
 521
 522     ip = ipa = ipb = 0.0;
 523     for (m = 0; (m < DIM); m++) /* 18 */
 524     {
 525         aa   = a[m];
 526         bb   = b[m];
 527         ip  += aa*bb;
 528         ipa += aa*aa;
 529         ipb += bb*bb;
 530     }
 531     ipab = ipa*ipb;
 532     if (ipab > 0)
 533     {
 534         cosval = ip*gmx_invsqrt(ipab);  /*  7 */
 535     }
 536     else
 537     {
 538         cosval = 1;
 539     }
 540     /* 25 TOTAL */
 541     if (cosval > 1.0)
 542     {
 543         return 1.0;
 544     }
 545     if (cosval < -1.0)
 546     {
 547         return -1.0;
 548     }
 549
 550     return cosval;
 551 }
 552
 553 /* WARNING:
 554  * Do _not_ use these routines to calculate the angle between two vectors
 555  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 556  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 557  * Instead, use the new gmx_angle() function directly.
 558  */
 559 static gmx_inline real
 560 cos_angle_no_table(const rvec a, const rvec b)
 561 {
 562     /* This version does not need the invsqrt lookup table */
 563     real   cosval;
 564     int    m;
 565     double aa, bb, ip, ipa, ipb; /* For accuracy these must be double! */
 566
 567     ip = ipa = ipb = 0.0;
 568     for (m = 0; (m < DIM); m++) /* 18 */
 569     {
 570         aa   = a[m];
 571         bb   = b[m];
 572         ip  += aa*bb;
 573         ipa += aa*aa;
 574         ipb += bb*bb;
 575     }
 576     cosval = ip/sqrt(ipa*ipb);  /* 12 */
 577     /* 30 TOTAL */
 578     if (cosval > 1.0)
 579     {
 580         return 1.0;
 581     }
 582     if (cosval < -1.0)
 583     {
 584         return -1.0;
 585     }
 586
 587     return cosval;
 588 }
 589
 590
 591 static gmx_inline void cprod(const rvec a, const rvec b, rvec c)
 592 {
 593     c[XX] = a[YY]*b[ZZ]-a[ZZ]*b[YY];
 594     c[YY] = a[ZZ]*b[XX]-a[XX]*b[ZZ];
 595     c[ZZ] = a[XX]*b[YY]-a[YY]*b[XX];
 596 }
 597
 598 static gmx_inline void dcprod(const dvec a, const dvec b, dvec c)
 599 {
 600     c[XX] = a[YY]*b[ZZ]-a[ZZ]*b[YY];
 601     c[YY] = a[ZZ]*b[XX]-a[XX]*b[ZZ];
 602     c[ZZ] = a[XX]*b[YY]-a[YY]*b[XX];
 603 }
 604
 605 /* This routine calculates the angle between a & b without any loss of accuracy close to 0/PI.
 606  * If you only need cos(theta), use the cos_angle() routines to save a few cycles.
 607  * This routine is faster than it might appear, since atan2 is accelerated on many CPUs (e.g. x86).
 608  */
 609 static gmx_inline real
 610 gmx_angle(const rvec a, const rvec b)
 611 {
 612     rvec w;
 613     real wlen, s;
 614
 615     cprod(a, b, w);
 616
 617     wlen  = norm(w);
 618     s     = iprod(a, b);
 619
 620     return atan2(wlen, s);
 621 }
 622
 623 static gmx_inline void mmul_ur0(matrix a, matrix b, matrix dest)
 624 {
 625     dest[XX][XX] = a[XX][XX]*b[XX][XX];
 626     dest[XX][YY] = 0.0;
 627     dest[XX][ZZ] = 0.0;
 628     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 629     dest[YY][YY] =                    a[YY][YY]*b[YY][YY];
 630     dest[YY][ZZ] = 0.0;
 631     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 632     dest[ZZ][YY] =                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 633     dest[ZZ][ZZ] =                                        a[ZZ][ZZ]*b[ZZ][ZZ];
 634 }
 635
 636 static gmx_inline void mmul(matrix a, matrix b, matrix dest)
 637 {
 638     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 639     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 640     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 641     dest[XX][YY] = a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 642     dest[YY][YY] = a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 643     dest[ZZ][YY] = a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 644     dest[XX][ZZ] = a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 645     dest[YY][ZZ] = a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 646     dest[ZZ][ZZ] = a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 647 }
 648
 649 static gmx_inline void transpose(matrix src, matrix dest)
 650 {
 651     dest[XX][XX] = src[XX][XX];
 652     dest[YY][XX] = src[XX][YY];
 653     dest[ZZ][XX] = src[XX][ZZ];
 654     dest[XX][YY] = src[YY][XX];
 655     dest[YY][YY] = src[YY][YY];
 656     dest[ZZ][YY] = src[YY][ZZ];
 657     dest[XX][ZZ] = src[ZZ][XX];
 658     dest[YY][ZZ] = src[ZZ][YY];
 659     dest[ZZ][ZZ] = src[ZZ][ZZ];
 660 }
 661
 662 static gmx_inline void tmmul(matrix a, matrix b, matrix dest)
 663 {
 664     /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 665     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 666     dest[XX][YY] = a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 667     dest[XX][ZZ] = a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 668     dest[YY][XX] = a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 669     dest[YY][YY] = a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 670     dest[YY][ZZ] = a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 671     dest[ZZ][XX] = a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 672     dest[ZZ][YY] = a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 673     dest[ZZ][ZZ] = a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 674 }
 675
 676 static gmx_inline void mtmul(matrix a, matrix b, matrix dest)
 677 {
 678     /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 679     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 680     dest[XX][YY] = a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 681     dest[XX][ZZ] = a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 682     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 683     dest[YY][YY] = a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 684     dest[YY][ZZ] = a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 685     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 686     dest[ZZ][YY] = a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 687     dest[ZZ][ZZ] = a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 688 }
 689
 690 static gmx_inline real det(matrix a)
 691 {
 692     return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 693              -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 694              +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 695 }
 696
 697
 698 static gmx_inline void m_add(matrix a, matrix b, matrix dest)
 699 {
 700     dest[XX][XX] = a[XX][XX]+b[XX][XX];
 701     dest[XX][YY] = a[XX][YY]+b[XX][YY];
 702     dest[XX][ZZ] = a[XX][ZZ]+b[XX][ZZ];
 703     dest[YY][XX] = a[YY][XX]+b[YY][XX];
 704     dest[YY][YY] = a[YY][YY]+b[YY][YY];
 705     dest[YY][ZZ] = a[YY][ZZ]+b[YY][ZZ];
 706     dest[ZZ][XX] = a[ZZ][XX]+b[ZZ][XX];
 707     dest[ZZ][YY] = a[ZZ][YY]+b[ZZ][YY];
 708     dest[ZZ][ZZ] = a[ZZ][ZZ]+b[ZZ][ZZ];
 709 }
 710
 711 static gmx_inline void m_sub(matrix a, matrix b, matrix dest)
 712 {
 713     dest[XX][XX] = a[XX][XX]-b[XX][XX];
 714     dest[XX][YY] = a[XX][YY]-b[XX][YY];
 715     dest[XX][ZZ] = a[XX][ZZ]-b[XX][ZZ];
 716     dest[YY][XX] = a[YY][XX]-b[YY][XX];
 717     dest[YY][YY] = a[YY][YY]-b[YY][YY];
 718     dest[YY][ZZ] = a[YY][ZZ]-b[YY][ZZ];
 719     dest[ZZ][XX] = a[ZZ][XX]-b[ZZ][XX];
 720     dest[ZZ][YY] = a[ZZ][YY]-b[ZZ][YY];
 721     dest[ZZ][ZZ] = a[ZZ][ZZ]-b[ZZ][ZZ];
 722 }
 723
 724 static gmx_inline void msmul(matrix m1, real r1, matrix dest)
 725 {
 726     dest[XX][XX] = r1*m1[XX][XX];
 727     dest[XX][YY] = r1*m1[XX][YY];
 728     dest[XX][ZZ] = r1*m1[XX][ZZ];
 729     dest[YY][XX] = r1*m1[YY][XX];
 730     dest[YY][YY] = r1*m1[YY][YY];
 731     dest[YY][ZZ] = r1*m1[YY][ZZ];
 732     dest[ZZ][XX] = r1*m1[ZZ][XX];
 733     dest[ZZ][YY] = r1*m1[ZZ][YY];
 734     dest[ZZ][ZZ] = r1*m1[ZZ][ZZ];
 735 }
 736
 737 static gmx_inline void m_inv_ur0(matrix src, matrix dest)
 738 {
 739     double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
 740     if (fabs(tmp) <= 100*GMX_REAL_MIN)
 741     {
 742         gmx_fatal(FARGS, "Can not invert matrix, determinant is zero");
 743     }
 744
 745     dest[XX][XX] = 1/src[XX][XX];
 746     dest[YY][YY] = 1/src[YY][YY];
 747     dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
 748     dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
 749                     - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
 750     dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
 751     dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
 752     dest[XX][YY] = 0.0;
 753     dest[XX][ZZ] = 0.0;
 754     dest[YY][ZZ] = 0.0;
 755 }
 756
 757 static gmx_inline void m_inv(matrix src, matrix dest)
 758 {
 759     const real smallreal = (real)1.0e-24;
 760     const real largereal = (real)1.0e24;
 761     real       deter, c, fc;
 762
 763     deter = det(src);
 764     c     = (real)1.0/deter;
 765     fc    = (real)fabs(c);
 766
 767     if ((fc <= smallreal) || (fc >= largereal))
 768     {
 769         gmx_fatal(FARGS, "Can not invert matrix, determinant = %e", deter);
 770     }
 771
 772     dest[XX][XX] = c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 773     dest[XX][YY] = -c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 774     dest[XX][ZZ] = c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 775     dest[YY][XX] = -c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 776     dest[YY][YY] = c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 777     dest[YY][ZZ] = -c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 778     dest[ZZ][XX] = c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 779     dest[ZZ][YY] = -c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 780     dest[ZZ][ZZ] = c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 781 }
 782
 783 static gmx_inline void mvmul(matrix a, const rvec src, rvec dest)
 784 {
 785     dest[XX] = a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 786     dest[YY] = a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 787     dest[ZZ] = a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 788 }
 789
 790
 791 static gmx_inline void mvmul_ur0(matrix a, const rvec src, rvec dest)
 792 {
 793     dest[ZZ] = a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 794     dest[YY] = a[YY][XX]*src[XX]+a[YY][YY]*src[YY];
 795     dest[XX] = a[XX][XX]*src[XX];
 796 }
 797
 798 static gmx_inline void tmvmul_ur0(matrix a, const rvec src, rvec dest)
 799 {
 800     dest[XX] = a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 801     dest[YY] =                   a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 802     dest[ZZ] =                                     a[ZZ][ZZ]*src[ZZ];
 803 }
 804
 805 static gmx_inline void unitv(const rvec src, rvec dest)
 806 {
 807     real linv;
 808
 809     linv     = gmx_invsqrt(norm2(src));
 810     dest[XX] = linv*src[XX];
 811     dest[YY] = linv*src[YY];
 812     dest[ZZ] = linv*src[ZZ];
 813 }
 814
 815 static gmx_inline void unitv_no_table(const rvec src, rvec dest)
 816 {
 817     real linv;
 818
 819     linv     = 1.0/sqrt(norm2(src));
 820     dest[XX] = linv*src[XX];
 821     dest[YY] = linv*src[YY];
 822     dest[ZZ] = linv*src[ZZ];
 823 }
 824
 825 static void calc_lll(rvec box, rvec lll)
 826 {
 827     lll[XX] = 2.0*M_PI/box[XX];
 828     lll[YY] = 2.0*M_PI/box[YY];
 829     lll[ZZ] = 2.0*M_PI/box[ZZ];
 830 }
 831
 832 static gmx_inline real trace(matrix m)
 833 {
 834     return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 835 }
 836
 837 static gmx_inline real _divide_err(real a, real b, const char *file, int line)
 838 {
 839     if (fabs(b) <= GMX_REAL_MIN)
 840     {
 841         gmx_fatal(FARGS, "Dividing by zero, file %s, line %d", file, line);
 842     }
 843     return a/b;
 844 }
 845
 846 static gmx_inline int _mod(int a, int b, char *file, int line)
 847 {
 848     if (b == 0)
 849     {
 850         gmx_fatal(FARGS, "Modulo zero, file %s, line %d", file, line);
 851     }
 852     return a % b;
 853 }
 854
 855 /* Operations on multidimensional rvecs, used e.g. in edsam.c */
 856 static gmx_inline void m_rveccopy(int dim, rvec *a, rvec *b)
 857 {
 858     /* b = a */
 859     int i;
 860
 861     for (i = 0; i < dim; i++)
 862     {
 863         copy_rvec(a[i], b[i]);
 864     }
 865 }
 866
 867 /*computer matrix vectors from base vectors and angles */
 868 static gmx_inline void matrix_convert(matrix box, rvec vec, rvec angle)
 869 {
 870     svmul(DEG2RAD, angle, angle);
 871     box[XX][XX] = vec[XX];
 872     box[YY][XX] = vec[YY]*cos(angle[ZZ]);
 873     box[YY][YY] = vec[YY]*sin(angle[ZZ]);
 874     box[ZZ][XX] = vec[ZZ]*cos(angle[YY]);
 875     box[ZZ][YY] = vec[ZZ]
 876         *(cos(angle[XX])-cos(angle[YY])*cos(angle[ZZ]))/sin(angle[ZZ]);
 877     box[ZZ][ZZ] = sqrt(sqr(vec[ZZ])
 878                        -box[ZZ][XX]*box[ZZ][XX]-box[ZZ][YY]*box[ZZ][YY]);
 879 }
 880
 881 #define divide_err(a, b) _divide_err((a), (b), __FILE__, __LINE__)
 882 #define mod(a, b)    _mod((a), (b), __FILE__, __LINE__)
 883
 884 #ifdef __cplusplus
 885 }
 886
 887 static gmx_inline real det(const matrix a)
 888 {
 889     return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 890              -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 891              +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 892 }
 893
 894 static gmx_inline void mvmul(const matrix a, const rvec src, rvec dest)
 895 {
 896     dest[XX] = a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 897     dest[YY] = a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 898     dest[ZZ] = a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 899 }
 900
 901 static gmx_inline void tmvmul_ur0(const matrix a, const rvec src, rvec dest)
 902 {
 903     dest[XX] = a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 904     dest[YY] =                   a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 905     dest[ZZ] =                                     a[ZZ][ZZ]*src[ZZ];
 906 }
 907
 908 #endif
 909
 910 #endif