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