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