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