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