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