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