reworked on RVec

+ added fit func inside

diff --git a/src/domains.cpp b/src/domains.cpp
index f8efe64f944f508dfa1b1474104de1f071f51a21..0f1b0bd9ae0af62114964b6babf3d6ea2e4b80f0 100644 (file)
--- a/src/domains.cpp
+++ b/src/domains.cpp
@@ -49,14 +49,129 @@
 #include <gromacs/utility/smalloc.h>
 #include <gromacs/math/do_fit.h>
 
-#include "fittingn.cpp"
-
 #define MAX_NTRICVEC 12
 
 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 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 FtCnst) {
+    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 (FtCnst == 0) {
+        FtCnst = 0.00001;
+    }
+    if (f1 == 0) {
+        return;
+    } else {
+        int count = 0;
+        while (f1 - f2 < 0 || f1 - f2 > FtCnst) {
+            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++;
+        }
+        ApplyFit(b, x, y, z, A, B, C);
+    }
+}
+
 struct node {
     short int n;
     RVec r;
@@ -377,7 +492,7 @@ Domains::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
         //#pragma omp for schedule(dynamic)
         for (int j = 0; j < bone; j++) {
             std::vector < RVec > temp = trajectory[1];
-            MyFitNew(trajectory[0], temp, w_rls2[j]);
+            MyFitNew(trajectory[0], temp, w_rls2[j], 0);
             update_graph(graph[j], temp, epsi);
         }
     //}