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