src/fittingn.cpp

   1 #include <gromacs/trajectoryanalysis.h>
   2
   3 #include <vector>
   4 #include <math.h>
   5
   6 using namespace gmx;
   7 using gmx::RVec;
   8
   9 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) {
  10     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) +
  11                     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) +
  12                     pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2)  );
  13 }
  14
  15 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) {
  16     double t01, t02, t03, t04, t05, t06, t07;
  17     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);
  18     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);
  19     t03 = pow(aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y, 2);
  20     t04 = sqrt(t01 + t02 + t03);
  21     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);
  22     t06 = (aiz + sin(B) * bix_plus_x - cos(A) * cos(B) * biz_plus_z - cos(B) * sin(A) * biy_plus_y);
  23     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);
  24
  25     //#pragma omp parallel sections
  26     //{
  27         //#pragma omp section
  28             F += t04;
  29         //#pragma omp section
  30             Fx += -(2 * cos(B) * cos(C) * t05 - 2 * sin(B) * t06 + 2 * cos(B) * sin(C) * t07) / (2 * t04);
  31         //#pragma omp section
  32             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);
  33         //#pragma omp section
  34             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);
  35         //#pragma omp section
  36             Fa += -(2 * (cos(A) * cos(B) * biy_plus_y - cos(B) * sin(A) * biz_plus_z) * t06 -
  37                 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 +
  38                 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);
  39         //#pragma omp section
  40             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 +
  41                 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 -
  42                 2 * (cos(B) * bix_plus_x + sin(A) * sin(B) * biy_plus_y + cos(A) * sin(B) * biz_plus_z) * t06) / (2 * t04);
  43         //#pragma omp section
  44             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 -
  45                 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);
  46     //}
  47 }
  48
  49 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) {
  50     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;
  51     AmLfa = A - L * fa;
  52     BmLfb = B - L * fb;
  53     CmLfc = C - L * fc;
  54     BIXpXmLfx = bix + x - L * fx;
  55     BIYpYmLfy = biy + y - L * fy;
  56     BIZpZmLfz = biz + z - L * fz;
  57     t1 = (cos(AmLfa) * cos(CmLfc) + sin(AmLfa) * sin(BmLfb) * sin(CmLfc));
  58     t2 = (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc));
  59     t3 = (cos(AmLfa) * sin(CmLfc) - cos(CmLfc) * sin(AmLfa) * sin(BmLfb));
  60     t4 = (sin(AmLfa) * sin(CmLfc) + cos(AmLfa) * cos(CmLfc) * sin(BmLfb));
  61     t5 = pow(aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy, 2);
  62     t6 = pow(aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx, 2);
  63     t7 = pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2);
  64
  65     t8 = fc * sin(AmLfa) * sin(CmLfc);
  66     t9 = fa * cos(AmLfa) * cos(CmLfc);
  67     t10 = fb * cos(AmLfa) * cos(BmLfb) * sin(CmLfc);
  68     t11 = fc * cos(AmLfa) * cos(CmLfc) * sin(BmLfb);
  69     t12 = fa * sin(AmLfa) * sin(BmLfb) * sin(CmLfc);
  70
  71     t13 = fa * cos(AmLfa) * sin(BmLfb) * sin(CmLfc);
  72     t14 = fc * cos(AmLfa) * sin(CmLfc);
  73     t15 = fa * cos(CmLfc) * sin(AmLfa);
  74     t16 = fb * cos(BmLfb) * sin(AmLfa) * sin(CmLfc);
  75     t17 = fc * cos(CmLfc) * sin(AmLfa) * sin(BmLfb);
  76
  77     t18 = fx * cos(BmLfb) * sin(CmLfc);
  78     t19 = fc * cos(BmLfb) * cos(CmLfc);
  79     t20 = fb * sin(BmLfb) * sin(CmLfc);
  80
  81     fLd +=  (2 * (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
  82                 (   BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
  83                 2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
  84                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) -
  85                     fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy + fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
  86                 2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
  87                 (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)) +
  88                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
  89                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * t3 +
  90                     fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) /
  91             (2 * sqrt(  t5 + t6 + pow(aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx, 2)));
  92
  93
  94     fLdd += (   (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
  95                 (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) +
  96                     2 * fb * fy * sin(AmLfa) * sin(BmLfb) + pow(fa, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz + pow(fb, 2) * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz +
  97                     pow(fa, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + pow(fb, 2) * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy + 2 * fa * fb * cos(AmLfa) * sin(BmLfb) * BIYpYmLfy -
  98                     2 * fa * fb * sin(AmLfa) * sin(BmLfb) * BIZpZmLfz) +
  99                 (BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
 100                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) +
 101                     BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) +
 102                     fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) - fy * (cos(AmLfa) * sin(CmLfc) -
 103                     cos(CmLfc) * sin(AmLfa) * sin(BmLfb)) + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) -
 104                     fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
 105                 (2 * BIZpZmLfz * (fa * cos(AmLfa) * sin(CmLfc) +
 106                     fc * cos(CmLfc) * sin(AmLfa) + fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) -
 107                     fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) + 2 * BIYpYmLfy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) +
 108                     t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) - fc * sin(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
 109                     2 * fy * t3 + 2 * fz * t4 +
 110                     2 * fx * cos(BmLfb) * cos(CmLfc) - 2 * fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - 2 * fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
 111                 (2 * aix + 2 * t3 * BIYpYmLfy - 2 * (sin(AmLfa) * sin(CmLfc) +
 112                     cos(AmLfa) * cos(CmLfc) * sin(BmLfb)) * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
 113                 (BIZpZmLfz * (pow(fa, 2) * sin(AmLfa) * sin(CmLfc) +
 114                     pow(fc, 2) * sin(AmLfa) * sin(CmLfc) - 2 * fa * fc * cos(AmLfa) * cos(CmLfc) + pow(fa, 2) * cos(AmLfa) * cos(CmLfc) * sin(BmLfb) +
 115                     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) +
 116                     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) -
 117                     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) +
 118                     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) +
 119                     2 * fb * fc * cos(BmLfb) * sin(AmLfa) * sin(CmLfc)) - 2 * fz * (fa * cos(AmLfa) * sin(CmLfc) + fc * cos(CmLfc) * sin(AmLfa) +
 120                     fb * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) - t15 * sin(BmLfb) - fc * cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) -
 121                     2 * fy * (fa * sin(AmLfa) * sin(CmLfc) - fc * cos(AmLfa) * cos(CmLfc) + t9 * sin(BmLfb) + fb * cos(BmLfb) * cos(CmLfc) * sin(AmLfa) -
 122                     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 +
 123                     pow(fc, 2) * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx - 2 * fb * fc * sin(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
 124                 (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) *
 125                 (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) +
 126                 (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
 127                     fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) *
 128                 (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 +
 129                     2 * fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + 2 * fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + 2 * fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
 130                 (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * (cos(CmLfc) * sin(AmLfa) - cos(AmLfa) * sin(BmLfb) * sin(CmLfc)) * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
 131                 (BIZpZmLfz * (pow(fa, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) -
 132                     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) +
 133                     pow(fc, 2) * cos(AmLfa) * sin(BmLfb) * sin(CmLfc) - 2 * fb * fc * cos(AmLfa) * cos(BmLfb) * cos(CmLfc) + 2 * fa * t16 +
 134                     2 * fa * t17) + BIYpYmLfy * (pow(fa, 2) * cos(AmLfa) * cos(CmLfc) + pow(fc, 2) * cos(AmLfa) * cos(CmLfc) - 2 * fa * t8 +
 135                     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) -
 136                     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) -
 137                     2 * fc * fx * cos(BmLfb) * cos(CmLfc) + 2 * fb * fx * sin(BmLfb) * sin(CmLfc) +
 138                     pow(fb, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + pow(fc, 2) * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx + 2 * fb * fc * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx)) /
 139             (2 * sqrt(  t5 + t6 + t7)) -
 140             (   (2 *    (aiy - t1 * BIYpYmLfy + t2 * BIZpZmLfz - cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
 141                         (BIZpZmLfz * (t8 - t9 + t10 + t11 - t12) + fy * t1 - fz * t2 + BIYpYmLfy * (t13 - t14 - t15 + t16 + t17) + t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) -
 142                         2 * (aiz + sin(BmLfb) * BIXpXmLfx - cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
 143                         (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
 144                             fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy) +
 145                         2 * (aix + t3 * BIYpYmLfy - t4 * BIZpZmLfz - cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
 146                         (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)) +
 147                         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)) -
 148                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx)) *
 149                 (   (2 * aix + 2 * t3 * BIYpYmLfy - 2 * t4 * BIZpZmLfz - 2 * cos(BmLfb) * cos(CmLfc) * BIXpXmLfx) *
 150                     (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)) +
 151                         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)) -
 152                         fy * t3 + fz * t4 + fx * cos(BmLfb) * cos(CmLfc) - fb * cos(CmLfc) * sin(BmLfb) * BIXpXmLfx - fc * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) +
 153                     (2 * aiy - 2 * t1 * BIYpYmLfy + 2 * t2 * BIZpZmLfz - 2 * cos(BmLfb) * sin(CmLfc) * BIXpXmLfx) *
 154                     (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) +
 155                         t18 + t19 * BIXpXmLfx - t20 * BIXpXmLfx) - (2 * aiz + 2 * sin(BmLfb) * BIXpXmLfx - 2 * cos(AmLfa) * cos(BmLfb) * BIZpZmLfz - 2 * cos(BmLfb) * sin(AmLfa) * BIYpYmLfy) *
 156                     (fx * sin(BmLfb) - fy * cos(BmLfb) * sin(AmLfa) + fb * cos(BmLfb) * BIXpXmLfx - fz * cos(AmLfa) * cos(BmLfb) - fa * cos(AmLfa) * cos(BmLfb) * BIYpYmLfy +
 157                         fa * cos(BmLfb) * sin(AmLfa) * BIZpZmLfz + fb * cos(AmLfa) * sin(BmLfb) * BIZpZmLfz + fb * sin(AmLfa) * sin(BmLfb) * BIYpYmLfy))) /
 158             (4 * pow    (   t5 + t6 + t7, 3 / 2));
 159
 160 }
 161
 162
 163 void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
 164     RVec temp;
 165     //#pragma omp parallel
 166     //{
 167         //#pragma omp for schedule(dynamic)
 168         for (int i = 0; i < b.size(); i++) {
 169             temp = b[i];
 170             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);
 171             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);
 172             b[i][2] = cos(A) * cos(B) * (temp[2] + z) - sin(B) * (temp[0] + x) + cos(B) * sin(A) * (temp[1] + y);
 173         }
 174     //}
 175 }
 176
 177 void CalcMid (std::vector< RVec > a, std::vector< RVec > b, RVec &midA, RVec &midB, std::vector< std::pair< int, int > > pairs) {
 178     midA[0] = 0;
 179     midA[1] = 0;
 180     midA[2] = 0;
 181
 182     midB[0] = 0;
 183     midB[1] = 0;
 184     midB[2] = 0;
 185
 186     for (int i = 0; i < pairs.size(); i++) {
 187         rvec_inc(midA, a[pairs[i].first]);
 188         rvec_inc(midB, b[pairs[i].second]);
 189     }
 190     midA[0] /= pairs.size();
 191     midA[1] /= pairs.size();
 192     midA[2] /= pairs.size();
 193
 194     midB[0] /= pairs.size();
 195     midB[1] /= pairs.size();
 196     midB[2] /= pairs.size();
 197 }
 198
 199 void MyFitNew (std::vector< RVec > a, std::vector< RVec > &b, std::vector< std::pair< int, int > > pairs) {
 200     double f1 = 0, f2 = 0, fx = 0, fy = 0, fz = 0, fa = 0, fb = 0, fc = 0, l = 1;
 201     RVec ma, mb;
 202     CalcMid(a, b, ma, mb, pairs);
 203     rvec_dec(ma, mb);
 204     double x = ma[0], y = ma[1], z = ma[2], A = 0, B = 0, C = 0;
 205     for (int i = 0; i < pairs.size(); i++) {
 206         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);
 207     }
 208     if (f1 == 0) {
 209         return;
 210     } else {
 211         double fLd, fLdd;
 212         //l = 0;
 213         int count = 0;
 214             while (f1 - f2 < 0 || f1 - f2 > 0.00001) {
 215                 f1 = 0; fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
 216                 for (int i = 0; i < pairs.size(); i++) {
 217                     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);
 218                 }
 219                 /*do {
 220                     fLd = 0;
 221                     fLdd = 0;
 222                     for (int i = 0; i < pairs.size(); i++) {
 223                         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);
 224                     }
 225                     l -= fLd / fLdd;
 226                 } while (std::abs(fLd) > 1);*/
 227                 //std::cout << " Ltempo= " << Ltempo << " iter= " << Lco << "\n";
 228                 while (true) {
 229                     f2 = 0;
 230                     for (int i = 0; i < pairs.size(); i++) {
 231                         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);
 232                     }
 233                     count++;
 234                     if (f2 < f1) {
 235                         x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
 236                         fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
 237                         break;
 238                     } else {
 239                         l *= 0.85;
 240                     }
 241                 }
 242                 count++;
 243             }
 244         //std::cout << "iteration count: " << count << "\n";
 245         ApplyFit(b, x, y, z, A, B, C);
 246     }
 247 }