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