b2b628e2eaab600524e4f43adfe4f9a2cdb5af3a
[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 #include <cstdio>
48
49 #include <omp.h>
50
51 #include <gromacs/trajectoryanalysis.h>
52 #include <gromacs/pbcutil/pbc.h>
53 #include <gromacs/utility/smalloc.h>
54 #include <gromacs/math/do_fit.h>
55 #include <gromacs/fileio/trxio.h>
56
57 using namespace gmx;
58
59 using gmx::RVec;
60
61 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) {
62     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) +
63                     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) +
64                     pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2)  );
65 }
66
67 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) {
68     double t01, t02, t03, t04, t05, t06, t07;
69     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);
70     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);
71     t03 = pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2);
72     t04 = sqrt(t01 + t02 + t03);
73     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);
74     t06 = (aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y);
75     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);
76
77     //#pragma omp parallel sections
78     //{
79         //#pragma omp section
80             F += t04;
81         //#pragma omp section
82             Fx += -(2 * cos(B) * cos(C) * t05 - 2 * sin(B) * t06 + 2 * cos(B) * sin(C) * t07) / (2 * t04);
83         //#pragma omp section
84             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);
85         //#pragma omp section
86             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);
87         //#pragma omp section
88             Fa += -(2 * (cos(A) * cos(B) * biy_plus_y - cos(B) * sin(A) * biz_plus_z) * t06 -
89                 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 +
90                 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);
91         //#pragma omp section
92             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 +
93                 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 -
94                 2 * (cos(B) * bix_plus_x + sin(A) * sin(B) * biy_plus_y + cos(A) * sin(B) * biz_plus_z) * t06) / (2 * t04);
95         //#pragma omp section
96             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 -
97                 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);
98     //}
99 }
100
101 /*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) {
102     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;
103     AmLfa = A - L * fa;
104     BmLfb = B - L * fb;
105     CmLfc = C - L * fc;
106     BIXpXmLfx = bix + x - L * fx;
107     BIYpYmLfy = biy + y - L * fy;
108     BIZpZmLfz = biz + z - L * fz;
109     t1 = (cos(AmLfa) * cos(CmLfc) + sin(AmLfa) * sin(BmLfb) * sin(CmLfc));
110     t2 = (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc));
111     t3 = (cos(AmLfa) * sin(CmLfc) - cos(CmLfc) * sin(AmLfa) * sin(BmLfb));
112     t4 = (sin(AmLfa) * sin(CmLfc) + cos(AmLfa) * cos(CmLfc) * sin(BmLfb));
113     t5 = pow(aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy, 2);
114     t6 = pow(aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx, 2);
115     t7 = pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2);
116
117     t8 = fc * sin(AmLfa) * sin(CmLfc);
118     t9 = fa * cos(AmLfa) * cos(CmLfc);
119     t10 = fb * cos(AmLfa) * cos(BmLfb) * sin(CmLfc);
120     t11 = fc * cos(AmLfa) * cos(CmLfc) * sin(BmLfb);
121     t12 = fa * sin(AmLfa) * sin(BmLfb) * sin(CmLfc);
122
123     t13 = fa * cos(AmLfa) * sin(BmLfb) * sin(CmLfc);
124     t14 = fc * cos(AmLfa) * sin(CmLfc);
125     t15 = fa * cos(CmLfc) * sin(AmLfa);
126     t16 = fb * cos(BmLfb) * sin(AmLfa) * sin(CmLfc);
127     t17 = fc * cos(CmLfc) * sin(AmLfa) * sin(BmLfb);
128
129     t18 = fx * cos(BmLfb) * sin(CmLfc);
130     t19 = fc * cos(BmLfb) * cos(CmLfc);
131     t20 = fb * sin(BmLfb) * sin(CmLfc);
132
133     fLd +=  (2 * (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
134                 (   BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
135                 2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
136                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) -
137                     fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy + fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
138                 2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
139                 (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)) +
140                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
141                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * t3 +
142                     fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) /
143             (2 * sqrt(  t5 + t6 + pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2)));
144
145
146     fLdd += (   (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
147                 (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) +
148                     2 * fb * fy * sin(AmLfa) * sin(BmLfb) + pow(fa, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz + pow(fb, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz +
149                     pow(fa, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + pow(fb, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + 2 * fa * fb * cos(AmLfa) * sin(BmLfb) * BIYpYmLfy -
150                     2 * fa * fb * sin(AmLfa) * sin(BmLfb) * BIZpZmLfz) +
151                 (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
152                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
153                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
154                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * (cos(AmLfa) * sin(CmLfc) -
155                     cos(CmLfc) * sin(AmLfa) * sin(BmLfb)) + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) -
156                     fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
157                 (2 * BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) +
158                     fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) -
159                     fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) + 2 * BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) +
160                     t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
161                     2 * fy * t3 + 2 * fz * t4 +
162                     2 * fx * cos(BmLfb) * cos(CmLfc) - 2 * fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - 2 * fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
163                 (2 * aix + 2 * t3 * BIYpYmLfy - 2 * (sin(AmLfa) * sin(CmLfc) +
164                     cos(AmLfa) * cos(CmLfc) * sin(BmLfb)) * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
165                 (BIZpZmLfz * (pow(fa, 2) * sin(AmLfa) * sin(CmLfc) +
166                     pow(fc, 2) * sin(AmLfa) * sin(CmLfc) - 2 * fa * fc * cos(AmLfa) * cos(CmLfc) + pow(fa, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) +
167                     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) +
168                     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) -
169                     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) +
170                     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) +
171                     2 * fb * fc * cos(BmLfb) * sin(AmLfa) * sin(CmLfc)) - 2 * fz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
172                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
173                     2 * fy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) -
174                     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 +
175                     pow(fc, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx - 2 * fb * fc * sin(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
176                 (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) *
177                 (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) +
178                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
179                     fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) *
180                 (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 +
181                     2 * fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + 2 * fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + 2 * fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
182                 (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
183                 (BIZpZmLfz * (pow(fa, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) -
184                     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) +
185                     pow(fc, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) - 2 * fb * fc * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) + 2 * fa * t16 +
186                     2 * fa * t17) + BIYpYmLfy * (pow(fa, 2) * cos(AmLfa) * cos(CmLfc) + pow(fc, 2) * cos(AmLfa) * cos(CmLfc) - 2 * fa * t8 +
187                     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) -
188                     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) -
189                     2 * fc * fx * cos(BmLfb) * cos(CmLfc) + 2 * fb * fx * sin(BmLfb) * sin(CmLfc) +
190                     pow(fb, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + pow(fc, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + 2 * fb * fc * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx)) /
191             (2 * sqrt(  t5 + t6 + t7)) -
192             (   (2 *    (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
193                         (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
194                         2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
195                         (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
196                             fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
197                         2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
198                         (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)) +
199                         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)) -
200                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) *
201                 (   (2 * aix + 2 * t3 * BIYpYmLfy - 2 * t4 * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
202                     (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)) +
203                         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)) -
204                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
205                     (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * t2 * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
206                     (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) +
207                         t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) - (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
208                     (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
209                         fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy))) /
210             (4 * pow    (   t5 + t6 + t7, 3 / 2));
211
212 }*/
213
214 void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
215     #pragma omp parallel
216     {
217         #pragma omp for schedule(dynamic)
218         for (int i = 0; i < b.size(); i++) {
219             RVec 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*/double 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 = fopen (Fname, "a");
303     for (int i = 1; i < trj.size(); i++) {
304         fprintf(a, "TITLE     test t= %9.5f step= %5d\nMODEL %d\n", (/*float*/double)(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                 fprintf(a, "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     //fprintf(a, "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*/double 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 // -s '/home/titov_ai/BioStore/titov_ai/trp_cage/kv1/trp-cage.pdb' -f '/home/titov_ai/BioStore/titov_ai/trp_cage/kv1/test.xtc' -sf '/home/titov_ai/BioStore/titov_ai/trp_cage/kv1/SLfull' -n '/home/titov_ai/BioStore/titov_ai/trp_cage/kv1/full.ndx' -pdb_out '/home/titov_ai/BioStore/titov_ai/trp_cage/kv1/testfit.xtc' -method 1
521
522 void
523 Fitng::initAnalysis(const TrajectoryAnalysisSettings &settings, const TopologyInformation &top)
524 {
525     index.resize(0);
526     for (ArrayRef< const int >::iterator ai = sel_[0].atomIndices().begin(); (ai < sel_[0].atomIndices().end()); ai++) {
527         index.push_back(*ai);
528     }
529
530     saved_traj.resize(0);
531
532     if (sel_.size() > 1 && method >= 2) {
533         domain_reading(sel_, index, domains_index_number);
534     }
535
536 }
537
538 void
539 Fitng::initAfterFirstFrame(const TrajectoryAnalysisSettings &settings, const t_trxframe &fr)
540 {
541     trajectory.resize(0);
542     std::vector < RVec > a;
543     a.resize(0);
544     trajectory.push_back(a);
545     for (int i = 0; i < index.size(); i++) {
546         trajectory[0].push_back(fr.x[index[i]]);
547     }
548     trajectory.push_back(a);
549     switch (method) {
550         case 1:
551             pairs.resize(1);
552             for (int i = 0; i < index.size(); i++) {
553                 pairs[0].push_back(std::make_pair(i, i));
554             }
555             break;
556         case 2:
557             pairs.resize(1);
558             for (int i = 0; i < domains_index_number.size(); i++) {
559                 for (int j = 0; j < domains_index_number[i].size(); j++) {
560                     pairs[0].push_back(std::make_pair(domains_index_number[i][j].second, domains_index_number[i][j].second));
561                 }
562             }
563             break;
564         case 3:
565             MakeDomainFitPairs(pairs, domains_index_number);
566             break;
567         case 4:
568             domain_expansion(index, domains_index_number, trajectory[0]);
569             MakeDomainFitPairs(pairs, domains_index_number);
570             break;
571     }
572
573     op = open_trx(OutPutTrjName.c_str(), "w+");
574 }
575
576 void
577 Fitng::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
578                       TrajectoryAnalysisModuleData *pdata)
579 {
580     trajectory[1].resize(0);
581     for (int i = 0; i < index.size(); i++) {
582         trajectory[1].push_back(fr.x[index[i]]);
583     }
584     TrajFitting(trajectory, FitConst, pairs);
585
586     t_trxframe c = fr;
587     for (int i = 0; i < index.size(); i++) {
588         copy_rvec(trajectory[1][i].as_vec(), c.x[index[i]]);
589     }
590     //trajectory.push_back(trajectory[1]);
591     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) {
592         std::cout << "\nFileOutPut error\nframe number = " << frnr << "\n";
593     }
594
595     printPDBtraj((OutPutTrjName + ".pdb").c_str(), trajectory, pairs, index);
596 }
597
598
599 void
600 Fitng::finishAnalysis(int nframes)
601 {
602     std::cout << "\nGame Over\n";
603 }
604
605 void
606 Fitng::writeOutput()
607 {
608     //printPDBtraj((OutPutTrjName + ".pdb").c_str(), trajectory, pairs, index);
609 }
610
611
612 /*! \brief
613  * The main function for the analysis template.
614  */
615 int
616 main(int argc, char *argv[])
617 {
618     return gmx::TrajectoryAnalysisCommandLineRunner::runAsMain<Fitng>(argc, argv);
619 }