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