include/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 #ifdef HAVE_CONFIG_H
  39 #include <config.h>
  40 /*
  41 #define gmx_inline inline
  42 #else
  43 #ifdef __GNUC__
  44 #define gmx_inline __inline
  45 #else
  46 #define inline
  47 #endif
  48 */
  49 #endif
  50
  51 /*
  52   collection of in-line ready operations:
  53
  54   lookup-table optimized scalar operations:
  55   real gmx_invsqrt(real x)
  56   void vecinvsqrt(real in[],real out[],int n)
  57   void vecrecip(real in[],real out[],int n)
  58   real sqr(real x)
  59   double dsqr(double x)
  60
  61   vector operations:
  62   void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  63   void dvec_add(const dvec a,const dvec b,dvec c)  c = a + b
  64   void ivec_add(const ivec a,const ivec b,ivec c)  c = a + b
  65   void rvec_inc(rvec a,const rvec b)               a += b
  66   void dvec_inc(dvec a,const dvec b)               a += b
  67   void ivec_inc(ivec a,const ivec b)               a += b
  68   void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  69   void dvec_sub(const dvec a,const dvec b,dvec c)  c = a - b
  70   void rvec_dec(rvec a,rvec b)                     a -= b
  71   void copy_rvec(const rvec a,rvec b)              b = a (reals)
  72   void copy_dvec(const dvec a,dvec b)              b = a (reals)
  73   void copy_ivec(const ivec a,ivec b)              b = a (integers)
  74   void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  75   void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  76   void dsvmul(double a,dvec v1,dvec v2)            v2 = a * v1
  77   void clear_rvec(rvec a)                          a = 0
  78   void clear_dvec(dvec a)                          a = 0
  79   void clear_ivec(rvec a)                          a = 0
  80   void clear_rvecs(int n,rvec v[])
  81   real iprod(rvec a,rvec b)                        = a . b (inner product)
  82   double diprod(dvec a,dvec b)                     = a . b (inner product)
  83   real iiprod(ivec a,ivec b)                       = a . b (integers)
  84   real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  85   double dnorm2(dvec a)                            = | a |^2 ( = x*y*z )
  86   real norm(rvec a)                                = | a |
  87   double dnorm(dvec a)                             = | a |
  88   void cprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  89   void dprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  90   void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  91   real cos_angle(rvec a,rvec b)
  92   real cos_angle_no_table(rvec a,rvec b)
  93   real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  94   void unitv(rvec src,rvec dest)                   dest = src / |src|
  95   void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
  96
  97   matrix (3x3) operations:
  98     ! indicates that dest should not be the same as a, b or src
  99     the _ur0 varieties work on matrices that have only zeros
 100     in the upper right part, such as box matrices, these varieties
 101     could produce less rounding errors, not due to the operations themselves,
 102     but because the compiler can easier recombine the operations
 103   void copy_mat(matrix a,matrix b)                 b = a
 104   void clear_mat(matrix a)                         a = 0
 105   void mmul(matrix a,matrix b,matrix dest)      !  dest = a . b
 106   void mmul_ur0(matrix a,matrix b,matrix dest)     dest = a . b
 107   void transpose(matrix src,matrix dest)        !  dest = src*
 108   void tmmul(matrix a,matrix b,matrix dest)     !  dest = a* . b
 109   void mtmul(matrix a,matrix b,matrix dest)     !  dest = a . b*
 110   real det(matrix a)                               = det(a)
 111   void m_add(matrix a,matrix b,matrix dest)        dest = a + b
 112   void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
 113   void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
 114   void m_inv_ur0(matrix src,matrix dest)           dest = src^-1
 115   void m_inv(matrix src,matrix dest)            !  dest = src^-1
 116   void mvmul(matrix a,rvec src,rvec dest)       !  dest = a . src
 117   void mvmul_ur0(matrix a,rvec src,rvec dest)      dest = a . src
 118   void tmvmul_ur0(matrix a,rvec src,rvec dest)     dest = a* . src
 119   real trace(matrix m)                             = trace(m)
 120 */
 121
 122 #include "types/simple.h"
 123 #include "maths.h"
 124 #include "typedefs.h"
 125 #include "sysstuff.h"
 126 #include "macros.h"
 127 #include "gmx_fatal.h"
 128 #include "mpelogging.h"
 129 #include "physics.h"
 130
 131 #define EXP_LSB         0x00800000
 132 #define EXP_MASK        0x7f800000
 133 #define EXP_SHIFT       23
 134 #define FRACT_MASK      0x007fffff
 135 #define FRACT_SIZE      11              /* significant part of fraction */
 136 #define FRACT_SHIFT     (EXP_SHIFT-FRACT_SIZE)
 137 #define EXP_ADDR(val)   (((val)&EXP_MASK)>>EXP_SHIFT)
 138 #define FRACT_ADDR(val) (((val)&(FRACT_MASK|EXP_LSB))>>FRACT_SHIFT)
 139
 140 #define PR_VEC(a)       a[XX],a[YY],a[ZZ]
 141
 142 #ifdef GMX_SOFTWARE_INVSQRT
 143 extern const unsigned int *  gmx_invsqrt_exptab;
 144 extern const unsigned int *  gmx_invsqrt_fracttab;
 145 #endif
 146
 147
 148 typedef union
 149 {
 150   unsigned int bval;
 151   float fval;
 152 } t_convert;
 153
 154
 155 #ifdef GMX_SOFTWARE_INVSQRT
 156 static real gmx_invsqrt(real x)
 157 {
 158   const real  half=0.5;
 159   const real  three=3.0;
 160   t_convert   result,bit_pattern;
 161   unsigned int exp,fract;
 162   real        lu;
 163   real        y;
 164 #ifdef GMX_DOUBLE
 165   real        y2;
 166 #endif
 167
 168   bit_pattern.fval=x;
 169   exp   = EXP_ADDR(bit_pattern.bval);
 170   fract = FRACT_ADDR(bit_pattern.bval);
 171   result.bval=gmx_invsqrt_exptab[exp] | gmx_invsqrt_fracttab[fract];
 172   lu    = result.fval;
 173
 174   y=(half*lu*(three-((x*lu)*lu)));
 175 #ifdef GMX_DOUBLE
 176   y2=(half*y*(three-((x*y)*y)));
 177
 178   return y2;                    /* 10 Flops */
 179 #else
 180   return y;                     /* 5  Flops */
 181 #endif
 182 }
 183 #define INVSQRT_DONE
 184 #endif /* gmx_invsqrt */
 185
 186 #ifdef GMX_POWERPC_SQRT
 187 static real gmx_invsqrt(real x)
 188 {
 189   const real  half=0.5;
 190   const real  three=3.0;
 191   t_convert   result,bit_pattern;
 192   unsigned int exp,fract;
 193   real        lu;
 194   real        y;
 195 #ifdef GMX_DOUBLE
 196   real        y2;
 197 #endif
 198
 199   lu = __frsqrte((double)x);
 200
 201   y=(half*lu*(three-((x*lu)*lu)));
 202
 203 #if (GMX_POWERPC_SQRT==2)
 204   /* Extra iteration required */
 205   y=(half*y*(three-((x*y)*y)));
 206 #endif
 207
 208 #ifdef GMX_DOUBLE
 209   y2=(half*y*(three-((x*y)*y)));
 210
 211   return y2;                    /* 10 Flops */
 212 #else
 213   return y;                     /* 5  Flops */
 214 #endif
 215 }
 216 #define INVSQRT_DONE
 217 #endif /* powerpc_invsqrt */
 218
 219
 220 #ifndef INVSQRT_DONE
 221 #define gmx_invsqrt(x) (1.0f/sqrt(x))
 222 #endif
 223
 224
 225
 226
 227
 228 static real sqr(real x)
 229 {
 230   return (x*x);
 231 }
 232
 233 static inline double dsqr(double x)
 234 {
 235   return (x*x);
 236 }
 237
 238 extern void vecinvsqrt(real in[],real out[],int n);
 239 /* Perform out[i]=1.0/sqrt(in[i]) for n elements */
 240
 241
 242 extern void vecrecip(real in[],real out[],int n);
 243 /* Perform out[i]=1.0/(in[i]) for n elements */
 244
 245 /* Note: If you need a fast version of vecinvsqrt
 246  * and/or vecrecip, call detectcpu() and run the SSE/3DNow/SSE2/Altivec
 247  * versions if your hardware supports it.
 248  *
 249  * To use those routines, your memory HAS TO BE CACHE-ALIGNED.
 250  * Start by allocating 31 bytes more than you need, put
 251  * this in a temp variable (e.g. _buf, so you can free it later), and
 252  * create your aligned array buf with
 253  *
 254  *  buf=(real *) ( ( (unsigned long int)_buf + 31 ) & (~0x1f) );
 255  */
 256
 257
 258 static inline void rvec_add(const rvec a,const rvec b,rvec c)
 259 {
 260   real x,y,z;
 261
 262   x=a[XX]+b[XX];
 263   y=a[YY]+b[YY];
 264   z=a[ZZ]+b[ZZ];
 265
 266   c[XX]=x;
 267   c[YY]=y;
 268   c[ZZ]=z;
 269 }
 270
 271 static inline void dvec_add(const dvec a,const dvec b,dvec c)
 272 {
 273   double x,y,z;
 274
 275   x=a[XX]+b[XX];
 276   y=a[YY]+b[YY];
 277   z=a[ZZ]+b[ZZ];
 278
 279   c[XX]=x;
 280   c[YY]=y;
 281   c[ZZ]=z;
 282 }
 283
 284 static inline void ivec_add(const ivec a,const ivec b,ivec c)
 285 {
 286   int x,y,z;
 287
 288   x=a[XX]+b[XX];
 289   y=a[YY]+b[YY];
 290   z=a[ZZ]+b[ZZ];
 291
 292   c[XX]=x;
 293   c[YY]=y;
 294   c[ZZ]=z;
 295 }
 296
 297 static inline void rvec_inc(rvec a,const rvec b)
 298 {
 299   real x,y,z;
 300
 301   x=a[XX]+b[XX];
 302   y=a[YY]+b[YY];
 303   z=a[ZZ]+b[ZZ];
 304
 305   a[XX]=x;
 306   a[YY]=y;
 307   a[ZZ]=z;
 308 }
 309
 310 static inline void dvec_inc(dvec a,const dvec b)
 311 {
 312   double x,y,z;
 313
 314   x=a[XX]+b[XX];
 315   y=a[YY]+b[YY];
 316   z=a[ZZ]+b[ZZ];
 317
 318   a[XX]=x;
 319   a[YY]=y;
 320   a[ZZ]=z;
 321 }
 322
 323 static inline void rvec_sub(const rvec a,const rvec b,rvec c)
 324 {
 325   real x,y,z;
 326
 327   x=a[XX]-b[XX];
 328   y=a[YY]-b[YY];
 329   z=a[ZZ]-b[ZZ];
 330
 331   c[XX]=x;
 332   c[YY]=y;
 333   c[ZZ]=z;
 334 }
 335
 336 static inline void dvec_sub(const dvec a,const dvec b,dvec c)
 337 {
 338   double x,y,z;
 339
 340   x=a[XX]-b[XX];
 341   y=a[YY]-b[YY];
 342   z=a[ZZ]-b[ZZ];
 343
 344   c[XX]=x;
 345   c[YY]=y;
 346   c[ZZ]=z;
 347 }
 348
 349 static inline void rvec_dec(rvec a,const rvec b)
 350 {
 351   real x,y,z;
 352
 353   x=a[XX]-b[XX];
 354   y=a[YY]-b[YY];
 355   z=a[ZZ]-b[ZZ];
 356
 357   a[XX]=x;
 358   a[YY]=y;
 359   a[ZZ]=z;
 360 }
 361
 362 static inline void copy_rvec(const rvec a,rvec b)
 363 {
 364   b[XX]=a[XX];
 365   b[YY]=a[YY];
 366   b[ZZ]=a[ZZ];
 367 }
 368
 369 static inline void copy_dvec(const dvec a,dvec b)
 370 {
 371   b[XX]=a[XX];
 372   b[YY]=a[YY];
 373   b[ZZ]=a[ZZ];
 374 }
 375
 376 static inline void copy_ivec(const ivec a,ivec b)
 377 {
 378   b[XX]=a[XX];
 379   b[YY]=a[YY];
 380   b[ZZ]=a[ZZ];
 381 }
 382
 383 static inline void ivec_sub(const ivec a,const ivec b,ivec c)
 384 {
 385   int x,y,z;
 386
 387   x=a[XX]-b[XX];
 388   y=a[YY]-b[YY];
 389   z=a[ZZ]-b[ZZ];
 390
 391   c[XX]=x;
 392   c[YY]=y;
 393   c[ZZ]=z;
 394 }
 395
 396 static inline void copy_mat(matrix a,matrix b)
 397 {
 398   copy_rvec(a[XX],b[XX]);
 399   copy_rvec(a[YY],b[YY]);
 400   copy_rvec(a[ZZ],b[ZZ]);
 401 }
 402
 403 static inline void svmul(real a,const rvec v1,rvec v2)
 404 {
 405   v2[XX]=a*v1[XX];
 406   v2[YY]=a*v1[YY];
 407   v2[ZZ]=a*v1[ZZ];
 408 }
 409
 410 static inline void dsvmul(double a,const dvec v1,dvec v2)
 411 {
 412   v2[XX]=a*v1[XX];
 413   v2[YY]=a*v1[YY];
 414   v2[ZZ]=a*v1[ZZ];
 415 }
 416
 417 static inline real distance2(const rvec v1,const rvec v2)
 418 {
 419   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 420 }
 421
 422 static inline void clear_rvec(rvec a)
 423 {
 424   /* The ibm compiler has problems with inlining this
 425    * when we use a const real variable
 426    */
 427   a[XX]=0.0;
 428   a[YY]=0.0;
 429   a[ZZ]=0.0;
 430 }
 431
 432 static inline void clear_dvec(dvec a)
 433 {
 434   /* The ibm compiler has problems with inlining this
 435    * when we use a const real variable
 436    */
 437   a[XX]=0.0;
 438   a[YY]=0.0;
 439   a[ZZ]=0.0;
 440 }
 441
 442 static inline void clear_ivec(ivec a)
 443 {
 444   a[XX]=0;
 445   a[YY]=0;
 446   a[ZZ]=0;
 447 }
 448
 449 static inline void clear_rvecs(int n,rvec v[])
 450 {
 451 /*  memset(v[0],0,DIM*n*sizeof(v[0][0])); */
 452   int i;
 453
 454   GMX_MPE_LOG(ev_clear_rvecs_start);
 455
 456   for(i=0; (i<n); i++)
 457     clear_rvec(v[i]);
 458
 459   GMX_MPE_LOG(ev_clear_rvecs_finish);
 460 }
 461
 462 static inline void clear_mat(matrix a)
 463 {
 464 /*  memset(a[0],0,DIM*DIM*sizeof(a[0][0])); */
 465
 466   const real nul=0.0;
 467
 468   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 469   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 470   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 471 }
 472
 473 static inline real iprod(const rvec a,const rvec b)
 474 {
 475   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 476 }
 477
 478 static inline double diprod(const dvec a,const dvec b)
 479 {
 480   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 481 }
 482
 483 static inline int iiprod(const ivec a,const ivec b)
 484 {
 485   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 486 }
 487
 488 static inline real norm2(const rvec a)
 489 {
 490   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 491 }
 492
 493 static inline double dnorm2(const dvec a)
 494 {
 495   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 496 }
 497
 498 static inline real norm(const rvec a)
 499 {
 500   return (real)sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 501 }
 502
 503 static inline double dnorm(const dvec a)
 504 {
 505   return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 506 }
 507
 508 static inline real cos_angle(const rvec a,const rvec b)
 509 {
 510   /*
 511    *                  ax*bx + ay*by + az*bz
 512    * cos-vec (a,b) =  ---------------------
 513    *                      ||a|| * ||b||
 514    */
 515   real   cosval;
 516   int    m;
 517   double aa,bb,ip,ipa,ipb,ipab; /* For accuracy these must be double! */
 518
 519   ip=ipa=ipb=0.0;
 520   for(m=0; (m<DIM); m++) {              /* 18           */
 521     aa   = a[m];
 522     bb   = b[m];
 523     ip  += aa*bb;
 524     ipa += aa*aa;
 525     ipb += bb*bb;
 526   }
 527   ipab = ipa*ipb;
 528   if (ipab > 0)
 529     cosval = ip*gmx_invsqrt(ipab);              /*  7           */
 530   else
 531     cosval = 1;
 532                                         /* 25 TOTAL     */
 533   if (cosval > 1.0)
 534     return  1.0;
 535   if (cosval <-1.0)
 536     return -1.0;
 537
 538   return cosval;
 539 }
 540
 541 static inline real cos_angle_no_table(const rvec a,const rvec b)
 542 {
 543   /* This version does not need the invsqrt lookup table */
 544   real   cosval;
 545   int    m;
 546   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 547
 548   ip=ipa=ipb=0.0;
 549   for(m=0; (m<DIM); m++) {              /* 18           */
 550     aa   = a[m];
 551     bb   = b[m];
 552     ip  += aa*bb;
 553     ipa += aa*aa;
 554     ipb += bb*bb;
 555   }
 556   cosval=ip/sqrt(ipa*ipb);              /* 12           */
 557                                         /* 30 TOTAL     */
 558   if (cosval > 1.0)
 559     return  1.0;
 560   if (cosval <-1.0)
 561     return -1.0;
 562
 563   return cosval;
 564 }
 565
 566 static inline void cprod(const rvec a,const rvec b,rvec c)
 567 {
 568   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 569   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 570   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 571 }
 572
 573 static inline void dcprod(const dvec a,const dvec b,dvec c)
 574 {
 575   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 576   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 577   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 578 }
 579
 580 static inline void mmul_ur0(matrix a,matrix b,matrix dest)
 581 {
 582   dest[XX][XX]=a[XX][XX]*b[XX][XX];
 583   dest[XX][YY]=0.0;
 584   dest[XX][ZZ]=0.0;
 585   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 586   dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
 587   dest[YY][ZZ]=0.0;
 588   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 589   dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 590   dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
 591 }
 592
 593 static inline void mmul(matrix a,matrix b,matrix dest)
 594 {
 595   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 596   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 597   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 598   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 599   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 600   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 601   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 602   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 603   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 604 }
 605
 606 static inline void transpose(matrix src,matrix dest)
 607 {
 608   dest[XX][XX]=src[XX][XX];
 609   dest[YY][XX]=src[XX][YY];
 610   dest[ZZ][XX]=src[XX][ZZ];
 611   dest[XX][YY]=src[YY][XX];
 612   dest[YY][YY]=src[YY][YY];
 613   dest[ZZ][YY]=src[YY][ZZ];
 614   dest[XX][ZZ]=src[ZZ][XX];
 615   dest[YY][ZZ]=src[ZZ][YY];
 616   dest[ZZ][ZZ]=src[ZZ][ZZ];
 617 }
 618
 619 static inline void tmmul(matrix a,matrix b,matrix dest)
 620 {
 621   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 622   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 623   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 624   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 625   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 626   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 627   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 628   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 629   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 630   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 631 }
 632
 633 static inline void mtmul(matrix a,matrix b,matrix dest)
 634 {
 635   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 636   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 637   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 638   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 639   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 640   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 641   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 642   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 643   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 644   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 645 }
 646
 647 static inline real det(matrix a)
 648 {
 649   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 650           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 651           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 652 }
 653
 654 static inline void m_add(matrix a,matrix b,matrix dest)
 655 {
 656   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 657   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 658   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 659   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 660   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 661   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 662   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 663   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 664   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 665 }
 666
 667 static inline void m_sub(matrix a,matrix b,matrix dest)
 668 {
 669   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 670   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 671   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 672   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 673   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 674   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 675   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 676   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 677   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 678 }
 679
 680 static inline void msmul(matrix m1,real r1,matrix dest)
 681 {
 682   dest[XX][XX]=r1*m1[XX][XX];
 683   dest[XX][YY]=r1*m1[XX][YY];
 684   dest[XX][ZZ]=r1*m1[XX][ZZ];
 685   dest[YY][XX]=r1*m1[YY][XX];
 686   dest[YY][YY]=r1*m1[YY][YY];
 687   dest[YY][ZZ]=r1*m1[YY][ZZ];
 688   dest[ZZ][XX]=r1*m1[ZZ][XX];
 689   dest[ZZ][YY]=r1*m1[ZZ][YY];
 690   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 691 }
 692
 693 static inline void m_inv_ur0(matrix src,matrix dest)
 694 {
 695   double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
 696   if (fabs(tmp) <= 100*GMX_REAL_MIN)
 697     gmx_fatal(FARGS,"Can not invert matrix, determinant is zero");
 698
 699   dest[XX][XX] = 1/src[XX][XX];
 700   dest[YY][YY] = 1/src[YY][YY];
 701   dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
 702   dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
 703                   - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
 704   dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
 705   dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
 706   dest[XX][YY] = 0.0;
 707   dest[XX][ZZ] = 0.0;
 708   dest[YY][ZZ] = 0.0;
 709 }
 710
 711 static inline void m_inv(matrix src,matrix dest)
 712 {
 713   const real smallreal = (real)1.0e-24;
 714   const real largereal = (real)1.0e24;
 715   real  deter,c,fc;
 716
 717   deter = det(src);
 718   c     = (real)1.0/deter;
 719   fc    = (real)fabs(c);
 720
 721   if ((fc <= smallreal) || (fc >= largereal))
 722     gmx_fatal(FARGS,"Can not invert matrix, determinant = %e",deter);
 723
 724   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 725   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 726   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 727   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 728   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 729   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 730   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 731   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 732   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 733 }
 734
 735 static inline void mvmul(matrix a,const rvec src,rvec dest)
 736 {
 737   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 738   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 739   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 740 }
 741
 742 static inline void mvmul_ur0(matrix a,const rvec src,rvec dest)
 743 {
 744   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 745   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY];
 746   dest[XX]=a[XX][XX]*src[XX];
 747 }
 748
 749 static inline void tmvmul_ur0(matrix a,const rvec src,rvec dest)
 750 {
 751   dest[XX]=a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 752   dest[YY]=                  a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 753   dest[ZZ]=                                    a[ZZ][ZZ]*src[ZZ];
 754 }
 755
 756 static inline void unitv(const rvec src,rvec dest)
 757 {
 758   real linv;
 759
 760   linv=gmx_invsqrt(norm2(src));
 761   dest[XX]=linv*src[XX];
 762   dest[YY]=linv*src[YY];
 763   dest[ZZ]=linv*src[ZZ];
 764 }
 765
 766 static inline void unitv_no_table(const rvec src,rvec dest)
 767 {
 768   real linv;
 769
 770   linv=1.0/sqrt(norm2(src));
 771   dest[XX]=linv*src[XX];
 772   dest[YY]=linv*src[YY];
 773   dest[ZZ]=linv*src[ZZ];
 774 }
 775
 776 static void calc_lll(rvec box,rvec lll)
 777 {
 778   lll[XX] = 2.0*M_PI/box[XX];
 779   lll[YY] = 2.0*M_PI/box[YY];
 780   lll[ZZ] = 2.0*M_PI/box[ZZ];
 781 }
 782
 783 static inline real trace(matrix m)
 784 {
 785   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 786 }
 787
 788 static inline real _divide(real a,real b,const char *file,int line)
 789 {
 790     if (fabs(b) <= GMX_REAL_MIN)
 791         gmx_fatal(FARGS,"Dividing by zero, file %s, line %d",file,line);
 792     return a/b;
 793 }
 794
 795 static inline int _mod(int a,int b,char *file,int line)
 796 {
 797   if(b==0)
 798     gmx_fatal(FARGS,"Modulo zero, file %s, line %d",file,line);
 799   return a % b;
 800 }
 801
 802 /* Operations on multidimensional rvecs, used e.g. in edsam.c */
 803 static void m_rveccopy(int dim, rvec *a, rvec *b)
 804 {
 805     /* b = a */
 806     int i;
 807
 808     for (i=0; i<dim; i++)
 809         copy_rvec(a[i],b[i]);
 810 }
 811
 812 /*computer matrix vectors from base vectors and angles */
 813 static void matrix_convert(matrix box, rvec vec, rvec angle)
 814 {
 815     svmul(DEG2RAD,angle,angle);
 816     box[XX][XX] = vec[XX];
 817     box[YY][XX] = vec[YY]*cos(angle[ZZ]);
 818     box[YY][YY] = vec[YY]*sin(angle[ZZ]);
 819     box[ZZ][XX] = vec[ZZ]*cos(angle[YY]);
 820     box[ZZ][YY] = vec[ZZ]
 821                          *(cos(angle[XX])-cos(angle[YY])*cos(angle[ZZ]))/sin(angle[ZZ]);
 822     box[ZZ][ZZ] = sqrt(sqr(vec[ZZ])
 823                        -box[ZZ][XX]*box[ZZ][XX]-box[ZZ][YY]*box[ZZ][YY]);
 824 }
 825
 826 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 827 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 828
 829 #endif  /* _vec_h */