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