src/gromacs/math/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,2015,2016, 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 GMX_MATH_VEC_H
  38 #define GMX_MATH_VEC_H
  39
  40 /*
  41    collection of in-line ready operations:
  42
  43    vector operations:
  44    void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  45    void dvec_add(const dvec a,const dvec b,dvec c)  c = a + b
  46    void ivec_add(const ivec a,const ivec b,ivec c)  c = a + b
  47    void rvec_inc(rvec a,const rvec b)               a += b
  48    void dvec_inc(dvec a,const dvec b)               a += b
  49    void ivec_inc(ivec a,const ivec b)               a += b
  50    void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  51    void dvec_sub(const dvec a,const dvec b,dvec c)  c = a - b
  52    void rvec_dec(rvec a,rvec b)                     a -= b
  53    void copy_rvec(const rvec a,rvec b)              b = a (reals)
  54    void copy_dvec(const dvec a,dvec b)              b = a (reals)
  55    void copy_rvec_to_dvec(const rvec a,dvec b)      b = a (reals)
  56    void copy_dvec_to_rvec(const dvec a,rvec b)      b = a (reals)
  57    void copy_ivec(const ivec a,ivec b)              b = a (integers)
  58    void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  59    void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  60    void dsvmul(double a,dvec v1,dvec v2)            v2 = a * v1
  61    void clear_rvec(rvec a)                          a = 0
  62    void clear_dvec(dvec a)                          a = 0
  63    void clear_ivec(rvec a)                          a = 0
  64    void clear_rvecs(int n,rvec v[])
  65    real iprod(rvec a,rvec b)                        = a . b (inner product)
  66    double diprod(dvec a,dvec b)                     = a . b (inner product)
  67    real iiprod(ivec a,ivec b)                       = a . b (integers)
  68    real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  69    double dnorm2(dvec a)                            = | a |^2 ( = x*y*z )
  70    real norm(rvec a)                                = | a |
  71    double dnorm(dvec a)                             = | a |
  72    void cprod(rvec a,rvec b,rvec c)                 c = a x b (cross product)
  73    void dcprod(dvec a,dvec b,dvec c)                c = a x b (cross product)
  74    void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  75    real cos_angle(rvec a,rvec b)
  76    real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  77    void unitv(rvec src,rvec dest)                   dest = src / |src|
  78
  79    matrix (3x3) operations:
  80     ! indicates that dest should not be the same as a, b or src
  81     the _ur0 varieties work on matrices that have only zeros
  82     in the upper right part, such as box matrices, these varieties
  83     could produce less rounding errors, not due to the operations themselves,
  84     but because the compiler can easier recombine the operations
  85    void copy_mat(matrix a,matrix b)                 b = a
  86    void clear_mat(matrix a)                         a = 0
  87    void mmul(matrix a,matrix b,matrix dest)      !  dest = a . b
  88    void mmul_ur0(matrix a,matrix b,matrix dest)     dest = a . b
  89    void transpose(matrix src,matrix dest)        !  dest = src*
  90    void tmmul(matrix a,matrix b,matrix dest)     !  dest = a* . b
  91    void mtmul(matrix a,matrix b,matrix dest)     !  dest = a . b*
  92    real det(matrix a)                               = det(a)
  93    void m_add(matrix a,matrix b,matrix dest)        dest = a + b
  94    void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
  95    void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
  96    void mvmul(matrix a,rvec src,rvec dest)       !  dest = a . src
  97    void mvmul_ur0(matrix a,rvec src,rvec dest)      dest = a . src
  98    void tmvmul_ur0(matrix a,rvec src,rvec dest)     dest = a* . src
  99    real trace(matrix m)                             = trace(m)
 100  */
 101
 102 #include <cmath>
 103
 104 #include "gromacs/math/functions.h"
 105 #include "gromacs/math/vectypes.h"
 106 #include "gromacs/utility/real.h"
 107
 108 static inline void rvec_add(const rvec a, const rvec b, rvec c)
 109 {
 110     real x, y, z;
 111
 112     x = a[XX]+b[XX];
 113     y = a[YY]+b[YY];
 114     z = a[ZZ]+b[ZZ];
 115
 116     c[XX] = x;
 117     c[YY] = y;
 118     c[ZZ] = z;
 119 }
 120
 121 static inline void dvec_add(const dvec a, const dvec b, dvec c)
 122 {
 123     double x, y, z;
 124
 125     x = a[XX]+b[XX];
 126     y = a[YY]+b[YY];
 127     z = a[ZZ]+b[ZZ];
 128
 129     c[XX] = x;
 130     c[YY] = y;
 131     c[ZZ] = z;
 132 }
 133
 134 static inline void ivec_add(const ivec a, const ivec b, ivec c)
 135 {
 136     int x, y, z;
 137
 138     x = a[XX]+b[XX];
 139     y = a[YY]+b[YY];
 140     z = a[ZZ]+b[ZZ];
 141
 142     c[XX] = x;
 143     c[YY] = y;
 144     c[ZZ] = z;
 145 }
 146
 147 static inline void rvec_inc(rvec a, const rvec b)
 148 {
 149     real x, y, z;
 150
 151     x = a[XX]+b[XX];
 152     y = a[YY]+b[YY];
 153     z = a[ZZ]+b[ZZ];
 154
 155     a[XX] = x;
 156     a[YY] = y;
 157     a[ZZ] = z;
 158 }
 159
 160 static inline void dvec_inc(dvec a, const dvec b)
 161 {
 162     double x, y, z;
 163
 164     x = a[XX]+b[XX];
 165     y = a[YY]+b[YY];
 166     z = a[ZZ]+b[ZZ];
 167
 168     a[XX] = x;
 169     a[YY] = y;
 170     a[ZZ] = z;
 171 }
 172
 173 static inline void rvec_sub(const rvec a, const rvec b, rvec c)
 174 {
 175     real x, y, z;
 176
 177     x = a[XX]-b[XX];
 178     y = a[YY]-b[YY];
 179     z = a[ZZ]-b[ZZ];
 180
 181     c[XX] = x;
 182     c[YY] = y;
 183     c[ZZ] = z;
 184 }
 185
 186 static inline void dvec_sub(const dvec a, const dvec b, dvec c)
 187 {
 188     double x, y, z;
 189
 190     x = a[XX]-b[XX];
 191     y = a[YY]-b[YY];
 192     z = a[ZZ]-b[ZZ];
 193
 194     c[XX] = x;
 195     c[YY] = y;
 196     c[ZZ] = z;
 197 }
 198
 199 static inline void rvec_dec(rvec a, const rvec b)
 200 {
 201     real x, y, z;
 202
 203     x = a[XX]-b[XX];
 204     y = a[YY]-b[YY];
 205     z = a[ZZ]-b[ZZ];
 206
 207     a[XX] = x;
 208     a[YY] = y;
 209     a[ZZ] = z;
 210 }
 211
 212 static inline void copy_rvec(const rvec a, rvec b)
 213 {
 214     b[XX] = a[XX];
 215     b[YY] = a[YY];
 216     b[ZZ] = a[ZZ];
 217 }
 218
 219 static inline void copy_rvec_to_dvec(const rvec a, dvec b)
 220 {
 221     b[XX] = a[XX];
 222     b[YY] = a[YY];
 223     b[ZZ] = a[ZZ];
 224 }
 225
 226 static inline void copy_dvec_to_rvec(const dvec a, rvec b)
 227 {
 228     b[XX] = a[XX];
 229     b[YY] = a[YY];
 230     b[ZZ] = a[ZZ];
 231 }
 232
 233 static inline void copy_rvecn(const rvec *a, rvec *b, int startn, int endn)
 234 {
 235     int i;
 236     for (i = startn; i < endn; i++)
 237     {
 238         b[i][XX] = a[i][XX];
 239         b[i][YY] = a[i][YY];
 240         b[i][ZZ] = a[i][ZZ];
 241     }
 242 }
 243
 244 static inline void copy_dvec(const dvec a, dvec b)
 245 {
 246     b[XX] = a[XX];
 247     b[YY] = a[YY];
 248     b[ZZ] = a[ZZ];
 249 }
 250
 251 static inline void copy_ivec(const ivec a, ivec b)
 252 {
 253     b[XX] = a[XX];
 254     b[YY] = a[YY];
 255     b[ZZ] = a[ZZ];
 256 }
 257
 258 static inline void ivec_sub(const ivec a, const ivec b, ivec c)
 259 {
 260     int x, y, z;
 261
 262     x = a[XX]-b[XX];
 263     y = a[YY]-b[YY];
 264     z = a[ZZ]-b[ZZ];
 265
 266     c[XX] = x;
 267     c[YY] = y;
 268     c[ZZ] = z;
 269 }
 270
 271 static inline void copy_mat(const matrix a, matrix b)
 272 {
 273     copy_rvec(a[XX], b[XX]);
 274     copy_rvec(a[YY], b[YY]);
 275     copy_rvec(a[ZZ], b[ZZ]);
 276 }
 277
 278 static inline void svmul(real a, const rvec v1, rvec v2)
 279 {
 280     v2[XX] = a*v1[XX];
 281     v2[YY] = a*v1[YY];
 282     v2[ZZ] = a*v1[ZZ];
 283 }
 284
 285 static inline void dsvmul(double a, const dvec v1, dvec v2)
 286 {
 287     v2[XX] = a*v1[XX];
 288     v2[YY] = a*v1[YY];
 289     v2[ZZ] = a*v1[ZZ];
 290 }
 291
 292 static inline real distance2(const rvec v1, const rvec v2)
 293 {
 294     return gmx::square(v2[XX]-v1[XX]) + gmx::square(v2[YY]-v1[YY]) + gmx::square(v2[ZZ]-v1[ZZ]);
 295 }
 296
 297 static inline void clear_rvec(rvec a)
 298 {
 299     /* The ibm compiler has problems with inlining this
 300      * when we use a const real variable
 301      */
 302     a[XX] = 0.0;
 303     a[YY] = 0.0;
 304     a[ZZ] = 0.0;
 305 }
 306
 307 static inline void clear_dvec(dvec a)
 308 {
 309     /* The ibm compiler has problems with inlining this
 310      * when we use a const real variable
 311      */
 312     a[XX] = 0.0;
 313     a[YY] = 0.0;
 314     a[ZZ] = 0.0;
 315 }
 316
 317 static inline void clear_ivec(ivec a)
 318 {
 319     a[XX] = 0;
 320     a[YY] = 0;
 321     a[ZZ] = 0;
 322 }
 323
 324 static inline void clear_rvecs(int n, rvec v[])
 325 {
 326     int i;
 327
 328     for (i = 0; (i < n); i++)
 329     {
 330         clear_rvec(v[i]);
 331     }
 332 }
 333
 334 static inline void clear_mat(matrix a)
 335 {
 336     const real nul = 0.0;
 337
 338     a[XX][XX] = a[XX][YY] = a[XX][ZZ] = nul;
 339     a[YY][XX] = a[YY][YY] = a[YY][ZZ] = nul;
 340     a[ZZ][XX] = a[ZZ][YY] = a[ZZ][ZZ] = nul;
 341 }
 342
 343 static inline real iprod(const rvec a, const rvec b)
 344 {
 345     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 346 }
 347
 348 static inline double diprod(const dvec a, const dvec b)
 349 {
 350     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 351 }
 352
 353 static inline int iiprod(const ivec a, const ivec b)
 354 {
 355     return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 356 }
 357
 358 static inline real norm2(const rvec a)
 359 {
 360     return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 361 }
 362
 363 static inline double dnorm2(const dvec a)
 364 {
 365     return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 366 }
 367
 368 /* WARNING:
 369  * As dnorm() uses sqrt() (which is slow) _only_ use it if you are sure you
 370  * don't need 1/dnorm(), otherwise use dnorm2()*dinvnorm(). */
 371 static inline double dnorm(const dvec a)
 372 {
 373     return std::sqrt(diprod(a, a));
 374 }
 375
 376 /* WARNING:
 377  * As norm() uses sqrt() (which is slow) _only_ use it if you are sure you
 378  * don't need 1/norm(), otherwise use norm2()*invnorm(). */
 379 static inline real norm(const rvec a)
 380 {
 381     return std::sqrt(iprod(a, a));
 382 }
 383
 384 static inline real invnorm(const rvec a)
 385 {
 386     return gmx::invsqrt(norm2(a));
 387 }
 388
 389 static inline real dinvnorm(const dvec a)
 390 {
 391     return gmx::invsqrt(dnorm2(a));
 392 }
 393
 394 /* WARNING:
 395  * Do _not_ use these routines to calculate the angle between two vectors
 396  * as acos(cos_angle(u,v)). While it might seem obvious, the acos function
 397  * is very flat close to -1 and 1, which will lead to accuracy-loss.
 398  * Instead, use the new gmx_angle() function directly.
 399  */
 400 static inline real cos_angle(const rvec a, const rvec b)
 401 {
 402     /*
 403      *                  ax*bx + ay*by + az*bz
 404      * cos-vec (a,b) =  ---------------------
 405      *                      ||a|| * ||b||
 406      */
 407     real   cosval;
 408     int    m;
 409     double aa, bb, ip, ipa, ipb, ipab; /* For accuracy these must be double! */
 410
 411     ip = ipa = ipb = 0.0;
 412     for (m = 0; (m < DIM); m++) /* 18 */
 413     {
 414         aa   = a[m];
 415         bb   = b[m];
 416         ip  += aa*bb;
 417         ipa += aa*aa;
 418         ipb += bb*bb;
 419     }
 420     ipab = ipa*ipb;
 421     if (ipab > 0)
 422     {
 423         cosval = ip*gmx::invsqrt(ipab);  /*  7 */
 424     }
 425     else
 426     {
 427         cosval = 1;
 428     }
 429     /* 25 TOTAL */
 430     if (cosval > 1.0)
 431     {
 432         return 1.0;
 433     }
 434     if (cosval < -1.0)
 435     {
 436         return -1.0;
 437     }
 438
 439     return cosval;
 440 }
 441
 442 static inline void cprod(const rvec a, const rvec b, rvec c)
 443 {
 444     c[XX] = a[YY]*b[ZZ]-a[ZZ]*b[YY];
 445     c[YY] = a[ZZ]*b[XX]-a[XX]*b[ZZ];
 446     c[ZZ] = a[XX]*b[YY]-a[YY]*b[XX];
 447 }
 448
 449 static inline void dcprod(const dvec a, const dvec b, dvec c)
 450 {
 451     c[XX] = a[YY]*b[ZZ]-a[ZZ]*b[YY];
 452     c[YY] = a[ZZ]*b[XX]-a[XX]*b[ZZ];
 453     c[ZZ] = a[XX]*b[YY]-a[YY]*b[XX];
 454 }
 455
 456 /* This routine calculates the angle between a & b without any loss of accuracy close to 0/PI.
 457  * If you only need cos(theta), use the cos_angle() routines to save a few cycles.
 458  * This routine is faster than it might appear, since atan2 is accelerated on many CPUs (e.g. x86).
 459  */
 460 static inline real gmx_angle(const rvec a, const rvec b)
 461 {
 462     rvec w;
 463     real wlen, s;
 464
 465     cprod(a, b, w);
 466
 467     wlen  = norm(w);
 468     s     = iprod(a, b);
 469
 470     return std::atan2(wlen, s);
 471 }
 472
 473 static inline double gmx_angle_between_dvecs(const dvec a, const dvec b)
 474 {
 475     dvec   w;
 476     double wlen, s;
 477
 478     dcprod(a, b, w);
 479
 480     wlen  = dnorm(w);
 481     s     = diprod(a, b);
 482
 483     return std::atan2(wlen, s);
 484 }
 485
 486 static inline void mmul_ur0(const matrix a, const matrix b, matrix dest)
 487 {
 488     dest[XX][XX] = a[XX][XX]*b[XX][XX];
 489     dest[XX][YY] = 0.0;
 490     dest[XX][ZZ] = 0.0;
 491     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
 492     dest[YY][YY] =                     a[YY][YY]*b[YY][YY];
 493     dest[YY][ZZ] = 0.0;
 494     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 495     dest[ZZ][YY] =                     a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 496     dest[ZZ][ZZ] =                                         a[ZZ][ZZ]*b[ZZ][ZZ];
 497 }
 498
 499 static inline void mmul(const matrix a, const matrix b, matrix dest)
 500 {
 501     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 502     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 503     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 504     dest[XX][YY] = a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 505     dest[YY][YY] = a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 506     dest[ZZ][YY] = a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 507     dest[XX][ZZ] = a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 508     dest[YY][ZZ] = a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 509     dest[ZZ][ZZ] = a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 510 }
 511
 512 static inline void transpose(const matrix src, matrix dest)
 513 {
 514     dest[XX][XX] = src[XX][XX];
 515     dest[YY][XX] = src[XX][YY];
 516     dest[ZZ][XX] = src[XX][ZZ];
 517     dest[XX][YY] = src[YY][XX];
 518     dest[YY][YY] = src[YY][YY];
 519     dest[ZZ][YY] = src[YY][ZZ];
 520     dest[XX][ZZ] = src[ZZ][XX];
 521     dest[YY][ZZ] = src[ZZ][YY];
 522     dest[ZZ][ZZ] = src[ZZ][ZZ];
 523 }
 524
 525 static inline void tmmul(const matrix a, const matrix b, matrix dest)
 526 {
 527     /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 528     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 529     dest[XX][YY] = a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 530     dest[XX][ZZ] = a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 531     dest[YY][XX] = a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 532     dest[YY][YY] = a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 533     dest[YY][ZZ] = a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 534     dest[ZZ][XX] = a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 535     dest[ZZ][YY] = a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 536     dest[ZZ][ZZ] = a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 537 }
 538
 539 static inline void mtmul(const matrix a, const matrix b, matrix dest)
 540 {
 541     /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 542     dest[XX][XX] = a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 543     dest[XX][YY] = a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 544     dest[XX][ZZ] = a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 545     dest[YY][XX] = a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 546     dest[YY][YY] = a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 547     dest[YY][ZZ] = a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 548     dest[ZZ][XX] = a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 549     dest[ZZ][YY] = a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 550     dest[ZZ][ZZ] = a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 551 }
 552
 553 static inline real det(const matrix a)
 554 {
 555     return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 556              -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 557              +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 558 }
 559
 560
 561 static inline void m_add(const matrix a, const matrix b, matrix dest)
 562 {
 563     dest[XX][XX] = a[XX][XX]+b[XX][XX];
 564     dest[XX][YY] = a[XX][YY]+b[XX][YY];
 565     dest[XX][ZZ] = a[XX][ZZ]+b[XX][ZZ];
 566     dest[YY][XX] = a[YY][XX]+b[YY][XX];
 567     dest[YY][YY] = a[YY][YY]+b[YY][YY];
 568     dest[YY][ZZ] = a[YY][ZZ]+b[YY][ZZ];
 569     dest[ZZ][XX] = a[ZZ][XX]+b[ZZ][XX];
 570     dest[ZZ][YY] = a[ZZ][YY]+b[ZZ][YY];
 571     dest[ZZ][ZZ] = a[ZZ][ZZ]+b[ZZ][ZZ];
 572 }
 573
 574 static inline void m_sub(const matrix a, const matrix b, matrix dest)
 575 {
 576     dest[XX][XX] = a[XX][XX]-b[XX][XX];
 577     dest[XX][YY] = a[XX][YY]-b[XX][YY];
 578     dest[XX][ZZ] = a[XX][ZZ]-b[XX][ZZ];
 579     dest[YY][XX] = a[YY][XX]-b[YY][XX];
 580     dest[YY][YY] = a[YY][YY]-b[YY][YY];
 581     dest[YY][ZZ] = a[YY][ZZ]-b[YY][ZZ];
 582     dest[ZZ][XX] = a[ZZ][XX]-b[ZZ][XX];
 583     dest[ZZ][YY] = a[ZZ][YY]-b[ZZ][YY];
 584     dest[ZZ][ZZ] = a[ZZ][ZZ]-b[ZZ][ZZ];
 585 }
 586
 587 static inline void msmul(const matrix m1, real r1, matrix dest)
 588 {
 589     dest[XX][XX] = r1*m1[XX][XX];
 590     dest[XX][YY] = r1*m1[XX][YY];
 591     dest[XX][ZZ] = r1*m1[XX][ZZ];
 592     dest[YY][XX] = r1*m1[YY][XX];
 593     dest[YY][YY] = r1*m1[YY][YY];
 594     dest[YY][ZZ] = r1*m1[YY][ZZ];
 595     dest[ZZ][XX] = r1*m1[ZZ][XX];
 596     dest[ZZ][YY] = r1*m1[ZZ][YY];
 597     dest[ZZ][ZZ] = r1*m1[ZZ][ZZ];
 598 }
 599
 600 static inline void mvmul(const matrix a, const rvec src, rvec dest)
 601 {
 602     dest[XX] = a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 603     dest[YY] = a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 604     dest[ZZ] = a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 605 }
 606
 607
 608 static inline void mvmul_ur0(const matrix a, const rvec src, rvec dest)
 609 {
 610     dest[ZZ] = a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 611     dest[YY] = a[YY][XX]*src[XX]+a[YY][YY]*src[YY];
 612     dest[XX] = a[XX][XX]*src[XX];
 613 }
 614
 615 static inline void tmvmul_ur0(const matrix a, const rvec src, rvec dest)
 616 {
 617     dest[XX] = a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
 618     dest[YY] =                   a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
 619     dest[ZZ] =                                     a[ZZ][ZZ]*src[ZZ];
 620 }
 621
 622 static inline void unitv(const rvec src, rvec dest)
 623 {
 624     real linv;
 625
 626     linv     = gmx::invsqrt(norm2(src));
 627     dest[XX] = linv*src[XX];
 628     dest[YY] = linv*src[YY];
 629     dest[ZZ] = linv*src[ZZ];
 630 }
 631
 632 static inline real trace(const matrix m)
 633 {
 634     return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 635 }
 636
 637 #endif