added fitting lib (kinda)

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c72f456a43095ef9367670cf8ba8df32e4bda32d..956f8e06e67dd019b4686f9ece135fc91caf0f61 100644 (file)
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,5 +1,6 @@
 set(GROMACS_CXX_FLAGS "${GROMACS_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 add_executable(fitng  fitng.cpp)
+add_library(fittingn STATIC fittingn.h fittingn.cpp)
 include_directories(
         ${GROMACS_INCLUDE_DIRS}
         ${CMAKE_SOURCE_DIR}/include)

diff --git a/src/fitng.cpp b/src/fitng.cpp
index 1a04511dbe5df56ea5d97e82b577f42d1738e6d2..5ddd12b7630301efa0fdfdaee07d261d283ba114 100644 (file)
--- a/src/fitng.cpp
+++ b/src/fitng.cpp
@@ -43,124 +43,18 @@
 #include <iostream>
 #include <chrono>
 #include <omp.h>
-#include <thread>
-#include <string>
-#include <math.h>
-#include <algorithm>
 
 #include <gromacs/trajectoryanalysis.h>
 #include <gromacs/pbcutil/pbc.h>
-#include <gromacs/pbcutil/rmpbc.h>
 #include <gromacs/utility/smalloc.h>
-#include <gromacs/math/vectypes.h>
-#include <gromacs/math/vec.h>
 #include <gromacs/math/do_fit.h>
 
+#include "fittingn.h"
+
 using namespace gmx;
 
 using gmx::RVec;
 
-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) {
-    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) +
-                    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) +
-                    pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2)  );
-}
-
-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) {
-    double t01, t02, t03, t04, t05, t06, t07;
-    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);
-    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);
-    t03 = pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2);
-    t04 = sqrt(t01 + t02 + t03);
-    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);
-    t06 = (aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y);
-    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);
-
-    F += t04;
-    Fx += -(2 * cos(B) * cos(C) * t05 - 2 * sin(B) * t06 + 2 * cos(B) * sin(C) * t07) / (2 * t04);
-    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);
-    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);
-    Fa += -(2 * (cos(A) * cos(B) * biy_plus_y - cos(B) * sin(A) * biz_plus_z) * t06 -
-           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 +
-           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);
-    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 +
-           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 -
-           2 * (cos(B) * bix_plus_x + sin(A) * sin(B) * biy_plus_y + cos(A) * sin(B) * biz_plus_z) * t06) / (2 * t04);
-    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 -
-          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);
-}
-
-void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
-    RVec temp;
-    for (int i = 0; i < b.size(); i++) {
-        temp = b[i];
-        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);
-        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);
-        b[i][2] = cos(A)*cos(B)*(temp[2] + z) - sin(B)*(temp[0] + x) + cos(B)*sin(A)*(temp[1] + y);
-    }
-}
-
-void CalcMid (std::vector< RVec > a, std::vector< RVec > b, RVec &midA, RVec &midB, std::vector< std::pair< int, int > > pairs) {
-    midA[0] = 0;
-    midA[1] = 0;
-    midA[2] = 0;
-
-    midB[0] = 0;
-    midB[1] = 0;
-    midB[2] = 0;
-
-    for (int i = 0; i < pairs.size(); i++) {
-        rvec_inc(midA, a[pairs[i].first]);
-        rvec_inc(midB, b[pairs[i].second]);
-    }
-    midA[0] /= pairs.size();
-    midA[1] /= pairs.size();
-    midA[2] /= pairs.size();
-
-    midB[0] /= pairs.size();
-    midB[1] /= pairs.size();
-    midB[2] /= pairs.size();
-}
-
-void MyFitNew (std::vector< RVec > a, std::vector< RVec > &b, std::vector< std::pair< int, int > > pairs) {
-    double f1 = 0, f2 = 0, fx = 0, fy = 0, fz = 0, fa = 0, fb = 0, fc = 0, l = 1;
-    RVec ma, mb;
-    CalcMid(a, b, ma, mb, pairs);
-    rvec_dec(ma, mb);
-    double x = ma[0], y = ma[1], z = ma[2], A = 0, B = 0, C = 0;
-    for (int i = 0; i < pairs.size(); i++) {
-        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);
-    }
-    if (f1 == 0) {
-        return;
-    } else {
-    int count = 0;
-        while (f1 - f2 < 0 || f1 - f2 > 0.00001) {
-            f1 = 0; fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
-            for (int i = 0; i < pairs.size(); i++) {
-                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);
-            }
-            while (true) {
-                f2 = 0;
-                for (int i = 0; i < pairs.size(); i++) {
-                    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);
-                }
-                count++;
-                if (f2 < f1) {
-                    x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
-                    fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
-                    break;
-                } else {
-                    l *= 0.85;
-                }
-            }
-            count++;
-       }
-    std::cout << "iteration count: " << count << "\n";
-    ApplyFit(b, x, y, z, A, B, C);
-    }
-}
-
 class Fitng : public TrajectoryAnalysisModule
 {
     public:
@@ -325,11 +219,13 @@ Fitng::finishAnalysis(int nframes)
         }
     }
 
-    //std::chrono::time_point<std::chrono::system_clock> start, end;
+    std::chrono::time_point<std::chrono::system_clock> start, end;
     for (int j = 1; j < trajectory.size(); j++) {
 
         for (int i = 0; i < index.size(); i++) {
-            dist1 += F(trajectory[0][i][0], trajectory[0][i][1], trajectory[0][i][2], trajectory[j][i][0] + 0, trajectory[j][i][1] + 0, trajectory[j][i][2] + 0, 0, 0, 0);
+            dist1 += ftn::F(    static_cast< double >(trajectory[0][i][0]), static_cast< double >(trajectory[0][i][1]), static_cast< double >(trajectory[0][i][2]),
+                                static_cast< double >(trajectory[j][i][0] + 0), static_cast< double >(trajectory[j][i][1] + 0), static_cast< double >(trajectory[j][i][2] + 0),
+                                static_cast< double >(0), static_cast< double >(0), static_cast< double >(0)    );
         }
         std::cout << "\nbasic dist = " << dist1 /*/ index.size() */<< "\n";
 
@@ -337,12 +233,14 @@ Fitng::finishAnalysis(int nframes)
         //  My Fit
         //
 
-        //start = std::chrono::system_clock::now();
-        MyFitNew(trajectory[0], trajectory[j], pairs);
-        //end = std::chrono::system_clock::now();
-        //std::cout << std::chrono::duration_cast<std::chrono::microseconds> (end-start).count() << " ";
+        start = std::chrono::system_clock::now();
+        ftn::MyFitNew(trajectory[0], trajectory[j], pairs);
+        end = std::chrono::system_clock::now();
+        std::cout << std::chrono::duration_cast<std::chrono::microseconds> (end-start).count() << " ";
         for (int i = 0; i < index.size(); i++) {
-            dist2 += F(trajectory[0][i][0], trajectory[0][i][1], trajectory[0][i][2], trajectory[j][i][0] + 0, trajectory[j][i][1] + 0, trajectory[j][i][2] + 0, 0, 0, 0);
+            dist2 += ftn::F(    static_cast< double >(trajectory[0][i][0]), static_cast< double >(trajectory[0][i][1]), static_cast< double >(trajectory[0][i][2]),
+                                static_cast< double >(trajectory[j][i][0] + 0), static_cast< double >(trajectory[j][i][1] + 0), static_cast< double >(trajectory[j][i][2] + 0),
+                                static_cast< double >(0), static_cast< double >(0), static_cast< double >(0));
         }
         std::cout << "my fit dist = " << dist2 /*/ index.size()*/<< "\n";
 
@@ -357,7 +255,9 @@ Fitng::finishAnalysis(int nframes)
         //end = std::chrono::system_clock::now();
         //std::cout << std::chrono::duration_cast<std::chrono::microseconds> (end-start).count() << "\n";
         for (int i = 0; i < index.size(); i++) {
-            dist2 += F(traj1[i][0], traj1[i][1], traj1[i][2], traj2[j][i][0] + 0, traj2[j][i][1] + 0, traj2[j][i][2] + 0, 0, 0, 0);
+            dist2 += ftn::F(    static_cast< double >(traj1[i][0]), static_cast< double >(traj1[i][1]), static_cast< double >(traj1[i][2]),
+                                static_cast< double >(traj2[j][i][0] + 0), static_cast< double >(traj2[j][i][1] + 0), static_cast< double >(traj2[j][i][2] + 0),
+                                static_cast< double >(0), static_cast< double >(0), static_cast< double >(0));
         }
         std::cout << "old fit dist = " << dist2 /*/ index.size()*/<< "\n";
 

diff --git a/src/fittingn.cpp b/src/fittingn.cpp
new file mode 100644 (file)
index 0000000..914710b
--- /dev/null
+++ b/src/fittingn.cpp
@@ -0,0 +1,247 @@
+#include <gromacs/trajectoryanalysis.h>
+
+#include <vector>
+#include <math.h>
+
+using namespace gmx;
+using gmx::RVec;
+
+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) {
+    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) +
+                    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) +
+                    pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2)  );
+}
+
+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) {
+    double t01, t02, t03, t04, t05, t06, t07;
+    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);
+    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);
+    t03 = pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2);
+    t04 = sqrt(t01 + t02 + t03);
+    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);
+    t06 = (aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y);
+    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);
+
+    //#pragma omp parallel sections
+    //{
+        //#pragma omp section
+            F += t04;
+        //#pragma omp section
+            Fx += -(2 * cos(B) * cos(C) * t05 - 2 * sin(B) * t06 + 2 * cos(B) * sin(C) * t07) / (2 * t04);
+        //#pragma omp section
+            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);
+        //#pragma omp section
+            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);
+        //#pragma omp section
+            Fa += -(2 * (cos(A) * cos(B) * biy_plus_y - cos(B) * sin(A) * biz_plus_z) * t06 -
+                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 +
+                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);
+        //#pragma omp section
+            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 +
+                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 -
+                2 * (cos(B) * bix_plus_x + sin(A) * sin(B) * biy_plus_y + cos(A) * sin(B) * biz_plus_z) * t06) / (2 * t04);
+        //#pragma omp section
+            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 -
+                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);
+    //}
+}
+
+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) {
+    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;
+    AmLfa = A - L * fa;
+    BmLfb = B - L * fb;
+    CmLfc = C - L * fc;
+    BIXpXmLfx = bix + x - L * fx;
+    BIYpYmLfy = biy + y - L * fy;
+    BIZpZmLfz = biz + z - L * fz;
+    t1 = (cos(AmLfa) * cos(CmLfc) + sin(AmLfa) * sin(BmLfb) * sin(CmLfc));
+    t2 = (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc));
+    t3 = (cos(AmLfa) * sin(CmLfc) - cos(CmLfc) * sin(AmLfa) * sin(BmLfb));
+    t4 = (sin(AmLfa) * sin(CmLfc) + cos(AmLfa) * cos(CmLfc) * sin(BmLfb));
+    t5 = pow(aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy, 2);
+    t6 = pow(aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx, 2);
+    t7 = pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2);
+
+    t8 = fc * sin(AmLfa) * sin(CmLfc);
+    t9 = fa * cos(AmLfa) * cos(CmLfc);
+    t10 = fb * cos(AmLfa) * cos(BmLfb) * sin(CmLfc);
+    t11 = fc * cos(AmLfa) * cos(CmLfc) * sin(BmLfb);
+    t12 = fa * sin(AmLfa) * sin(BmLfb) * sin(CmLfc);
+
+    t13 = fa * cos(AmLfa) * sin(BmLfb) * sin(CmLfc);
+    t14 = fc * cos(AmLfa) * sin(CmLfc);
+    t15 = fa * cos(CmLfc) * sin(AmLfa);
+    t16 = fb * cos(BmLfb) * sin(AmLfa) * sin(CmLfc);
+    t17 = fc * cos(CmLfc) * sin(AmLfa) * sin(BmLfb);
+
+    t18 = fx * cos(BmLfb) * sin(CmLfc);
+    t19 = fc * cos(BmLfb) * cos(CmLfc);
+    t20 = fb * sin(BmLfb) * sin(CmLfc);
+
+    fLd +=  (2 * (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
+                (   BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
+                2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
+                (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) -
+                    fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy + fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
+                2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
+                (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)) +
+                    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)) - fy * t3 +
+                    fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) /
+            (2 * sqrt(  t5 + t6 + pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2)));
+
+
+    fLdd += (   (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
+                (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) +
+                    2 * fb * fy * sin(AmLfa) * sin(BmLfb) + pow(fa, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz + pow(fb, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz +
+                    pow(fa, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + pow(fb, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + 2 * fa * fb * cos(AmLfa) * sin(BmLfb) * BIYpYmLfy -
+                    2 * fa * fb * sin(AmLfa) * sin(BmLfb) * BIZpZmLfz) +
+                (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)) +
+                    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)) - fy * (cos(AmLfa) * sin(CmLfc) -
+                    cos(CmLfc) * sin(AmLfa) * sin(BmLfb)) + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) -
+                    fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
+                (2 * 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)) + 2 * 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)) -
+                    2 * fy * t3 + 2 * fz * t4 +
+                    2 * fx * cos(BmLfb) * cos(CmLfc) - 2 * fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - 2 * fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
+                (2 * aix + 2 * t3 * BIYpYmLfy - 2 * (sin(AmLfa) * sin(CmLfc) +
+                    cos(AmLfa) * cos(CmLfc) * sin(BmLfb)) * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
+                (BIZpZmLfz * (pow(fa, 2) * sin(AmLfa) * sin(CmLfc) +
+                    pow(fc, 2) * sin(AmLfa) * sin(CmLfc) - 2 * fa * fc * cos(AmLfa) * cos(CmLfc) + pow(fa, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) +
+                    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) +
+                    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) -
+                    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) +
+                    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) +
+                    2 * fb * fc * cos(BmLfb) * sin(AmLfa) * sin(CmLfc)) - 2 * fz * (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)) -
+                    2 * fy * (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)) + 2 * fb * fx * cos(CmLfc) * sin(BmLfb) + 2 * fc * t18 + pow(fb, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx +
+                    pow(fc, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx - 2 * fb * fc * sin(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
+                (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) *
+                (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) +
+                (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
+                    fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) *
+                (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 +
+                    2 * fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + 2 * fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + 2 * fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
+                (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
+                (BIZpZmLfz * (pow(fa, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) -
+                    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) +
+                    pow(fc, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) - 2 * fb * fc * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) + 2 * fa * t16 +
+                    2 * fa * t17) + BIYpYmLfy * (pow(fa, 2) * cos(AmLfa) * cos(CmLfc) + pow(fc, 2) * cos(AmLfa) * cos(CmLfc) - 2 * fa * t8 +
+                    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) -
+                    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) -
+                    2 * fc * fx * cos(BmLfb) * cos(CmLfc) + 2 * fb * fx * sin(BmLfb) * sin(CmLfc) +
+                    pow(fb, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + pow(fc, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + 2 * fb * fc * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx)) /
+            (2 * sqrt(  t5 + t6 + t7)) -
+            (   (2 *    (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
+                        (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
+                        2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
+                        (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
+                            fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
+                        2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
+                        (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)) +
+                        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)) -
+                        fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) *
+                (   (2 * aix + 2 * t3 * BIYpYmLfy - 2 * t4 * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
+                    (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)) +
+                        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)) -
+                        fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
+                    (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * t2 * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
+                    (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) +
+                        t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) - (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
+                    (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
+                        fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy))) /
+            (4 * pow    (   t5 + t6 + t7, 3 / 2));
+
+}
+
+
+void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
+    RVec temp;
+    //#pragma omp parallel
+    //{
+        //#pragma omp for schedule(dynamic)
+        for (int i = 0; i < b.size(); i++) {
+            temp = b[i];
+            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);
+            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);
+            b[i][2] = cos(A) * cos(B) * (temp[2] + z) - sin(B) * (temp[0] + x) + cos(B) * sin(A) * (temp[1] + y);
+        }
+    //}
+}
+
+void CalcMid (std::vector< RVec > a, std::vector< RVec > b, RVec &midA, RVec &midB, std::vector< std::pair< int, int > > pairs) {
+    midA[0] = 0;
+    midA[1] = 0;
+    midA[2] = 0;
+
+    midB[0] = 0;
+    midB[1] = 0;
+    midB[2] = 0;
+
+    for (int i = 0; i < pairs.size(); i++) {
+        rvec_inc(midA, a[pairs[i].first]);
+        rvec_inc(midB, b[pairs[i].second]);
+    }
+    midA[0] /= pairs.size();
+    midA[1] /= pairs.size();
+    midA[2] /= pairs.size();
+
+    midB[0] /= pairs.size();
+    midB[1] /= pairs.size();
+    midB[2] /= pairs.size();
+}
+
+void MyFitNew (std::vector< RVec > a, std::vector< RVec > &b, std::vector< std::pair< int, int > > pairs) {
+    double f1 = 0, f2 = 0, fx = 0, fy = 0, fz = 0, fa = 0, fb = 0, fc = 0, l = 1;
+    RVec ma, mb;
+    CalcMid(a, b, ma, mb, pairs);
+    rvec_dec(ma, mb);
+    double x = ma[0], y = ma[1], z = ma[2], A = 0, B = 0, C = 0;
+    for (int i = 0; i < pairs.size(); i++) {
+        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);
+    }
+    if (f1 == 0) {
+        return;
+    } else {
+        double fLd, fLdd;
+        //l = 0;
+        int count = 0;
+            while (f1 - f2 < 0 || f1 - f2 > 0.00001) {
+                f1 = 0; fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
+                for (int i = 0; i < pairs.size(); i++) {
+                    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);
+                }
+                /*do {
+                    fLd = 0;
+                    fLdd = 0;
+                    for (int i = 0; i < pairs.size(); i++) {
+                        searchL(fLd, fLdd, a[pairs[i].first][0], a[pairs[i].first][1], a[pairs[i].first][2], b[pairs[i].second][0], b[pairs[i].second][1], b[pairs[i].second][2], x, y, z, A, B, C, l, fx, fy, fz, fa, fb, fc);
+                    }
+                    l -= fLd / fLdd;
+                } while (std::abs(fLd) > 1);*/
+                //std::cout << " Ltempo= " << Ltempo << " iter= " << Lco << "\n";
+                while (true) {
+                    f2 = 0;
+                    for (int i = 0; i < pairs.size(); i++) {
+                        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);
+                    }
+                    count++;
+                    if (f2 < f1) {
+                        x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
+                        fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
+                        break;
+                    } else {
+                        l *= 0.85;
+                    }
+                }
+                count++;
+            }
+        //std::cout << "iteration count: " << count << "\n";
+        ApplyFit(b, x, y, z, A, B, C);
+    }
+}

diff --git a/src/fittingn.h b/src/fittingn.h
new file mode 100644 (file)
index 0000000..29e9b32
--- /dev/null
+++ b/src/fittingn.h
@@ -0,0 +1,33 @@
+#ifndef FITTINGN_H
+#define FITTINGN_H
+
+// где-то косяк в Смаке, без этой строчки undefined reference to
+
+#include <type_traits>
+#include <iostream>
+#include <string>
+
+using namespace gmx;
+using gmx::RVec;
+
+
+//template< class t1, class = typename std::enable_if<std::is_floating_point< t1 >::value >::type >
+//double F (t1 aix, t1 aiy, t1 aiz, t1 bix_plus_x, t1 biy_plus_y, t1 biz_plus_z, t1 A, t1 B, t1 C);
+
+namespace ftn {
+
+    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);
+
+    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);
+
+    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);
+
+    void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C);
+
+    void CalcMid (std::vector< RVec > a, std::vector< RVec > b, RVec &midA, RVec &midB, std::vector< std::pair< int, int > > pairs);
+
+    void MyFitNew (std::vector< RVec > a, std::vector< RVec > &b, std::vector< std::pair< int, int > > pairs);
+
+}
+
+#endif // FITTINGN_H