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