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