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