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