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