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