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