include/vec.h

   1 /*
   2  * This file is part of the GROMACS molecular simulation package.
   3  *
   4  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
   5  * Copyright (c) 2001-2004, The GROMACS development team,
   6  * check out http://www.gromacs.org for more information.
   7  * Copyright (c) 2012, by the GROMACS development team, led by
   8  * David van der Spoel, Berk Hess, Erik Lindahl, and including many
   9  * others, as listed in the AUTHORS file in the top-level source
  10  * directory and at http://www.gromacs.org.
  11  *
  12  * GROMACS is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU Lesser General Public License
  14  * as published by the Free Software Foundation; either version 2.1
  15  * of the License, or (at your option) any later version.
  16  *
  17  * GROMACS is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  20  * Lesser General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU Lesser General Public
  23  * License along with GROMACS; if not, see
  24  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
  25  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
  26  *
  27  * If you want to redistribute modifications to GROMACS, please
  28  * consider that scientific software is very special. Version
  29  * control is crucial - bugs must be traceable. We will be happy to
  30  * consider code for inclusion in the official distribution, but
  31  * derived work must not be called official GROMACS. Details are found
  32  * in the README & COPYING files - if they are missing, get the
  33  * official version at http://www.gromacs.org.
  34  *
  35  * To help us fund GROMACS development, we humbly ask that you cite
  36  * the research papers on the package. Check out http://www.gromacs.org.
  37  */
  38 #ifndef _vec_h
  39 #define _vec_h
  40
  41 /*
  42   collection of in-line ready operations:
  43
  44   lookup-table optimized scalar operations:
  45   real gmx_invsqrt(real x)
  46   void vecinvsqrt(real in[],real out[],int n)
  47   void vecrecip(real in[],real out[],int n)
  48   real sqr(real x)
  49   double dsqr(double x)
  50
  51   vector operations:
  52   void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  53   void dvec_add(const dvec a,const dvec b,dvec c)  c = a + b
  54   void ivec_add(const ivec a,const ivec b,ivec c)  c = a + b
  55   void rvec_inc(rvec a,const rvec b)               a += b
  56   void dvec_inc(dvec a,const dvec b)               a += b
  57   void ivec_inc(ivec a,const ivec b)               a += b
  58   void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  59   void dvec_sub(const dvec a,const dvec b,dvec c)  c = a - b
  60   void rvec_dec(rvec a,rvec b)                     a -= b
  61   void copy_rvec(const rvec a,rvec b)              b = a (reals)
  62   void copy_dvec(const dvec a,dvec b)              b = a (reals)
  63   void copy_ivec(const ivec a,ivec b)              b = a (integers)
  64   void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  65   void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  66   void dsvmul(double a,dvec v1,dvec v2)            v2 = a * v1
  67   void clear_rvec(rvec a)                          a = 0
  68   void clear_dvec(dvec a)                          a = 0
  69   void clear_ivec(rvec a)                          a = 0
  70   void clear_rvecs(int n,rvec v[])
  71   real iprod(rvec a,rvec b)                        = a . b (inner product)
  72   double diprod(dvec a,dvec b)                     = a . b (inner product)
  73   real iiprod(ivec a,ivec b)                       = a . b (integers)
  74   real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  75   double dnorm2(dvec a)                            = | a |^2 ( = x*y*z )
  76   real norm(rvec a)                                = | a |
  77   double dnorm(dvec a)                             = | a |
  78   void cprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  79   void dprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  80   void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  81   real cos_angle(rvec a,rvec b)
  82   real cos_angle_no_table(rvec a,rvec b)
  83   real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  84   void unitv(rvec src,rvec dest)                   dest = src / |src|
  85   void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
  86
  87   matrix (3x3) operations:
  88     ! indicates that dest should not be the same as a, b or src
  89     the _ur0 varieties work on matrices that have only zeros
  90     in the upper right part, such as box matrices, these varieties
  91     could produce less rounding errors, not due to the operations themselves,
  92     but because the compiler can easier recombine the operations
  93   void copy_mat(matrix a,matrix b)                 b = a
  94   void clear_mat(matrix a)                         a = 0
  95   void mmul(matrix a,matrix b,matrix dest)      !  dest = a . b
  96   void mmul_ur0(matrix a,matrix b,matrix dest)     dest = a . b
  97   void transpose(matrix src,matrix dest)        !  dest = src*
  98   void tmmul(matrix a,matrix b,matrix dest)     !  dest = a* . b
  99   void mtmul(matrix a,matrix b,matrix dest)     !  dest = a . b*
 100   real det(matrix a)                               = det(a)
 101   void m_add(matrix a,matrix b,matrix dest)        dest = a + b
 102   void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
 103   void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
 104   void m_inv_ur0(matrix src,matrix dest)           dest = src^-1
 105   void m_inv(matrix src,matrix dest)            !  dest = src^-1
 106   void mvmul(matrix a,rvec src,rvec dest)       !  dest = a . src
 107   void mvmul_ur0(matrix a,rvec src,rvec dest)      dest = a . src
 108   void tmvmul_ur0(matrix a,rvec src,rvec dest)     dest = a* . src
 109   real trace(matrix m)                             = trace(m)
 110 */
 111 #include "visibility.h"
 112 #include "types/simple.h"
 113 #include "maths.h"
 114 #include "typedefs.h"
 115 #include "sysstuff.h"
 116 #include "macros.h"
 117 #include "gmx_fatal.h"
 118 #include "mpelogging.h"
 119 #include "physics.h"
 120
 121 #ifdef __cplusplus
 122 extern "C" {
 123 #elif 0
 124 } /* avoid screwing up indentation */
 125 #endif
 126
 127
 128 #define EXP_LSB         0x00800000
 129 #define EXP_MASK        0x7f800000
 130 #define EXP_SHIFT       23
 131 #define FRACT_MASK      0x007fffff
 132 #define FRACT_SIZE      11              /* significant part of fraction */
 133 #define FRACT_SHIFT     (EXP_SHIFT-FRACT_SIZE)
 134 #define EXP_ADDR(val)   (((val)&EXP_MASK)>>EXP_SHIFT)
 135 #define FRACT_ADDR(val) (((val)&(FRACT_MASK|EXP_LSB))>>FRACT_SHIFT)
 136
 137 #define PR_VEC(a)       a[XX],a[YY],a[ZZ]
 138
 139 #ifdef GMX_SOFTWARE_INVSQRT
 140 GMX_LIBGMX_EXPORT
 141 extern const unsigned int *  gmx_invsqrt_exptab;
 142 GMX_LIBGMX_EXPORT
 143 extern const unsigned int *  gmx_invsqrt_fracttab;
 144 #endif
 145
 146
 147 typedef union
 148 {
 149   unsigned int bval;
 150   float fval;
 151 } t_convert;
 152
 153
 154 #ifdef GMX_SOFTWARE_INVSQRT
 155 static real gmx_software_invsqrt(real x)
 156 {
 157   const real  half=0.5;
 158   const real  three=3.0;
 159   t_convert   result,bit_pattern;
 160   unsigned int exp,fract;
 161   real        lu;
 162   real        y;
 163 #ifdef GMX_DOUBLE
 164   real        y2;
 165 #endif
 166
 167   bit_pattern.fval=x;
 168   exp   = EXP_ADDR(bit_pattern.bval);
 169   fract = FRACT_ADDR(bit_pattern.bval);
 170   result.bval=gmx_invsqrt_exptab[exp] | gmx_invsqrt_fracttab[fract];
 171   lu    = result.fval;
 172
 173   y=(half*lu*(three-((x*lu)*lu)));
 174 #ifdef GMX_DOUBLE
 175   y2=(half*y*(three-((x*y)*y)));
 176
 177   return y2;                    /* 10 Flops */
 178 #else
 179   return y;                     /* 5  Flops */
 180 #endif
 181 }
 182 #define gmx_invsqrt(x) gmx_software_invsqrt(x)
 183 #define INVSQRT_DONE
 184 #endif /* gmx_invsqrt */
 185
 186 #ifdef GMX_POWERPC_SQRT
 187 static real gmx_powerpc_invsqrt(real x)
 188 {
 189   const real  half=0.5;
 190   const real  three=3.0;
 191   t_convert   result,bit_pattern;
 192   unsigned int exp,fract;
 193   real        lu;
 194   real        y;
 195 #ifdef GMX_DOUBLE
 196   real        y2;
 197 #endif
 198
 199   lu = __frsqrte((double)x);
 200
 201   y=(half*lu*(three-((x*lu)*lu)));
 202
 203 #if (GMX_POWERPC_SQRT==2)
 204   /* Extra iteration required */
 205   y=(half*y*(three-((x*y)*y)));
 206 #endif
 207
 208 #ifdef GMX_DOUBLE
 209   y2=(half*y*(three-((x*y)*y)));
 210
 211   return y2;                    /* 10 Flops */
 212 #else
 213   return y;                     /* 5  Flops */
 214 #endif
 215 }
 216 #define gmx_invsqrt(x) gmx_powerpc_invsqrt(x)
 217 #define INVSQRT_DONE
 218 #endif /* powerpc_invsqrt */
 219
 220 #ifndef INVSQRT_DONE
 221 #    ifdef GMX_DOUBLE
 222 #        ifdef HAVE_RSQRT
 223 #            define gmx_invsqrt(x)     rsqrt(x)
 224 #        else
 225 #            define gmx_invsqrt(x)     (1.0/sqrt(x))
 226 #        endif
 227 #    else /* single */
 228 #        ifdef HAVE_RSQRTF
 229 #            define gmx_invsqrt(x)     rsqrtf(x)
 230 #        elif defined HAVE_RSQRT
 231 #            define gmx_invsqrt(x)     rsqrt(x)
 232 #        elif defined HAVE_SQRTF
 233 #            define gmx_invsqrt(x)     (1.0/sqrtf(x))
 234 #        else
 235 #            define gmx_invsqrt(x)     (1.0/sqrt(x))
 236 #        endif
 237 #    endif
 238 #endif
 239
 240
 241 static real sqr(real x)
 242 {
 243   return (x*x);
 244 }
 245
 246 static gmx_inline double dsqr(double x)
 247 {
 248   return (x*x);
 249 }
 250
 251 /* Maclaurin series for sinh(x)/x, useful for NH chains and MTTK pressure control
 252    Here, we compute it to 10th order, which might be overkill, 8th is probably enough,
 253    but it's not very much more expensive. */
 254
 255 static gmx_inline real series_sinhx(real x)
 256 {
 257   real x2 = x*x;
 258   return (1 + (x2/6.0)*(1 + (x2/20.0)*(1 + (x2/42.0)*(1 + (x2/72.0)*(1 + (x2/110.0))))));
 259 }
 260
 261 void vecinvsqrt(real in[],real out[],int n);
 262 /* Perform out[i]=1.0/sqrt(in[i]) for n elements */
 263
 264
 265 void vecrecip(real in[],real out[],int n);
 266 /* Perform out[i]=1.0/(in[i]) for n elements */
 267
 268 /* Note: If you need a fast version of vecinvsqrt
 269  * and/or vecrecip, call detectcpu() and run the SSE/3DNow/SSE2/Altivec
 270  * versions if your hardware supports it.
 271  *
 272  * To use those routines, your memory HAS TO BE CACHE-ALIGNED.
 273  * Use snew_aligned(ptr,size,32) to allocate and sfree_aligned to free.
 274  */
 275
 276
 277 static gmx_inline void rvec_add(const rvec a,const rvec b,rvec c)
 278 {
 279   real x,y,z;
 280
 281   x=a[XX]+b[XX];
 282   y=a[YY]+b[YY];
 283   z=a[ZZ]+b[ZZ];
 284
 285   c[XX]=x;
 286   c[YY]=y;
 287   c[ZZ]=z;
 288 }
 289
 290 static gmx_inline void dvec_add(const dvec a,const dvec b,dvec c)
 291 {
 292   double x,y,z;
 293
 294   x=a[XX]+b[XX];
 295   y=a[YY]+b[YY];
 296   z=a[ZZ]+b[ZZ];
 297
 298   c[XX]=x;
 299   c[YY]=y;
 300   c[ZZ]=z;
 301 }
 302
 303 static gmx_inline void ivec_add(const ivec a,const ivec b,ivec c)
 304 {
 305   int x,y,z;
 306
 307   x=a[XX]+b[XX];
 308   y=a[YY]+b[YY];
 309   z=a[ZZ]+b[ZZ];
 310
 311   c[XX]=x;
 312   c[YY]=y;
 313   c[ZZ]=z;
 314 }
 315
 316 static gmx_inline void rvec_inc(rvec a,const rvec b)
 317 {
 318   real x,y,z;
 319
 320   x=a[XX]+b[XX];
 321   y=a[YY]+b[YY];
 322   z=a[ZZ]+b[ZZ];
 323
 324   a[XX]=x;
 325   a[YY]=y;
 326   a[ZZ]=z;
 327 }
 328
 329 static gmx_inline void dvec_inc(dvec a,const dvec b)
 330 {
 331   double x,y,z;
 332
 333   x=a[XX]+b[XX];
 334   y=a[YY]+b[YY];
 335   z=a[ZZ]+b[ZZ];
 336
 337   a[XX]=x;
 338   a[YY]=y;
 339   a[ZZ]=z;
 340 }
 341
 342 static gmx_inline void rvec_sub(const rvec a,const rvec b,rvec c)
 343 {
 344   real x,y,z;
 345
 346   x=a[XX]-b[XX];
 347   y=a[YY]-b[YY];
 348   z=a[ZZ]-b[ZZ];
 349
 350   c[XX]=x;
 351   c[YY]=y;
 352   c[ZZ]=z;
 353 }
 354
 355 static gmx_inline void dvec_sub(const dvec a,const dvec b,dvec c)
 356 {
 357   double x,y,z;
 358
 359   x=a[XX]-b[XX];
 360   y=a[YY]-b[YY];
 361   z=a[ZZ]-b[ZZ];
 362
 363   c[XX]=x;
 364   c[YY]=y;
 365   c[ZZ]=z;
 366 }
 367
 368 static gmx_inline void rvec_dec(rvec a,const rvec b)
 369 {
 370   real x,y,z;
 371
 372   x=a[XX]-b[XX];
 373   y=a[YY]-b[YY];
 374   z=a[ZZ]-b[ZZ];
 375
 376   a[XX]=x;
 377   a[YY]=y;
 378   a[ZZ]=z;
 379 }
 380
 381 static gmx_inline void copy_rvec(const rvec a,rvec b)
 382 {
 383   b[XX]=a[XX];
 384   b[YY]=a[YY];
 385   b[ZZ]=a[ZZ];
 386 }
 387
 388 static gmx_inline void copy_rvecn(rvec *a,rvec *b,int startn, int endn)
 389 {
 390   int i;
 391   for (i=startn;i<endn;i++) {
 392     b[i][XX]=a[i][XX];
 393     b[i][YY]=a[i][YY];
 394     b[i][ZZ]=a[i][ZZ];
 395   }
 396 }
 397
 398 static gmx_inline void copy_dvec(const dvec a,dvec b)
 399 {
 400   b[XX]=a[XX];
 401   b[YY]=a[YY];
 402   b[ZZ]=a[ZZ];
 403 }
 404
 405 static gmx_inline void copy_ivec(const ivec a,ivec b)
 406 {
 407   b[XX]=a[XX];
 408   b[YY]=a[YY];
 409   b[ZZ]=a[ZZ];
 410 }
 411
 412 static gmx_inline void ivec_sub(const ivec a,const ivec b,ivec c)
 413 {
 414   int x,y,z;
 415
 416   x=a[XX]-b[XX];
 417   y=a[YY]-b[YY];
 418   z=a[ZZ]-b[ZZ];
 419
 420   c[XX]=x;
 421   c[YY]=y;
 422   c[ZZ]=z;
 423 }
 424
 425 static gmx_inline void copy_mat(matrix a,matrix b)
 426 {
 427   copy_rvec(a[XX],b[XX]);
 428   copy_rvec(a[YY],b[YY]);
 429   copy_rvec(a[ZZ],b[ZZ]);
 430 }
 431
 432 static gmx_inline void svmul(real a,const rvec v1,rvec 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 void dsvmul(double a,const dvec v1,dvec v2)
 440 {
 441   v2[XX]=a*v1[XX];
 442   v2[YY]=a*v1[YY];
 443   v2[ZZ]=a*v1[ZZ];
 444 }
 445
 446 static gmx_inline real distance2(const rvec v1,const rvec v2)
 447 {
 448   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 449 }
 450
 451 static gmx_inline void clear_rvec(rvec a)
 452 {
 453   /* The ibm compiler has problems with inlining this
 454    * when we use a const real variable
 455    */
 456   a[XX]=0.0;
 457   a[YY]=0.0;
 458   a[ZZ]=0.0;
 459 }
 460
 461 static gmx_inline void clear_dvec(dvec a)
 462 {
 463   /* The ibm compiler has problems with inlining this
 464    * when we use a const real variable
 465    */
 466   a[XX]=0.0;
 467   a[YY]=0.0;
 468   a[ZZ]=0.0;
 469 }
 470
 471 static gmx_inline void clear_ivec(ivec a)
 472 {
 473   a[XX]=0;
 474   a[YY]=0;
 475   a[ZZ]=0;
 476 }
 477
 478 static gmx_inline void clear_rvecs(int n,rvec v[])
 479 {
 480 /*  memset(v[0],0,DIM*n*sizeof(v[0][0])); */
 481   int i;
 482
 483   GMX_MPE_LOG(ev_clear_rvecs_start);
 484
 485   for(i=0; (i<n); i++)
 486     clear_rvec(v[i]);
 487
 488   GMX_MPE_LOG(ev_clear_rvecs_finish);
 489 }
 490
 491 static gmx_inline void clear_mat(matrix a)
 492 {
 493 /*  memset(a[0],0,DIM*DIM*sizeof(a[0][0])); */
 494
 495   const real nul=0.0;
 496
 497   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 498   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 499   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 500 }
 501
 502 static gmx_inline real iprod(const rvec a,const rvec b)
 503 {
 504   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 505 }
 506
 507 static gmx_inline double diprod(const dvec a,const dvec b)
 508 {
 509   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 510 }
 511
 512 static gmx_inline int iiprod(const ivec a,const ivec b)
 513 {
 514   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 515 }
 516
 517 static gmx_inline real norm2(const rvec a)
 518 {
 519   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 520 }
 521
 522 static gmx_inline double dnorm2(const dvec a)
 523 {
 524   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 525 }
 526
 527 /* WARNING:
 528  * As dnorm() uses sqrt() (which is slow) _only_ use it if you are sure you
 529  * don't need 1/dnorm(), otherwise use dnorm2()*dinvnorm(). */
 530 static gmx_inline double dnorm(const dvec a)
 531 {
 532   return sqrt(diprod(a, a));
 533 }
 534
 535 /* WARNING:
 536  * As norm() uses sqrtf() (which is slow) _only_ use it if you are sure you
 537  * don't need 1/norm(), otherwise use norm2()*invnorm(). */
 538 static gmx_inline real norm(const rvec a)
 539 {
 540   /* This is ugly, but we deliberately do not define gmx_sqrt() and handle the
 541    * float/double case here instead to avoid gmx_sqrt() being accidentally used. */
 542 #ifdef GMX_DOUBLE
 543   return dnorm(a);
 544 #elif defined HAVE_SQRTF
 545   return sqrtf(iprod(a, a));
 546 #else
 547   return sqrt(iprod(a, a));
 548 #endif
 549 }
 550
 551 static gmx_inline real invnorm(const rvec a)
 552 {
 553     return gmx_invsqrt(norm2(a));
 554 }
 555
 556 static gmx_inline real dinvnorm(const dvec a)
 557 {
 558     return gmx_invsqrt(dnorm2(a));
 559 }
 560
 561 /* WARNING:
 562  * Do _not_ use these routines to calculate the angle between two vectors
 563  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 564  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 565  * Instead, use the new gmx_angle() function directly.
 566  */
 567 static gmx_inline real
 568 cos_angle(const rvec a,const rvec b)
 569 {
 570   /*
 571    *                  ax*bx + ay*by + az*bz
 572    * cos-vec (a,b) =  ---------------------
 573    *                      ||a|| * ||b||
 574    */
 575   real   cosval;
 576   int    m;
 577   double aa,bb,ip,ipa,ipb,ipab; /* For accuracy these must be double! */
 578
 579   ip=ipa=ipb=0.0;
 580   for(m=0; (m<DIM); m++) {              /* 18           */
 581     aa   = a[m];
 582     bb   = b[m];
 583     ip  += aa*bb;
 584     ipa += aa*aa;
 585     ipb += bb*bb;
 586   }
 587   ipab = ipa*ipb;
 588   if (ipab > 0)
 589     cosval = ip*gmx_invsqrt(ipab);              /*  7           */
 590   else
 591     cosval = 1;
 592                                         /* 25 TOTAL     */
 593   if (cosval > 1.0)
 594     return  1.0;
 595   if (cosval <-1.0)
 596     return -1.0;
 597
 598   return cosval;
 599 }
 600
 601 /* WARNING:
 602  * Do _not_ use these routines to calculate the angle between two vectors
 603  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 604  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 605  * Instead, use the new gmx_angle() function directly.
 606  */
 607 static gmx_inline real
 608 cos_angle_no_table(const rvec a,const rvec b)
 609 {
 610   /* This version does not need the invsqrt lookup table */
 611   real   cosval;
 612   int    m;
 613   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 614
 615   ip=ipa=ipb=0.0;
 616   for(m=0; (m<DIM); m++) {              /* 18           */
 617     aa   = a[m];
 618     bb   = b[m];
 619     ip  += aa*bb;
 620     ipa += aa*aa;
 621     ipb += bb*bb;
 622   }
 623   cosval=ip/sqrt(ipa*ipb);              /* 12           */
 624                                         /* 30 TOTAL     */
 625   if (cosval > 1.0)
 626     return  1.0;
 627   if (cosval <-1.0)
 628     return -1.0;
 629
 630   return cosval;
 631 }
 632
 633
 634 static gmx_inline void cprod(const rvec a,const rvec b,rvec 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 static gmx_inline void dcprod(const dvec a,const dvec b,dvec c)
 642 {
 643   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 644   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 645   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 646 }
 647
 648 /* This routine calculates the angle between a & b without any loss of accuracy close to 0/PI.
 649  * If you only need cos(theta), use the cos_angle() routines to save a few cycles.
 650  * This routine is faster than it might appear, since atan2 is accelerated on many CPUs (e.g. x86).
 651  */
 652 static gmx_inline real
 653 gmx_angle(const rvec a, const rvec b)
 654 {
 655     rvec w;
 656     real wlen,s;
 657
 658     cprod(a,b,w);
 659
 660     wlen  = norm(w);
 661     s     = iprod(a,b);
 662
 663     return atan2(wlen,s);
 664 }
 665
 666 static gmx_inline void mmul_ur0(matrix a,matrix b,matrix dest)
 667 {
 668   dest[XX][XX]=a[XX][XX]*b[XX][XX];
 669   dest[XX][YY]=0.0;
 670   dest[XX][ZZ]=0.0;
 671   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 672   dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
 673   dest[YY][ZZ]=0.0;
 674   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 675   dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 676   dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
 677 }
 678
 679 static gmx_inline void mmul(matrix a,matrix b,matrix dest)
 680 {
 681   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 682   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 683   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 684   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 685   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 686   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 687   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 688   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 689   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 690 }
 691
 692 static gmx_inline void transpose(matrix src,matrix dest)
 693 {
 694   dest[XX][XX]=src[XX][XX];
 695   dest[YY][XX]=src[XX][YY];
 696   dest[ZZ][XX]=src[XX][ZZ];
 697   dest[XX][YY]=src[YY][XX];
 698   dest[YY][YY]=src[YY][YY];
 699   dest[ZZ][YY]=src[YY][ZZ];
 700   dest[XX][ZZ]=src[ZZ][XX];
 701   dest[YY][ZZ]=src[ZZ][YY];
 702   dest[ZZ][ZZ]=src[ZZ][ZZ];
 703 }
 704
 705 static gmx_inline void tmmul(matrix a,matrix b,matrix dest)
 706 {
 707   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 708   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 709   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 710   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 711   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 712   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 713   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 714   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 715   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 716   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 717 }
 718
 719 static gmx_inline void mtmul(matrix a,matrix b,matrix dest)
 720 {
 721   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 722   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 723   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 724   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 725   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 726   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 727   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 728   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 729   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 730   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 731 }
 732
 733 static gmx_inline real det(matrix a)
 734 {
 735   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 736           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 737           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 738 }
 739
 740 static gmx_inline void m_add(matrix a,matrix b,matrix dest)
 741 {
 742   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 743   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 744   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 745   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 746   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 747   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 748   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 749   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 750   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 751 }
 752
 753 static gmx_inline void m_sub(matrix a,matrix b,matrix dest)
 754 {
 755   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 756   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 757   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 758   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 759   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 760   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 761   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 762   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 763   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 764 }
 765
 766 static gmx_inline void msmul(matrix m1,real r1,matrix dest)
 767 {
 768   dest[XX][XX]=r1*m1[XX][XX];
 769   dest[XX][YY]=r1*m1[XX][YY];
 770   dest[XX][ZZ]=r1*m1[XX][ZZ];
 771   dest[YY][XX]=r1*m1[YY][XX];
 772   dest[YY][YY]=r1*m1[YY][YY];
 773   dest[YY][ZZ]=r1*m1[YY][ZZ];
 774   dest[ZZ][XX]=r1*m1[ZZ][XX];
 775   dest[ZZ][YY]=r1*m1[ZZ][YY];
 776   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 777 }
 778
 779 static gmx_inline void m_inv_ur0(matrix src,matrix dest)
 780 {
 781   double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
 782   if (fabs(tmp) <= 100*GMX_REAL_MIN)
 783     gmx_fatal(FARGS,"Can not invert matrix, determinant is zero");
 784
 785   dest[XX][XX] = 1/src[XX][XX];
 786   dest[YY][YY] = 1/src[YY][YY];
 787   dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
 788   dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
 789                   - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
 790   dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
 791   dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
 792   dest[XX][YY] = 0.0;
 793   dest[XX][ZZ] = 0.0;
 794   dest[YY][ZZ] = 0.0;
 795 }
 796
 797 static gmx_inline void m_inv(matrix src,matrix dest)
 798 {
 799   const real smallreal = (real)1.0e-24;
 800   const real largereal = (real)1.0e24;
 801   real  deter,c,fc;
 802
 803   deter = det(src);
 804   c     = (real)1.0/deter;
 805   fc    = (real)fabs(c);
 806
 807   if ((fc <= smallreal) || (fc >= largereal))
 808     gmx_fatal(FARGS,"Can not invert matrix, determinant = %e",deter);
 809
 810   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 811   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 812   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 813   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 814   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 815   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 816   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 817   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 818   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 819 }
 820
 821 static gmx_inline void mvmul(matrix a,const rvec src,rvec dest)
 822 {
 823   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 824   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 825   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 826 }
 827
 828 static gmx_inline void mvmul_ur0(matrix a,const rvec src,rvec dest)
 829 {
 830   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 831   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY];
 832   dest[XX]=a[XX][XX]*src[XX];
 833 }
 834
 835 static gmx_inline void tmvmul_ur0(matrix a,const rvec src,rvec dest)
 836 {
 837   dest[XX]=a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 838   dest[YY]=                  a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 839   dest[ZZ]=                                    a[ZZ][ZZ]*src[ZZ];
 840 }
 841
 842 static gmx_inline void unitv(const rvec src,rvec dest)
 843 {
 844   real linv;
 845
 846   linv=gmx_invsqrt(norm2(src));
 847   dest[XX]=linv*src[XX];
 848   dest[YY]=linv*src[YY];
 849   dest[ZZ]=linv*src[ZZ];
 850 }
 851
 852 static gmx_inline void unitv_no_table(const rvec src,rvec dest)
 853 {
 854   real linv;
 855
 856   linv=1.0/sqrt(norm2(src));
 857   dest[XX]=linv*src[XX];
 858   dest[YY]=linv*src[YY];
 859   dest[ZZ]=linv*src[ZZ];
 860 }
 861
 862 static void calc_lll(rvec box,rvec lll)
 863 {
 864   lll[XX] = 2.0*M_PI/box[XX];
 865   lll[YY] = 2.0*M_PI/box[YY];
 866   lll[ZZ] = 2.0*M_PI/box[ZZ];
 867 }
 868
 869 static gmx_inline real trace(matrix m)
 870 {
 871   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 872 }
 873
 874 static gmx_inline real _divide(real a,real b,const char *file,int line)
 875 {
 876     if (fabs(b) <= GMX_REAL_MIN)
 877         gmx_fatal(FARGS,"Dividing by zero, file %s, line %d",file,line);
 878     return a/b;
 879 }
 880
 881 static gmx_inline int _mod(int a,int b,char *file,int line)
 882 {
 883   if(b==0)
 884     gmx_fatal(FARGS,"Modulo zero, file %s, line %d",file,line);
 885   return a % b;
 886 }
 887
 888 /* Operations on multidimensional rvecs, used e.g. in edsam.c */
 889 static void m_rveccopy(int dim, rvec *a, rvec *b)
 890 {
 891     /* b = a */
 892     int i;
 893
 894     for (i=0; i<dim; i++)
 895         copy_rvec(a[i],b[i]);
 896 }
 897
 898 /*computer matrix vectors from base vectors and angles */
 899 static void matrix_convert(matrix box, rvec vec, rvec angle)
 900 {
 901     svmul(DEG2RAD,angle,angle);
 902     box[XX][XX] = vec[XX];
 903     box[YY][XX] = vec[YY]*cos(angle[ZZ]);
 904     box[YY][YY] = vec[YY]*sin(angle[ZZ]);
 905     box[ZZ][XX] = vec[ZZ]*cos(angle[YY]);
 906     box[ZZ][YY] = vec[ZZ]
 907                          *(cos(angle[XX])-cos(angle[YY])*cos(angle[ZZ]))/sin(angle[ZZ]);
 908     box[ZZ][ZZ] = sqrt(sqr(vec[ZZ])
 909                        -box[ZZ][XX]*box[ZZ][XX]-box[ZZ][YY]*box[ZZ][YY]);
 910 }
 911
 912 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 913 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 914
 915 #ifdef __cplusplus
 916 }
 917 #endif
 918
 919
 920 #endif  /* _vec_h */