src/gromacs/math/3dtransforms.cpp

   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) 2010,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 #include "gromacs/math/3dtransforms.h"
  38
  39 #include <math.h>
  40 #include <stdio.h>
  41
  42 #include "gromacs/math/vec.h"
  43 #include "gromacs/utility/fatalerror.h"
  44
  45 #define N 4
  46
  47 void gmx_mat4_copy(mat4 a, mat4 b)
  48 {
  49     for (int i = 0; i < N; ++i)
  50     {
  51         for (int j = 0; j < N; ++j)
  52         {
  53             b[i][j] = a[i][j];
  54         }
  55     }
  56 }
  57
  58 void gmx_mat4_transform_point(mat4 m, rvec x, vec4 v)
  59 {
  60     int i;
  61
  62     for (i = 0; (i < N); i++)
  63     {
  64         v[i] = m[XX][i]*x[XX]+m[YY][i]*x[YY]+m[ZZ][i]*x[ZZ]+m[WW][i];
  65     }
  66 }
  67
  68 void gmx_mat4_mmul(mat4 A, mat4 B, mat4 C)
  69 {
  70     int i, j, k;
  71
  72     for (i = 0; i < N; i++)
  73     {
  74         for (j = 0; j < N; j++)
  75         {
  76             A[i][j] = 0;
  77             for (k = 0; (k < N); k++)
  78             {
  79                 A[i][j] += B[i][k]*C[k][j];
  80             }
  81         }
  82     }
  83 }
  84
  85 void gmx_mat4_init_unity(mat4 m)
  86 {
  87     int i, j;
  88
  89     for (i = 0; (i < N); i++)
  90     {
  91         for (j = 0; (j < N); j++)
  92         {
  93             if (i == j)
  94             {
  95                 m[i][j] = 1.0;
  96             }
  97             else
  98             {
  99                 m[i][j] = 0.0;
 100             }
 101         }
 102     }
 103 }
 104
 105 void gmx_mat4_init_rotation(int axis, real angle, mat4 A)
 106 {
 107     gmx_mat4_init_unity(A);
 108
 109     switch (axis)
 110     {
 111         case XX:
 112             A[YY][YY] =  cos(angle);
 113             A[YY][ZZ] = -sin(angle);
 114             A[ZZ][YY] =  sin(angle);
 115             A[ZZ][ZZ] =  cos(angle);
 116             break;
 117         case YY:
 118             A[XX][XX] =  cos(angle);
 119             A[XX][ZZ] =  sin(angle);
 120             A[ZZ][XX] = -sin(angle);
 121             A[ZZ][ZZ] =  cos(angle);
 122             break;
 123         case ZZ:
 124             A[XX][XX] =  cos(angle);
 125             A[XX][YY] = -sin(angle);
 126             A[YY][XX] =  sin(angle);
 127             A[YY][YY] =  cos(angle);
 128             break;
 129         default:
 130             gmx_fatal(FARGS, "Error: invalid axis: %d", axis);
 131     }
 132 }
 133
 134 void gmx_mat4_init_translation(real tx, real ty, real tz, mat4 A)
 135 {
 136     gmx_mat4_init_unity(A);
 137     A[3][XX] = tx;
 138     A[3][YY] = ty;
 139     A[3][ZZ] = tz;
 140 }
 141
 142 void gmx_mat4_print(FILE *fp, const char *s, mat4 A)
 143 {
 144     int i, j;
 145
 146     if (fp)
 147     {
 148         fprintf(fp, "%s: ", s);
 149         for (i = 0; i < N; i++)
 150         {
 151             fprintf(fp, "\t");
 152             for (j = 0; j < N; j++)
 153             {
 154                 fprintf(fp, "%10.5f", A[i][j]);
 155             }
 156             fprintf(fp, "\n");
 157         }
 158     }
 159 }
 160
 161 void gmx_vec4_print(FILE *fp, const char *s, vec4 a)
 162 {
 163     int j;
 164
 165     if (fp)
 166     {
 167         fprintf(fp, "%s: ", s);
 168         for (j = 0; j < N; j++)
 169         {
 170             fprintf(fp, "%10.5f", a[j]);
 171         }
 172         fprintf(fp, "\n");
 173     }
 174 }