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