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 /* WARNING:
 509  * As dnorm() uses sqrt() (which is slow) _only_ use it if you are sure you
 510  * don't need 1/dnorm(), otherwise use dnorm2()*dinvnorm(). */
 511 static gmx_inline double dnorm(const dvec a)
 512 {
 513   return sqrt(diprod(a, a));
 514 }
 515
 516 /* WARNING:
 517  * As norm() uses sqrtf() (which is slow) _only_ use it if you are sure you
 518  * don't need 1/norm(), otherwise use norm2()*invnorm(). */
 519 static gmx_inline real norm(const rvec a)
 520 {
 521   /* This is ugly, but we deliberately do not define gmx_sqrt() and handle the
 522    * float/double case here instead to avoid gmx_sqrt() being accidentally used. */
 523 #ifdef GMX_DOUBLE
 524   return dnorm(a);
 525 #else
 526   return sqrtf(iprod(a, a));
 527 #endif
 528 }
 529
 530 static gmx_inline real invnorm(const rvec a)
 531 {
 532     return gmx_invsqrt(norm2(a));
 533 }
 534
 535 static gmx_inline real dinvnorm(const dvec a)
 536 {
 537     return gmx_invsqrt(dnorm2(a));
 538 }
 539
 540 /* WARNING:
 541  * Do _not_ use these routines to calculate the angle between two vectors
 542  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 543  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 544  * Instead, use the new gmx_angle() function directly.
 545  */
 546 static gmx_inline real
 547 cos_angle(const rvec a,const rvec b)
 548 {
 549   /*
 550    *                  ax*bx + ay*by + az*bz
 551    * cos-vec (a,b) =  ---------------------
 552    *                      ||a|| * ||b||
 553    */
 554   real   cosval;
 555   int    m;
 556   double aa,bb,ip,ipa,ipb,ipab; /* For accuracy these must be double! */
 557
 558   ip=ipa=ipb=0.0;
 559   for(m=0; (m<DIM); m++) {              /* 18           */
 560     aa   = a[m];
 561     bb   = b[m];
 562     ip  += aa*bb;
 563     ipa += aa*aa;
 564     ipb += bb*bb;
 565   }
 566   ipab = ipa*ipb;
 567   if (ipab > 0)
 568     cosval = ip*gmx_invsqrt(ipab);              /*  7           */
 569   else
 570     cosval = 1;
 571                                         /* 25 TOTAL     */
 572   if (cosval > 1.0)
 573     return  1.0;
 574   if (cosval <-1.0)
 575     return -1.0;
 576
 577   return cosval;
 578 }
 579
 580 /* WARNING:
 581  * Do _not_ use these routines to calculate the angle between two vectors
 582  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 583  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 584  * Instead, use the new gmx_angle() function directly.
 585  */
 586 static gmx_inline real
 587 cos_angle_no_table(const rvec a,const rvec b)
 588 {
 589   /* This version does not need the invsqrt lookup table */
 590   real   cosval;
 591   int    m;
 592   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 593
 594   ip=ipa=ipb=0.0;
 595   for(m=0; (m<DIM); m++) {              /* 18           */
 596     aa   = a[m];
 597     bb   = b[m];
 598     ip  += aa*bb;
 599     ipa += aa*aa;
 600     ipb += bb*bb;
 601   }
 602   cosval=ip/sqrt(ipa*ipb);              /* 12           */
 603                                         /* 30 TOTAL     */
 604   if (cosval > 1.0)
 605     return  1.0;
 606   if (cosval <-1.0)
 607     return -1.0;
 608
 609   return cosval;
 610 }
 611
 612
 613 static gmx_inline void cprod(const rvec a,const rvec b,rvec c)
 614 {
 615   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 616   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 617   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 618 }
 619
 620 static gmx_inline void dcprod(const dvec a,const dvec b,dvec c)
 621 {
 622   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 623   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 624   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 625 }
 626
 627 /* This routine calculates the angle between a & b without any loss of accuracy close to 0/PI.
 628  * If you only need cos(theta), use the cos_angle() routines to save a few cycles.
 629  * This routine is faster than it might appear, since atan2 is accelerated on many CPUs (e.g. x86).
 630  */
 631 static gmx_inline real
 632 gmx_angle(const rvec a, const rvec b)
 633 {
 634     rvec w;
 635     real wlen,s;
 636
 637     cprod(a,b,w);
 638
 639     wlen  = norm(w);
 640     s     = iprod(a,b);
 641
 642     return atan2(wlen,s);
 643 }
 644
 645 static gmx_inline void mmul_ur0(matrix a,matrix b,matrix dest)
 646 {
 647   dest[XX][XX]=a[XX][XX]*b[XX][XX];
 648   dest[XX][YY]=0.0;
 649   dest[XX][ZZ]=0.0;
 650   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 651   dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
 652   dest[YY][ZZ]=0.0;
 653   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 654   dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 655   dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
 656 }
 657
 658 static gmx_inline void mmul(matrix a,matrix b,matrix dest)
 659 {
 660   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 661   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 662   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 663   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 664   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 665   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 666   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 667   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 668   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 669 }
 670
 671 static gmx_inline void transpose(matrix src,matrix dest)
 672 {
 673   dest[XX][XX]=src[XX][XX];
 674   dest[YY][XX]=src[XX][YY];
 675   dest[ZZ][XX]=src[XX][ZZ];
 676   dest[XX][YY]=src[YY][XX];
 677   dest[YY][YY]=src[YY][YY];
 678   dest[ZZ][YY]=src[YY][ZZ];
 679   dest[XX][ZZ]=src[ZZ][XX];
 680   dest[YY][ZZ]=src[ZZ][YY];
 681   dest[ZZ][ZZ]=src[ZZ][ZZ];
 682 }
 683
 684 static gmx_inline void tmmul(matrix a,matrix b,matrix dest)
 685 {
 686   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 687   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 688   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 689   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 690   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 691   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 692   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 693   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 694   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 695   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 696 }
 697
 698 static gmx_inline void mtmul(matrix a,matrix b,matrix dest)
 699 {
 700   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 701   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 702   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 703   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 704   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 705   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 706   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 707   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 708   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 709   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 710 }
 711
 712 static gmx_inline real det(matrix a)
 713 {
 714   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 715           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 716           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 717 }
 718
 719 static gmx_inline void m_add(matrix a,matrix b,matrix dest)
 720 {
 721   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 722   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 723   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 724   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 725   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 726   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 727   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 728   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 729   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 730 }
 731
 732 static gmx_inline void m_sub(matrix a,matrix b,matrix dest)
 733 {
 734   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 735   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 736   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 737   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 738   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 739   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 740   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 741   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 742   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 743 }
 744
 745 static gmx_inline void msmul(matrix m1,real r1,matrix dest)
 746 {
 747   dest[XX][XX]=r1*m1[XX][XX];
 748   dest[XX][YY]=r1*m1[XX][YY];
 749   dest[XX][ZZ]=r1*m1[XX][ZZ];
 750   dest[YY][XX]=r1*m1[YY][XX];
 751   dest[YY][YY]=r1*m1[YY][YY];
 752   dest[YY][ZZ]=r1*m1[YY][ZZ];
 753   dest[ZZ][XX]=r1*m1[ZZ][XX];
 754   dest[ZZ][YY]=r1*m1[ZZ][YY];
 755   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 756 }
 757
 758 static gmx_inline void m_inv_ur0(matrix src,matrix dest)
 759 {
 760   double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
 761   if (fabs(tmp) <= 100*GMX_REAL_MIN)
 762     gmx_fatal(FARGS,"Can not invert matrix, determinant is zero");
 763
 764   dest[XX][XX] = 1/src[XX][XX];
 765   dest[YY][YY] = 1/src[YY][YY];
 766   dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
 767   dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
 768                   - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
 769   dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
 770   dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
 771   dest[XX][YY] = 0.0;
 772   dest[XX][ZZ] = 0.0;
 773   dest[YY][ZZ] = 0.0;
 774 }
 775
 776 static gmx_inline void m_inv(matrix src,matrix dest)
 777 {
 778   const real smallreal = (real)1.0e-24;
 779   const real largereal = (real)1.0e24;
 780   real  deter,c,fc;
 781
 782   deter = det(src);
 783   c     = (real)1.0/deter;
 784   fc    = (real)fabs(c);
 785
 786   if ((fc <= smallreal) || (fc >= largereal))
 787     gmx_fatal(FARGS,"Can not invert matrix, determinant = %e",deter);
 788
 789   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 790   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 791   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 792   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 793   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 794   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 795   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 796   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 797   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 798 }
 799
 800 static gmx_inline void mvmul(matrix a,const rvec src,rvec dest)
 801 {
 802   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 803   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 804   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 805 }
 806
 807 static gmx_inline void mvmul_ur0(matrix a,const rvec src,rvec dest)
 808 {
 809   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 810   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY];
 811   dest[XX]=a[XX][XX]*src[XX];
 812 }
 813
 814 static gmx_inline void tmvmul_ur0(matrix a,const rvec src,rvec dest)
 815 {
 816   dest[XX]=a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 817   dest[YY]=                  a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 818   dest[ZZ]=                                    a[ZZ][ZZ]*src[ZZ];
 819 }
 820
 821 static gmx_inline void unitv(const rvec src,rvec dest)
 822 {
 823   real linv;
 824
 825   linv=gmx_invsqrt(norm2(src));
 826   dest[XX]=linv*src[XX];
 827   dest[YY]=linv*src[YY];
 828   dest[ZZ]=linv*src[ZZ];
 829 }
 830
 831 static gmx_inline void unitv_no_table(const rvec src,rvec dest)
 832 {
 833   real linv;
 834
 835   linv=1.0/sqrt(norm2(src));
 836   dest[XX]=linv*src[XX];
 837   dest[YY]=linv*src[YY];
 838   dest[ZZ]=linv*src[ZZ];
 839 }
 840
 841 static void calc_lll(rvec box,rvec lll)
 842 {
 843   lll[XX] = 2.0*M_PI/box[XX];
 844   lll[YY] = 2.0*M_PI/box[YY];
 845   lll[ZZ] = 2.0*M_PI/box[ZZ];
 846 }
 847
 848 static gmx_inline real trace(matrix m)
 849 {
 850   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 851 }
 852
 853 static gmx_inline real _divide(real a,real b,const char *file,int line)
 854 {
 855     if (fabs(b) <= GMX_REAL_MIN)
 856         gmx_fatal(FARGS,"Dividing by zero, file %s, line %d",file,line);
 857     return a/b;
 858 }
 859
 860 static gmx_inline int _mod(int a,int b,char *file,int line)
 861 {
 862   if(b==0)
 863     gmx_fatal(FARGS,"Modulo zero, file %s, line %d",file,line);
 864   return a % b;
 865 }
 866
 867 /* Operations on multidimensional rvecs, used e.g. in edsam.c */
 868 static void m_rveccopy(int dim, rvec *a, rvec *b)
 869 {
 870     /* b = a */
 871     int i;
 872
 873     for (i=0; i<dim; i++)
 874         copy_rvec(a[i],b[i]);
 875 }
 876
 877 /*computer matrix vectors from base vectors and angles */
 878 static void matrix_convert(matrix box, rvec vec, rvec angle)
 879 {
 880     svmul(DEG2RAD,angle,angle);
 881     box[XX][XX] = vec[XX];
 882     box[YY][XX] = vec[YY]*cos(angle[ZZ]);
 883     box[YY][YY] = vec[YY]*sin(angle[ZZ]);
 884     box[ZZ][XX] = vec[ZZ]*cos(angle[YY]);
 885     box[ZZ][YY] = vec[ZZ]
 886                          *(cos(angle[XX])-cos(angle[YY])*cos(angle[ZZ]))/sin(angle[ZZ]);
 887     box[ZZ][ZZ] = sqrt(sqr(vec[ZZ])
 888                        -box[ZZ][XX]*box[ZZ][XX]-box[ZZ][YY]*box[ZZ][YY]);
 889 }
 890
 891 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 892 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 893
 894 #ifdef __cplusplus
 895 }
 896 #endif
 897
 898
 899 #endif  /* _vec_h */