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  * Start by allocating 31 bytes more than you need, put
 268  * this in a temp variable (e.g. _buf, so you can free it later), and
 269  * create your aligned array buf with
 270  *
 271  *  buf=(real *) ( ( (unsigned long int)_buf + 31 ) & (~0x1f) );
 272  */
 273
 274
 275 static inline void rvec_add(const rvec a,const rvec b,rvec c)
 276 {
 277   real x,y,z;
 278
 279   x=a[XX]+b[XX];
 280   y=a[YY]+b[YY];
 281   z=a[ZZ]+b[ZZ];
 282
 283   c[XX]=x;
 284   c[YY]=y;
 285   c[ZZ]=z;
 286 }
 287
 288 static inline void dvec_add(const dvec a,const dvec b,dvec c)
 289 {
 290   double x,y,z;
 291
 292   x=a[XX]+b[XX];
 293   y=a[YY]+b[YY];
 294   z=a[ZZ]+b[ZZ];
 295
 296   c[XX]=x;
 297   c[YY]=y;
 298   c[ZZ]=z;
 299 }
 300
 301 static inline void ivec_add(const ivec a,const ivec b,ivec c)
 302 {
 303   int x,y,z;
 304
 305   x=a[XX]+b[XX];
 306   y=a[YY]+b[YY];
 307   z=a[ZZ]+b[ZZ];
 308
 309   c[XX]=x;
 310   c[YY]=y;
 311   c[ZZ]=z;
 312 }
 313
 314 static inline void rvec_inc(rvec a,const rvec b)
 315 {
 316   real x,y,z;
 317
 318   x=a[XX]+b[XX];
 319   y=a[YY]+b[YY];
 320   z=a[ZZ]+b[ZZ];
 321
 322   a[XX]=x;
 323   a[YY]=y;
 324   a[ZZ]=z;
 325 }
 326
 327 static inline void dvec_inc(dvec a,const dvec b)
 328 {
 329   double x,y,z;
 330
 331   x=a[XX]+b[XX];
 332   y=a[YY]+b[YY];
 333   z=a[ZZ]+b[ZZ];
 334
 335   a[XX]=x;
 336   a[YY]=y;
 337   a[ZZ]=z;
 338 }
 339
 340 static inline void rvec_sub(const rvec a,const rvec b,rvec c)
 341 {
 342   real x,y,z;
 343
 344   x=a[XX]-b[XX];
 345   y=a[YY]-b[YY];
 346   z=a[ZZ]-b[ZZ];
 347
 348   c[XX]=x;
 349   c[YY]=y;
 350   c[ZZ]=z;
 351 }
 352
 353 static inline void dvec_sub(const dvec a,const dvec b,dvec c)
 354 {
 355   double x,y,z;
 356
 357   x=a[XX]-b[XX];
 358   y=a[YY]-b[YY];
 359   z=a[ZZ]-b[ZZ];
 360
 361   c[XX]=x;
 362   c[YY]=y;
 363   c[ZZ]=z;
 364 }
 365
 366 static inline void rvec_dec(rvec a,const rvec b)
 367 {
 368   real x,y,z;
 369
 370   x=a[XX]-b[XX];
 371   y=a[YY]-b[YY];
 372   z=a[ZZ]-b[ZZ];
 373
 374   a[XX]=x;
 375   a[YY]=y;
 376   a[ZZ]=z;
 377 }
 378
 379 static inline void copy_rvec(const rvec a,rvec b)
 380 {
 381   b[XX]=a[XX];
 382   b[YY]=a[YY];
 383   b[ZZ]=a[ZZ];
 384 }
 385
 386 static inline void copy_rvecn(rvec *a,rvec *b,int startn, int endn)
 387 {
 388   int i;
 389   for (i=startn;i<endn;i++) {
 390     b[i][XX]=a[i][XX];
 391     b[i][YY]=a[i][YY];
 392     b[i][ZZ]=a[i][ZZ];
 393   }
 394 }
 395
 396 static inline void copy_dvec(const dvec a,dvec b)
 397 {
 398   b[XX]=a[XX];
 399   b[YY]=a[YY];
 400   b[ZZ]=a[ZZ];
 401 }
 402
 403 static inline void copy_ivec(const ivec a,ivec b)
 404 {
 405   b[XX]=a[XX];
 406   b[YY]=a[YY];
 407   b[ZZ]=a[ZZ];
 408 }
 409
 410 static inline void ivec_sub(const ivec a,const ivec b,ivec c)
 411 {
 412   int x,y,z;
 413
 414   x=a[XX]-b[XX];
 415   y=a[YY]-b[YY];
 416   z=a[ZZ]-b[ZZ];
 417
 418   c[XX]=x;
 419   c[YY]=y;
 420   c[ZZ]=z;
 421 }
 422
 423 static inline void copy_mat(matrix a,matrix b)
 424 {
 425   copy_rvec(a[XX],b[XX]);
 426   copy_rvec(a[YY],b[YY]);
 427   copy_rvec(a[ZZ],b[ZZ]);
 428 }
 429
 430 static inline void svmul(real a,const rvec v1,rvec v2)
 431 {
 432   v2[XX]=a*v1[XX];
 433   v2[YY]=a*v1[YY];
 434   v2[ZZ]=a*v1[ZZ];
 435 }
 436
 437 static inline void dsvmul(double a,const dvec v1,dvec v2)
 438 {
 439   v2[XX]=a*v1[XX];
 440   v2[YY]=a*v1[YY];
 441   v2[ZZ]=a*v1[ZZ];
 442 }
 443
 444 static inline real distance2(const rvec v1,const rvec v2)
 445 {
 446   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 447 }
 448
 449 static inline void clear_rvec(rvec a)
 450 {
 451   /* The ibm compiler has problems with inlining this
 452    * when we use a const real variable
 453    */
 454   a[XX]=0.0;
 455   a[YY]=0.0;
 456   a[ZZ]=0.0;
 457 }
 458
 459 static inline void clear_dvec(dvec a)
 460 {
 461   /* The ibm compiler has problems with inlining this
 462    * when we use a const real variable
 463    */
 464   a[XX]=0.0;
 465   a[YY]=0.0;
 466   a[ZZ]=0.0;
 467 }
 468
 469 static inline void clear_ivec(ivec a)
 470 {
 471   a[XX]=0;
 472   a[YY]=0;
 473   a[ZZ]=0;
 474 }
 475
 476 static inline void clear_rvecs(int n,rvec v[])
 477 {
 478 /*  memset(v[0],0,DIM*n*sizeof(v[0][0])); */
 479   int i;
 480
 481   GMX_MPE_LOG(ev_clear_rvecs_start);
 482
 483   for(i=0; (i<n); i++)
 484     clear_rvec(v[i]);
 485
 486   GMX_MPE_LOG(ev_clear_rvecs_finish);
 487 }
 488
 489 static inline void clear_mat(matrix a)
 490 {
 491 /*  memset(a[0],0,DIM*DIM*sizeof(a[0][0])); */
 492
 493   const real nul=0.0;
 494
 495   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 496   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 497   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 498 }
 499
 500 static inline real iprod(const rvec a,const rvec b)
 501 {
 502   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 503 }
 504
 505 static inline double diprod(const dvec a,const dvec b)
 506 {
 507   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 508 }
 509
 510 static inline int iiprod(const ivec a,const ivec b)
 511 {
 512   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 513 }
 514
 515 static inline real norm2(const rvec a)
 516 {
 517   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 518 }
 519
 520 static inline double dnorm2(const dvec a)
 521 {
 522   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 523 }
 524
 525 static inline real norm(const rvec a)
 526 {
 527   return (real)sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 528 }
 529
 530 static inline double dnorm(const dvec a)
 531 {
 532   return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 533 }
 534
 535 /* WARNING:
 536  * Do _not_ use these routines to calculate the angle between two vectors
 537  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 538  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 539  * Instead, use the new gmx_angle() function directly.
 540  */
 541 static inline real
 542 cos_angle(const rvec a,const rvec b)
 543 {
 544   /*
 545    *                  ax*bx + ay*by + az*bz
 546    * cos-vec (a,b) =  ---------------------
 547    *                      ||a|| * ||b||
 548    */
 549   real   cosval;
 550   int    m;
 551   double aa,bb,ip,ipa,ipb,ipab; /* For accuracy these must be double! */
 552
 553   ip=ipa=ipb=0.0;
 554   for(m=0; (m<DIM); m++) {              /* 18           */
 555     aa   = a[m];
 556     bb   = b[m];
 557     ip  += aa*bb;
 558     ipa += aa*aa;
 559     ipb += bb*bb;
 560   }
 561   ipab = ipa*ipb;
 562   if (ipab > 0)
 563     cosval = ip*gmx_invsqrt(ipab);              /*  7           */
 564   else
 565     cosval = 1;
 566                                         /* 25 TOTAL     */
 567   if (cosval > 1.0)
 568     return  1.0;
 569   if (cosval <-1.0)
 570     return -1.0;
 571
 572   return cosval;
 573 }
 574
 575 /* WARNING:
 576  * Do _not_ use these routines to calculate the angle between two vectors
 577  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 578  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 579  * Instead, use the new gmx_angle() function directly.
 580  */
 581 static inline real
 582 cos_angle_no_table(const rvec a,const rvec b)
 583 {
 584   /* This version does not need the invsqrt lookup table */
 585   real   cosval;
 586   int    m;
 587   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 588
 589   ip=ipa=ipb=0.0;
 590   for(m=0; (m<DIM); m++) {              /* 18           */
 591     aa   = a[m];
 592     bb   = b[m];
 593     ip  += aa*bb;
 594     ipa += aa*aa;
 595     ipb += bb*bb;
 596   }
 597   cosval=ip/sqrt(ipa*ipb);              /* 12           */
 598                                         /* 30 TOTAL     */
 599   if (cosval > 1.0)
 600     return  1.0;
 601   if (cosval <-1.0)
 602     return -1.0;
 603
 604   return cosval;
 605 }
 606
 607
 608 static inline void cprod(const rvec a,const rvec b,rvec c)
 609 {
 610   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 611   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 612   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 613 }
 614
 615 static inline void dcprod(const dvec a,const dvec b,dvec c)
 616 {
 617   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 618   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 619   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 620 }
 621
 622 /* This routine calculates the angle between a & b without any loss of accuracy close to 0/PI.
 623  * If you only need cos(theta), use the cos_angle() routines to save a few cycles.
 624  * This routine is faster than it might appear, since atan2 is accelerated on many CPUs (e.g. x86).
 625  */
 626 static inline real
 627 gmx_angle(const rvec a, const rvec b)
 628 {
 629     rvec w;
 630     real wlen,s,theta;
 631
 632     cprod(a,b,w);
 633
 634     wlen  = norm(w);
 635     s     = iprod(a,b);
 636
 637     return atan2(wlen,s);
 638 }
 639
 640 static inline void mmul_ur0(matrix a,matrix b,matrix dest)
 641 {
 642   dest[XX][XX]=a[XX][XX]*b[XX][XX];
 643   dest[XX][YY]=0.0;
 644   dest[XX][ZZ]=0.0;
 645   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 646   dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
 647   dest[YY][ZZ]=0.0;
 648   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 649   dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 650   dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
 651 }
 652
 653 static inline void mmul(matrix a,matrix b,matrix dest)
 654 {
 655   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 656   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 657   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 658   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 659   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 660   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 661   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 662   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 663   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 664 }
 665
 666 static inline void transpose(matrix src,matrix dest)
 667 {
 668   dest[XX][XX]=src[XX][XX];
 669   dest[YY][XX]=src[XX][YY];
 670   dest[ZZ][XX]=src[XX][ZZ];
 671   dest[XX][YY]=src[YY][XX];
 672   dest[YY][YY]=src[YY][YY];
 673   dest[ZZ][YY]=src[YY][ZZ];
 674   dest[XX][ZZ]=src[ZZ][XX];
 675   dest[YY][ZZ]=src[ZZ][YY];
 676   dest[ZZ][ZZ]=src[ZZ][ZZ];
 677 }
 678
 679 static inline void tmmul(matrix a,matrix b,matrix dest)
 680 {
 681   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 682   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 683   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 684   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 685   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 686   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 687   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 688   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 689   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 690   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 691 }
 692
 693 static inline void mtmul(matrix a,matrix b,matrix dest)
 694 {
 695   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 696   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 697   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 698   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 699   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 700   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 701   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 702   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 703   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 704   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 705 }
 706
 707 static inline real det(matrix a)
 708 {
 709   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 710           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 711           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 712 }
 713
 714 static inline void m_add(matrix a,matrix b,matrix dest)
 715 {
 716   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 717   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 718   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 719   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 720   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 721   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 722   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 723   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 724   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 725 }
 726
 727 static inline void m_sub(matrix a,matrix b,matrix dest)
 728 {
 729   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 730   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 731   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 732   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 733   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 734   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 735   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 736   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 737   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 738 }
 739
 740 static inline void msmul(matrix m1,real r1,matrix dest)
 741 {
 742   dest[XX][XX]=r1*m1[XX][XX];
 743   dest[XX][YY]=r1*m1[XX][YY];
 744   dest[XX][ZZ]=r1*m1[XX][ZZ];
 745   dest[YY][XX]=r1*m1[YY][XX];
 746   dest[YY][YY]=r1*m1[YY][YY];
 747   dest[YY][ZZ]=r1*m1[YY][ZZ];
 748   dest[ZZ][XX]=r1*m1[ZZ][XX];
 749   dest[ZZ][YY]=r1*m1[ZZ][YY];
 750   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 751 }
 752
 753 static inline void m_inv_ur0(matrix src,matrix dest)
 754 {
 755   double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
 756   if (fabs(tmp) <= 100*GMX_REAL_MIN)
 757     gmx_fatal(FARGS,"Can not invert matrix, determinant is zero");
 758
 759   dest[XX][XX] = 1/src[XX][XX];
 760   dest[YY][YY] = 1/src[YY][YY];
 761   dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
 762   dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
 763                   - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
 764   dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
 765   dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
 766   dest[XX][YY] = 0.0;
 767   dest[XX][ZZ] = 0.0;
 768   dest[YY][ZZ] = 0.0;
 769 }
 770
 771 static inline void m_inv(matrix src,matrix dest)
 772 {
 773   const real smallreal = (real)1.0e-24;
 774   const real largereal = (real)1.0e24;
 775   real  deter,c,fc;
 776
 777   deter = det(src);
 778   c     = (real)1.0/deter;
 779   fc    = (real)fabs(c);
 780
 781   if ((fc <= smallreal) || (fc >= largereal))
 782     gmx_fatal(FARGS,"Can not invert matrix, determinant = %e",deter);
 783
 784   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 785   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 786   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 787   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 788   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 789   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 790   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 791   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 792   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 793 }
 794
 795 static inline void mvmul(matrix a,const rvec src,rvec dest)
 796 {
 797   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 798   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 799   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 800 }
 801
 802 static inline void mvmul_ur0(matrix a,const rvec src,rvec dest)
 803 {
 804   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 805   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY];
 806   dest[XX]=a[XX][XX]*src[XX];
 807 }
 808
 809 static inline void tmvmul_ur0(matrix a,const rvec src,rvec dest)
 810 {
 811   dest[XX]=a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 812   dest[YY]=                  a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 813   dest[ZZ]=                                    a[ZZ][ZZ]*src[ZZ];
 814 }
 815
 816 static inline void unitv(const rvec src,rvec dest)
 817 {
 818   real linv;
 819
 820   linv=gmx_invsqrt(norm2(src));
 821   dest[XX]=linv*src[XX];
 822   dest[YY]=linv*src[YY];
 823   dest[ZZ]=linv*src[ZZ];
 824 }
 825
 826 static inline void unitv_no_table(const rvec src,rvec dest)
 827 {
 828   real linv;
 829
 830   linv=1.0/sqrt(norm2(src));
 831   dest[XX]=linv*src[XX];
 832   dest[YY]=linv*src[YY];
 833   dest[ZZ]=linv*src[ZZ];
 834 }
 835
 836 static void calc_lll(rvec box,rvec lll)
 837 {
 838   lll[XX] = 2.0*M_PI/box[XX];
 839   lll[YY] = 2.0*M_PI/box[YY];
 840   lll[ZZ] = 2.0*M_PI/box[ZZ];
 841 }
 842
 843 static inline real trace(matrix m)
 844 {
 845   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 846 }
 847
 848 static inline real _divide(real a,real b,const char *file,int line)
 849 {
 850     if (fabs(b) <= GMX_REAL_MIN)
 851         gmx_fatal(FARGS,"Dividing by zero, file %s, line %d",file,line);
 852     return a/b;
 853 }
 854
 855 static inline int _mod(int a,int b,char *file,int line)
 856 {
 857   if(b==0)
 858     gmx_fatal(FARGS,"Modulo zero, file %s, line %d",file,line);
 859   return a % b;
 860 }
 861
 862 /* Operations on multidimensional rvecs, used e.g. in edsam.c */
 863 static void m_rveccopy(int dim, rvec *a, rvec *b)
 864 {
 865     /* b = a */
 866     int i;
 867
 868     for (i=0; i<dim; i++)
 869         copy_rvec(a[i],b[i]);
 870 }
 871
 872 /*computer matrix vectors from base vectors and angles */
 873 static void matrix_convert(matrix box, rvec vec, rvec angle)
 874 {
 875     svmul(DEG2RAD,angle,angle);
 876     box[XX][XX] = vec[XX];
 877     box[YY][XX] = vec[YY]*cos(angle[ZZ]);
 878     box[YY][YY] = vec[YY]*sin(angle[ZZ]);
 879     box[ZZ][XX] = vec[ZZ]*cos(angle[YY]);
 880     box[ZZ][YY] = vec[ZZ]
 881                          *(cos(angle[XX])-cos(angle[YY])*cos(angle[ZZ]))/sin(angle[ZZ]);
 882     box[ZZ][ZZ] = sqrt(sqr(vec[ZZ])
 883                        -box[ZZ][XX]*box[ZZ][XX]-box[ZZ][YY]*box[ZZ][YY]);
 884 }
 885
 886 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 887 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 888
 889 #ifdef __cplusplus
 890 }
 891 #endif
 892
 893
 894 #endif  /* _vec_h */