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