sort of desided const
[alexxy/gromacs-fitng.git] / src / fitng.cpp
1 /*
2  * This file is part of the GROMACS molecular simulation package.
3  *
4  * Copyright (c) 2013,2014,2015, by the GROMACS development team, led by
5  * Mark Abraham, David van der Spoel, Berk Hess, and Erik Lindahl,
6  * and including many others, as listed in the AUTHORS file in the
7  * top-level source directory and at http://www.gromacs.org.
8  *
9  * GROMACS is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public License
11  * as published by the Free Software Foundation; either version 2.1
12  * of the License, or (at your option) any later version.
13  *
14  * GROMACS is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  * Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public
20  * License along with GROMACS; if not, see
21  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
22  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
23  *
24  * If you want to redistribute modifications to GROMACS, please
25  * consider that scientific software is very special. Version
26  * control is crucial - bugs must be traceable. We will be happy to
27  * consider code for inclusion in the official distribution, but
28  * derived work must not be called official GROMACS. Details are found
29  * in the README & COPYING files - if they are missing, get the
30  * official version at http://www.gromacs.org.
31  *
32  * To help us fund GROMACS development, we humbly ask that you cite
33  * the research papers on the package. Check out http://www.gromacs.org.
34  */
35 /*! \internal \file
36  * \brief
37  * Implements gmx::analysismodules::Freevolume.
38  *
39  * \author Titov Anatoly <Wapuk-cobaka@yandex.ru>
40  * \ingroup module_trajectoryanalysis
41  */
42
43 #include <iostream>
44 #include <chrono>
45 #include <string.h>
46 #include <algorithm>
47
48 #include <omp.h>
49
50 #include <gromacs/trajectoryanalysis.h>
51 #include <gromacs/pbcutil/pbc.h>
52 #include <gromacs/utility/smalloc.h>
53 #include <gromacs/math/do_fit.h>
54 #include <gromacs/fileio/trxio.h>
55
56 using namespace gmx;
57
58 using gmx::RVec;
59
60 double F (double aix, double aiy, double aiz, double bix_plus_x, double biy_plus_y, double biz_plus_z, double A, double B, double C) {
61     return  sqrt(   pow(aix + biy_plus_y * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - biz_plus_z * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * bix_plus_x, 2) +
62                     pow(aiy - biy_plus_y * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + biz_plus_z * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * bix_plus_x, 2) +
63                     pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2)  );
64 }
65
66 void searchF0xyzabc (double &F, double &Fx, double &Fy, double &Fz, double &Fa, double &Fb, double &Fc, double aix, double aiy, double aiz, double bix_plus_x, double biy_plus_y, double biz_plus_z, double A, double B, double C) {
67     double t01, t02, t03, t04, t05, t06, t07;
68     t01 = pow(aix + biy_plus_y * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - biz_plus_z * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * bix_plus_x, 2);
69     t02 = pow(aiy - biy_plus_y * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + biz_plus_z * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * bix_plus_x, 2);
70     t03 = pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2);
71     t04 = sqrt(t01 + t02 + t03);
72     t05 = (aix + biy_plus_y * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - biz_plus_z * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * bix_plus_x);
73     t06 = (aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y);
74     t07 = (aiy - biy_plus_y * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + biz_plus_z * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * bix_plus_x);
75
76     //#pragma omp parallel sections
77     //{
78         //#pragma omp section
79             F += t04;
80         //#pragma omp section
81             Fx += -(2 * cos(B) * cos(C) * t05 - 2 * sin(B) * t06 + 2 * cos(B) * sin(C) * t07) / (2 * t04);
82         //#pragma omp section
83             Fy += -(2 * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) * t07 - 2 * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) * t05 + 2 * cos(B) * sin(A) * t06) / (2 * t04);
84         //#pragma omp section
85             Fz += -(2 * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) * t05 - 2 * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) * t07 + 2 * cos(A) * cos(B) * t06) / (2 * t04);
86         //#pragma omp section
87             Fa += -(2 * (cos(A) * cos(B) * biy_plus_y - cos(B) * sin(A) * biz_plus_z) * t06 -
88                 2 * (biy_plus_y * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) + biz_plus_z * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C))) * t07 +
89                 2 * (biy_plus_y * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) + biz_plus_z * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B))) * t05) / (2 * t04);
90         //#pragma omp section
91             Fb += -(2 * (cos(A) * cos(B) * sin(C) * biz_plus_z - sin(B) * sin(C) * bix_plus_x + cos(B) * sin(A) * sin(C) * biy_plus_y) * t07 +
92                 2 * (cos(A) * cos(B) * cos(C) * biz_plus_z - cos(C) * sin(B) * bix_plus_x + cos(B) * cos(C) * sin(A) * biy_plus_y) * t05 -
93                 2 * (cos(B) * bix_plus_x + sin(A) * sin(B) * biy_plus_y + cos(A) * sin(B) * biz_plus_z) * t06) / (2 * t04);
94         //#pragma omp section
95             Fc += (2 * (biy_plus_y * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) - biz_plus_z * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) + cos(B) * sin(C) * bix_plus_x) * t05 -
96                 2 * (biz_plus_z * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - biy_plus_y * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) + cos(B) * cos(C) * bix_plus_x) * t07) / (2 * t04);
97     //}
98 }
99
100 /*void searchL (double &fLd, double &fLdd, double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C, double L, double fx, double fy, double fz, double fa, double fb, double fc) {
101     double AmLfa, BmLfb, CmLfc, BIXpXmLfx, BIYpYmLfy, BIZpZmLfz, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t20;
102     AmLfa = A - L * fa;
103     BmLfb = B - L * fb;
104     CmLfc = C - L * fc;
105     BIXpXmLfx = bix + x - L * fx;
106     BIYpYmLfy = biy + y - L * fy;
107     BIZpZmLfz = biz + z - L * fz;
108     t1 = (cos(AmLfa) * cos(CmLfc) + sin(AmLfa) * sin(BmLfb) * sin(CmLfc));
109     t2 = (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc));
110     t3 = (cos(AmLfa) * sin(CmLfc) - cos(CmLfc) * sin(AmLfa) * sin(BmLfb));
111     t4 = (sin(AmLfa) * sin(CmLfc) + cos(AmLfa) * cos(CmLfc) * sin(BmLfb));
112     t5 = pow(aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy, 2);
113     t6 = pow(aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx, 2);
114     t7 = pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2);
115
116     t8 = fc * sin(AmLfa) * sin(CmLfc);
117     t9 = fa * cos(AmLfa) * cos(CmLfc);
118     t10 = fb * cos(AmLfa) * cos(BmLfb) * sin(CmLfc);
119     t11 = fc * cos(AmLfa) * cos(CmLfc) * sin(BmLfb);
120     t12 = fa * sin(AmLfa) * sin(BmLfb) * sin(CmLfc);
121
122     t13 = fa * cos(AmLfa) * sin(BmLfb) * sin(CmLfc);
123     t14 = fc * cos(AmLfa) * sin(CmLfc);
124     t15 = fa * cos(CmLfc) * sin(AmLfa);
125     t16 = fb * cos(BmLfb) * sin(AmLfa) * sin(CmLfc);
126     t17 = fc * cos(CmLfc) * sin(AmLfa) * sin(BmLfb);
127
128     t18 = fx * cos(BmLfb) * sin(CmLfc);
129     t19 = fc * cos(BmLfb) * cos(CmLfc);
130     t20 = fb * sin(BmLfb) * sin(CmLfc);
131
132     fLd +=  (2 * (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
133                 (   BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
134                 2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
135                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) -
136                     fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy + fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
137                 2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
138                 (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
139                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
140                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * t3 +
141                     fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) /
142             (2 * sqrt(  t5 + t6 + pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2)));
143
144
145     fLdd += (   (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
146                 (2 * fb * fx * cos(BmLfb) - pow(fb, 2) * sin(BmLfb) * BIXpXmLfx - 2 * fa * fy * cos(AmLfa) * cos(BmLfb) + 2 * fa * fz * cos(BmLfb) * sin(AmLfa) + 2 * fb * fz * cos(AmLfa) * sin(BmLfb) +
147                     2 * fb * fy * sin(AmLfa) * sin(BmLfb) + pow(fa, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz + pow(fb, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz +
148                     pow(fa, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + pow(fb, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + 2 * fa * fb * cos(AmLfa) * sin(BmLfb) * BIYpYmLfy -
149                     2 * fa * fb * sin(AmLfa) * sin(BmLfb) * BIZpZmLfz) +
150                 (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
151                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
152                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
153                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * (cos(AmLfa) * sin(CmLfc) -
154                     cos(CmLfc) * sin(AmLfa) * sin(BmLfb)) + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) -
155                     fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
156                 (2 * BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) +
157                     fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) -
158                     fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) + 2 * BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) +
159                     t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
160                     2 * fy * t3 + 2 * fz * t4 +
161                     2 * fx * cos(BmLfb) * cos(CmLfc) - 2 * fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - 2 * fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
162                 (2 * aix + 2 * t3 * BIYpYmLfy - 2 * (sin(AmLfa) * sin(CmLfc) +
163                     cos(AmLfa) * cos(CmLfc) * sin(BmLfb)) * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
164                 (BIZpZmLfz * (pow(fa, 2) * sin(AmLfa) * sin(CmLfc) +
165                     pow(fc, 2) * sin(AmLfa) * sin(CmLfc) - 2 * fa * fc * cos(AmLfa) * cos(CmLfc) + pow(fa, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) +
166                     pow(fb, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) + pow(fc, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) + 2 * fa * fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) +
167                     2 * fb * fc * cos(AmLfa) * cos(BmLfb) * sin(CmLfc) - 2 * fa * fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) + BIYpYmLfy * (pow(fa, 2) * cos(CmLfc) * sin(AmLfa) * sin(BmLfb) -
168                     pow(fc, 2) * cos(AmLfa) * sin(CmLfc) - 2 * fa * fc * cos(CmLfc) * sin(AmLfa) - pow(fa, 2) * cos(AmLfa) * sin(CmLfc) + pow(fb, 2) * cos(CmLfc) * sin(AmLfa) * sin(BmLfb) +
169                     pow(fc, 2) * cos(CmLfc) * sin(AmLfa) * sin(BmLfb) - 2 * fa * fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) + 2 * fa * fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) +
170                     2 * fb * fc * cos(BmLfb) * sin(AmLfa) * sin(CmLfc)) - 2 * fz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
171                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
172                     2 * fy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) -
173                     fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) + 2 * fb * fx * cos(CmLfc) * sin(BmLfb) + 2 * fc * t18 + pow(fb, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx +
174                     pow(fc, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx - 2 * fb * fc * sin(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
175                 (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) *
176                 (2 * BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + 2 * fy * t1 - 2 * fz * t2 + 2 * BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + 2 * t18 + 2 * t19 * BIXpXmLfx - 2 * t20 * BIXpXmLfx) +
177                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
178                     fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) *
179                 (2 * fx * sin(BmLfb) - 2 * fy * cos(BmLfb) * sin(AmLfa) + 2 * fb * cos(BmLfb) * BIXpXmLfx - 2 * fz * cos(AmLfa) * cos(BmLfb) - 2 * fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
180                     2 * fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + 2 * fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + 2 * fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
181                 (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
182                 (BIZpZmLfz * (pow(fa, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) -
183                     pow(fc, 2) * cos(CmLfc) * sin(AmLfa) - 2 * fa * t14 - pow(fa, 2) * cos(CmLfc) * sin(AmLfa) + pow(fb, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) +
184                     pow(fc, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) - 2 * fb * fc * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) + 2 * fa * t16 +
185                     2 * fa * t17) + BIYpYmLfy * (pow(fa, 2) * cos(AmLfa) * cos(CmLfc) + pow(fc, 2) * cos(AmLfa) * cos(CmLfc) - 2 * fa * t8 +
186                     pow(fa, 2) * sin(AmLfa) * sin(BmLfb) * sin(CmLfc) + pow(fb, 2) * sin(AmLfa) * sin(BmLfb) * sin(CmLfc) + pow(fc, 2) * sin(AmLfa) * sin(BmLfb) * sin(CmLfc) -
187                     2 * fa * t10 - 2 * fa * t11 - 2 * fb * t19 * sin(AmLfa)) - 2 * fz * (t8 - t9 + t10 + t11 - t12) - 2 * fy * (t13 - t14 - t15 + t16 + t17) -
188                     2 * fc * fx * cos(BmLfb) * cos(CmLfc) + 2 * fb * fx * sin(BmLfb) * sin(CmLfc) +
189                     pow(fb, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + pow(fc, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + 2 * fb * fc * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx)) /
190             (2 * sqrt(  t5 + t6 + t7)) -
191             (   (2 *    (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
192                         (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
193                         2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
194                         (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
195                             fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
196                         2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
197                         (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
198                         BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
199                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) *
200                 (   (2 * aix + 2 * t3 * BIYpYmLfy - 2 * t4 * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
201                     (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
202                         BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
203                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
204                     (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * t2 * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
205                     (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * (cos(AmLfa) * cos(CmLfc) + sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) +
206                         t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) - (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
207                     (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
208                         fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy))) /
209             (4 * pow    (   t5 + t6 + t7, 3 / 2));
210
211 }*/
212
213 void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
214     RVec temp;
215     #pragma omp parallel
216     {
217         #pragma omp for schedule(dynamic)
218         for (int i = 0; i < b.size(); i++) {
219             temp = b[i];
220             b[i][0] = (temp[2] + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - (temp[1] + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) + cos(B) * cos(C) * (temp[0] + x);
221             b[i][1] = (temp[1] + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) - (temp[2] + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) + cos(B) * sin(C) * (temp[0] + x);
222             b[i][2] = cos(A) * cos(B) * (temp[2] + z) - sin(B) * (temp[0] + x) + cos(B) * sin(A) * (temp[1] + y);
223         }
224     }
225 }
226
227 void CalcMid (std::vector< RVec > a, std::vector< RVec > b, RVec &midA, RVec &midB, std::vector< std::pair< int, int > > pairs) {
228     midA[0] = 0;
229     midA[1] = 0;
230     midA[2] = 0;
231
232     midB[0] = 0;
233     midB[1] = 0;
234     midB[2] = 0;
235
236     for (int i = 0; i < pairs.size(); i++) {
237         rvec_inc(midA, a[pairs[i].first]);
238         rvec_inc(midB, b[pairs[i].second]);
239     }
240     midA[0] /= pairs.size();
241     midA[1] /= pairs.size();
242     midA[2] /= pairs.size();
243
244     midB[0] /= pairs.size();
245     midB[1] /= pairs.size();
246     midB[2] /= pairs.size();
247 }
248
249 float FrameMidLength(std::vector< RVec > a) {
250     RVec b;
251     b[0] = 0;
252     b[1] = 0;
253     b[2] = 0;
254     for (int i = 0; i < a.size(); i++) {
255         b += a[i];
256     }
257     return b.norm();
258 }
259
260 void MyFitNew (std::vector< RVec > a, std::vector< RVec > &b, std::vector< std::pair< int, int > > pairs, double FtCnst) {
261     double f1 = 0, f2 = 0, fx = 0, fy = 0, fz = 0, fa = 0, fb = 0, fc = 0, l = 1;
262     RVec ma, mb;
263     CalcMid(a, b, ma, mb, pairs);
264     rvec_dec(ma, mb);
265     double x = ma[0], y = ma[1], z = ma[2], A = 0, B = 0, C = 0;
266     for (int i = 0; i < pairs.size(); i++) {
267         f1 += F(a[pairs[i].first][0], a[pairs[i].first][1], a[pairs[i].first][2], b[pairs[i].second][0] + x, b[pairs[i].second][1] + y, b[pairs[i].second][2] + z, A, B, C);
268     }
269     if (FtCnst == 0) {
270         FtCnst = 0.0001;
271     }
272     if (f1 == 0) {
273         return;
274     } else {
275         //int count = 0;
276         while (f1 - f2 < 0 || f1 - f2 > FtCnst) {
277             f1 = 0; fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
278             for (int i = 0; i < pairs.size(); i++) {
279                 searchF0xyzabc(f1, fx, fy, fz, fa, fb, fc, a[pairs[i].first][0], a[pairs[i].first][1], a[pairs[i].first][2], b[pairs[i].second][0] + x, b[pairs[i].second][1] + y, b[pairs[i].second][2] + z, A, B, C);
280             }
281             while (true) {
282                 f2 = 0;
283                 for (int i = 0; i < pairs.size(); i++) {
284                     f2 += F(a[pairs[i].first][0], a[pairs[i].first][1], a[pairs[i].first][2], b[pairs[i].second][0] + (x - l * fx), b[pairs[i].second][1] + (y - l * fy), b[pairs[i].second][2] + (z - l * fz), A - l * fa, B - l * fb, C - l * fc);
285                 }
286                 //count++;
287                 if (f2 < f1) {
288                     x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
289                     fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
290                     break;
291                 } else {
292                     l *= 0.99; // may be put somewhere l > 0
293                 }
294             }
295         }
296         //std::cout << " " << count << "\n";
297         ApplyFit(b, x, y, z, A, B, C);
298     }
299 }
300
301 void printPDBtraj(const char* Fname, std::vector< std::vector < RVec > > trj, std::vector< std::vector< std::pair< int, int > > > prs, std::vector< int > ndx) {
302     FILE* a = freopen (Fname, "w", stdout);
303     for (int i = 1; i < trj.size(); i++) {
304         fprintf(a, "TITLE     test t= %9.5f step= %5d\nMODEL %d\n", (float)(i + 1), i + 1, i + 1);
305         for (int j = 0; j < prs.size(); j++) {
306             for (int k = 0; k < prs[j].size(); k++) {
307                 printf("ATOM  %5d  CA   CA %5d    %8.3f%8.3f%8.3f%6.2f%6.2f      %4s%2s  \n", ndx[prs[j][k].first], ndx[prs[j][k].first], (trj[i][prs[j][k].first][0] * 10), (trj[i][prs[j][k].first][1] * 10), (trj[i][prs[j][k].first][2] * 10), 1.0, 20.0, "    ", " ");
308             }
309         }
310         fprintf(a, "TER\nENDMDL\n");
311     }
312     printf("END\n");
313     fclose(a);
314 }
315
316 bool myfunction (const std::vector< std::pair< int, int > > i, const std::vector< std::pair< int, int > > j) {
317     return i.size() < j.size();
318 }
319
320 void domain_reading(SelectionList sList, std::vector< int > ind, std::vector< std::vector < std::pair< int, int > > > &d_ind_num) {
321     d_ind_num.resize(sList.size() - 1);
322     for (int i = 1; i < sList.size(); i++) {
323         d_ind_num[i - 1].resize(0);
324         for (ArrayRef< const int >::iterator ai = sList[i].atomIndices().begin(); (ai < sList[i].atomIndices().end()); ai++) {
325             d_ind_num[i - 1].push_back(std::make_pair(*ai, 0));
326         }
327     }
328
329     for (int i = 0; i < d_ind_num.size(); i++) {
330         int k;
331         for (int j = 0; j < d_ind_num[i].size(); j++) {
332             k = 0;
333             while (ind[k] != d_ind_num[i][j].first) {
334                 k++;
335             }
336             d_ind_num[i][j].second = k;
337         }
338     }
339
340     // its a point of interest as we can either take biggest domains of the given set and work from those or we take them in given order
341     //std::sort(d_ind_num.begin(), d_ind_num.end(), myfunction);
342
343     for (int i = 0; i < d_ind_num.size(); i++) {
344         if (d_ind_num[i].size() >= 4) {
345             for (int k = 0; k < d_ind_num[i].size(); k++) {
346                 for (int j = i + 1; j < d_ind_num.size(); j++) {
347                     for (int x = 0; x < d_ind_num[j].size(); x++) {
348                         if (d_ind_num[j][x].first == d_ind_num[i][k].first) {
349                             d_ind_num[j].erase(d_ind_num[j].begin() + x);
350                             x--;
351                         }
352                     }
353                 }
354             }
355         }
356     }
357 }
358
359 void domain_expansion(std::vector< int > indx, std::vector< std::vector < std::pair< int, int > > > &d_ind_num, std::vector< RVec > frame) {
360     std::vector< bool > flag;
361     flag.resize(indx.size(), 1);
362     for (int i = 0; i < d_ind_num.size(); i++) {
363         for (int j = 0; j < d_ind_num[i].size(); j++) {
364             flag[d_ind_num[i][j].second] = 0;
365         }
366     }
367     float dist;
368     int x3 = -1;
369     for (int i = 0; i < indx.size(); i++) {
370         if (flag[i]) {
371             dist = 0;
372             for (int x1 = 0; x1 < d_ind_num.size(); x1++) {
373                 for (int x2 = 0; x2 < d_ind_num[x1].size(); x2++) {
374                     if (dist == 0) {
375                         dist = (frame[i] - frame[d_ind_num[0][0].first]).norm();
376                         x3 = 0;
377                     }
378                     if ((frame[i] - frame[d_ind_num[x1][x2].first]).norm() < dist) {
379                         x3 = x1;
380                         dist = (frame[i] - frame[d_ind_num[x1][x2].first]).norm();
381                     }
382                 }
383             }
384             d_ind_num[x3].push_back(std::make_pair(indx[i], i));
385         }
386     }
387 }
388
389 void MakeDomainFitPairs(std::vector< std::vector< std::pair< int, int > > > &p, std::vector< std::vector< std::pair< int, int > > > d) {
390     p.resize(d.size());
391     for (int i = 0; i < d.size(); i++) {
392         p[i].resize(0);
393         for (int j = 0; j < d[i].size(); j++) {
394             p[i].push_back(std::make_pair(d[i][j].second, d[i][j].second));
395         }
396     }
397 }
398
399 void TrajFitting(std::vector< std::vector < RVec > > &trj, double FC, std::vector< std::vector< std::pair< int, int > > > prs) {
400
401     // its needed to be thought through on whats shared and whats private
402     //#pragma omp parallel for shared(trj, prs, FC)/* private(clone)*/
403     for (int i = 1; i < trj.size()/*2*/; i++) {
404         std::vector< std::vector < RVec > > clone;
405         clone.resize(0);
406         clone.resize(prs.size(), trj[i]);
407         std::chrono::time_point<std::chrono::system_clock> now1, now2;
408         for (int j = 0; j < prs.size(); j++) {
409             now1 = std::chrono::system_clock::now();
410             MyFitNew(trj[0], clone[j], prs[j], FC);
411             now2 = std::chrono::system_clock::now();
412             std::cout << " " << std::chrono::duration_cast<std::chrono::milliseconds>(now2 - now1).count() << "\n";
413             for (int k = 0; k < prs[j].size(); k++) {
414                 trj[i][prs[j][k].first] = clone[j][prs[j][k].first];
415             }
416         }
417         clone.resize(0);
418         //std::cout << "frame " << i << " fitted\n";
419     }
420 }
421
422 class Fitng : public TrajectoryAnalysisModule
423 {
424     public:
425
426         Fitng();
427         virtual ~Fitng();
428
429         //! Set the options and setting
430         virtual void initOptions(IOptionsContainer          *options,
431                                  TrajectoryAnalysisSettings *settings);
432
433         //! First routine called by the analysis framework
434         // virtual void initAnalysis(const t_trxframe &fr, t_pbc *pbc);
435         virtual void initAnalysis(const TrajectoryAnalysisSettings &settings,
436                                   const TopologyInformation        &top);
437
438         virtual void initAfterFirstFrame(const TrajectoryAnalysisSettings   &settings,
439                                          const t_trxframe                   &fr);
440
441         //! Call for each frame of the trajectory
442         // virtual void analyzeFrame(const t_trxframe &fr, t_pbc *pbc);
443         virtual void analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
444                                   TrajectoryAnalysisModuleData *pdata);
445
446         //! Last routine called by the analysis framework
447         // virtual void finishAnalysis(t_pbc *pbc);
448         virtual void finishAnalysis(int nframes);
449
450         //! Routine to write output, that is additional over the built-in
451         virtual void writeOutput();
452
453     private:
454         SelectionList                                               sel_;
455
456         std::vector< int >                                          index;
457         const int                                                   *ind;
458         std::vector< std::vector< std::pair< int, int > > >         domains_index_number;
459         std::vector< std::vector < RVec > >                         trajectory;
460         std::vector< t_trxframe >                                   saved_traj;
461         //Selection                                                   selec;
462         std::vector< std::vector< std::pair< int, int > > >         pairs;
463         int                                                         frames = 0;
464         int                                                         method = 1;
465         double                                                      FitConst = 0;
466         std::string                                                 OutPutTrjName;
467         t_trxstatus                                                 *op;
468 };
469
470 Fitng::Fitng(): TrajectoryAnalysisModule()
471 {
472 }
473
474 Fitng::~Fitng()
475 {
476 }
477
478 void
479 Fitng::initOptions(IOptionsContainer          *options,
480                      TrajectoryAnalysisSettings *settings)
481 {
482     static const char *const desc[] = {
483         "[THISMODULE] to be done"
484     };
485     // Add the descriptive text (program help text) to the options
486     settings->setHelpText(desc);
487     // Add option for selecting a subset of atoms
488     //options->addOption(SelectionOption("select")
489     //                       .store(&selec).required()
490     //                       .description("Atoms that are considered as part of the excluded volume"));
491     // Add option for selection list
492     options->addOption(SelectionOption("select_residue_and_domains").storeVector(&sel_)
493                            .required().dynamicMask().multiValue()
494                            .description("Selected group and domains based on it"));
495     // Add option for Fit constant
496     options->addOption(DoubleOption("FitConst")
497                             .store(&FitConst)
498                             .description("Fitting untill diff <= FitConst, by default == 0.00001"));
499     // Add option for output pdb traj (meh edition)
500     options->addOption(StringOption("pdb_out")
501                             .store(&OutPutTrjName)
502                             .description("transformed trajectory"));
503     // Add option for trajectory Fit transformation
504     options->addOption(IntegerOption("method")
505                             .store(&method)
506                             .description("1 - fitting all -> all | 2 - keep only domains, all -> all | 3 - keep only domains, domain -> domain | 4 - extend domains to full coverage, domain -> domain"));
507     // Control input settings
508     settings->setFlags(TrajectoryAnalysisSettings::efNoUserPBC);
509     settings->setPBC(true);
510 }
511
512 // /home/toluk/Develop/samples/reca_test_ground/
513 // -s '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.xtc' -sf '/home/toluk/Develop/samples/reca_test_ground/SL0' -n '/home/toluk/Develop/samples/reca_test_ground/t0.ndx' -pdb_out '/home/toluk/Develop/samples/reca_test_ground/FullCAFit.pdb' -e 1000
514 // -s '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.xtc' -sf '/home/toluk/Develop/samples/reca_test_ground/SL2' -n '/home/toluk/Develop/samples/reca_test_ground/t2.ndx' -pdb_out '/home/toluk/Develop/samples/reca_test_ground/CAdomainsFit.pdb' -e 1000 -method 2
515 // -s '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.xtc' -sf '/home/toluk/Develop/samples/reca_test_ground/SL2' -n '/home/toluk/Develop/samples/reca_test_ground/t2.ndx' -pdb_out '/home/toluk/Develop/samples/reca_test_ground/CAdomainsOnlyFit.pdb' -e 1000 -method 3
516 // -s '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_test_ground/reca_rd.mono.xtc' -sf '/home/toluk/Develop/samples/reca_test_ground/SL2' -n '/home/toluk/Develop/samples/reca_test_ground/t2.ndx' -pdb_out '/home/toluk/Develop/samples/reca_test_ground/CAdomainsWExtraOnlyFit.pdb' -e 1000 -method 4
517
518 // -s '/home/titov_ai/Develop/samples/reca_rd/reca_rd.mono.tpr' -f '/home/titov_ai/Develop/samples/reca_rd/reca_rd.mono.xtc' -sf '/home/titov_ai/Develop/samples/reca_rd/SLCa' -n '/home/titov_ai/Develop/samples/reca_rd/CA.ndx' -pdb_out '/home/titov_ai/Develop/samples/reca_rd/test.xtc' -method 1
519
520 void
521 Fitng::initAnalysis(const TrajectoryAnalysisSettings &settings, const TopologyInformation &top)
522 {
523     index.resize(0);
524     for (ArrayRef< const int >::iterator ai = sel_[0].atomIndices().begin(); (ai < sel_[0].atomIndices().end()); ai++) {
525         index.push_back(*ai);
526     }
527
528     saved_traj.resize(0);
529
530     if (sel_.size() > 1 && method >= 2) {
531         domain_reading(sel_, index, domains_index_number);
532     }
533 }
534
535 void
536 Fitng::initAfterFirstFrame(const TrajectoryAnalysisSettings &settings, const t_trxframe &fr)
537 {
538     trajectory.resize(0);
539     std::vector < RVec > a;
540     a.resize(0);
541     trajectory.push_back(a);
542     for (int i = 0; i < index.size(); i++) {
543         trajectory[0].push_back(fr.x[index[i]]);
544     }
545     trajectory.push_back(a);
546     switch (method) {
547         case 1:
548             pairs.resize(1);
549             for (int i = 0; i < index.size(); i++) {
550                 pairs[0].push_back(std::make_pair(i, i));
551             }
552             break;
553         case 2:
554             pairs.resize(1);
555             for (int i = 0; i < domains_index_number.size(); i++) {
556                 for (int j = 0; j < domains_index_number[i].size(); j++) {
557                     pairs[0].push_back(std::make_pair(domains_index_number[i][j].second, domains_index_number[i][j].second));
558                 }
559             }
560             break;
561         case 3:
562             MakeDomainFitPairs(pairs, domains_index_number);
563             break;
564         case 4:
565             domain_expansion(index, domains_index_number, trajectory[0]);
566             MakeDomainFitPairs(pairs, domains_index_number);
567             break;
568     }
569
570     op = open_trx(OutPutTrjName.c_str(), "w+");
571 }
572
573 void
574 Fitng::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
575                       TrajectoryAnalysisModuleData *pdata)
576 {
577     trajectory[1].resize(0);
578     for (int i = 0; i < index.size(); i++) {
579         trajectory[1].push_back(fr.x[index[i]]);
580     }
581     TrajFitting(trajectory, FitConst, pairs);
582
583     t_trxframe c = fr;
584     for (int i = 0; i < index.size(); i++) {
585         copy_rvec(trajectory[1][i].as_vec(), c.x[index[i]]);
586     }
587     //trajectory.push_back(trajectory[1]);
588     if (write_trxframe_indexed(op, static_cast<const t_trxframe *>(&c), index.size(), static_cast<const int *>(&index.front())/*saved_traj[i].index*/, NULL) != 0) {
589         std::cout << "\nFileOutPut error\nframe number = " << frnr << "\n";
590     }
591 }
592
593
594 void
595 Fitng::finishAnalysis(int nframes)
596 {
597     std::cout << "\nGame Over\n";
598 }
599
600 void
601 Fitng::writeOutput()
602 {
603     //printPDBtraj((OutPutTrjName + ".pdb").c_str(), trajectory, pairs, index);
604 }
605
606
607 /*! \brief
608  * The main function for the analysis template.
609  */
610 int
611 main(int argc, char *argv[])
612 {
613     return gmx::TrajectoryAnalysisCommandLineRunner::runAsMain<Fitng>(argc, argv);
614 }