src/gromacs/legacyheaders/vec.h

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