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