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