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