changed to linear kernel algo // test pasta
authorAnatoly <Titov_AI@pnpi.nrcki.ru>
Fri, 9 Feb 2018 11:57:04 +0000 (14:57 +0300)
committerAnatoly <Titov_AI@pnpi.nrcki.ru>
Fri, 9 Feb 2018 11:57:04 +0000 (14:57 +0300)
src/spirals.cpp

index eb9f95ed30220c78fc4baf2c150a45a3b0903686..5bd056718afa9e19542148ee80b5ce4eca2d225a 100644 (file)
@@ -34,6 +34,7 @@
  */
 #include <omp.h>
 #include <iostream>
+#include <math.h>
 
 #include <gromacs/trajectoryanalysis.h>
 #include <gromacs/math/do_fit.h>
 
 using namespace gmx;
 
-struct crcl {
-    long double x;
-    long double y;
-    long double z;
-    long double r;
-};
+double Fx (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           pow (pow (p2 * (x[i][2] - z0) - p3 * (x[i][1] - y0), 2) +
+                pow (p3 * (x[i][0] - x0) - p1 * (x[i][2] - z0), 2) +
+                pow (p1 * (x[i][1] - y0) - p2 * (x[i][0] - x0), 2), 0.5) /
+           pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5);
+    }
+    return ret;
+}
+
+double fx0 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           (2 * p2 * (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1])) + 2 * p3 * (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]))) /
+           (2 *     pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                            pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                            pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                    pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5));
+    }
+    return ret;
+}
+
+double fy0 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           -(2 * p1 * (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1])) - 2 * p3 * (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]))) /
+            (2 *    pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                            pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                            pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                    pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5));
+    }
+    return ret;
+}
 
-crcl return_crcl(long double x1, long double y1, long double z1,
-                 long double x2, long double y2, long double z2,
-                 long double x3, long double y3, long double z3) {
-    long double c, b, a, z, y, x, r;
-    long double a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11, a12;
-
-    c = (z3 - z1 * x3 / x1 - (z2 - z1 * x2 / x1) / (y2 - y1 * x2 / x1)) / (1 - x3 / x1 - (1 - x2 / x1) / (y2 - y1 * x2 / x1));
-    b = (y2 - y1 * x2 / x1) / (1 - x2 / x1 - 1 / c * (z2 - z1 * x2 / x1));
-    a = (x1) / (1 - y1 / b - z1 / c);
-
-    a1 = 1 / a;
-    a2 = 1 / b;
-    a3 = 1 / c;
-    a4 = 1;
-
-    a5 = 2 * (x3 - x1);
-    a6 = 2 * (y3 - y1);
-    a7 = 2 * (z3 - z1);
-    a8 = (x3 * x3 - x1 * x1) + (y3 * y3 - y1 * y1) + (z3 * z3 - z1 * z1);
-
-    a9  = 2 * (x3 - x2);
-    a10 = 2 * (y3 - y2);
-    a11 = 2 * (z3 - z2);
-    a12 = (x3 * x3 - x2 * x2) + (y3 * y3 - y2 * y2) + (z3 * z3 - z2 * z2);
-
-    z = (a12 - a9 * a4 / a1 - (a10 - a9 * a2 / a1) * (a8 - a5 * a4 / a1) / (a6 - a5 * a2 / a1)) /
-            (a11 - a9 * a3 / a1 - (a10 - a9 * a2 / a1) * (a7 - a5 * a3 / a1) / (a6 - a5 * a2 / a1));
-    y = 1 / (a6 - a5 * a2 / a1) * (a8 - a5 * a4 / a1 - z * (a7 - a5 * a3 / a1));
-    x = 1 / a1 * (a4 - a2 * y - a3 * z);
-    r = std::sqrt((x - x3) * (x - x3) + (y - y3) * (y - y3) + (z - z3) * (z - z3));
-
-    crcl rtrn;
-
-    rtrn.x = x;
-    rtrn.y = y;
-    rtrn.z = z;
-    rtrn.r = r;
-
-    return rtrn;
+double fz0 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           -(2 * p1 * (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2])) + 2 * p2 * (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]))) /
+            (2 *    pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                            pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                            pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                    pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5));
+    }
+    return ret;
 }
 
-long double lr_turn(std::vector< long double > a, std::vector< long double > b, std::vector< long double > c) {
-    return a[0] * b[1] * c[2] + a[1] * b[2] * c[0] + b[0] * c[1] * a[2] -
-            (a[2] * b[1] * c[0] + a[0] * b[2] * c[1] + a[1] * b[0] * c[2]);
+double fp1 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           -(2 * (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1])) * (y0 - x[i][1]) + 2 * (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2])) * (z0 - x[i][2])) /
+            (2 *    pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                            pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                            pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                    pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5)) -
+            (p1 *   pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                            pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                            pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5)) /
+                    pow (p1 * p1 + p2 * p2 + p3 * p3, 1.5);
+    }
+    return ret;
 }
 
-long double kernel_dist(crcl a, crcl b) {
-    return std::sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
+double fp2 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           (2 * (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1])) * (x0 - x[i][0]) - 2 * (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2])) * (z0 - x[i][2])) /
+           (2 *    pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                           pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                           pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                   pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5)) -
+           (p2 *   pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                           pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                           pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5)) /
+                   pow (p1 * p1 + p2 * p2 + p3 * p3, 1.5);
+    }
+    return ret;
+}
+
+double fp3 (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x) {
+    double ret = 0;
+    for (int i = 0; i < x.size(); i++) {
+        ret +=
+           (2 * (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2])) * (x0 - x[i][0]) + 2 * (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2])) * (y0 - x[i][1])) /
+           (2 *    pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                           pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                           pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5) *
+                   pow (p1 * p1 + p2 * p2 + p3 * p3, 0.5)) -
+           (p3 *   pow (   pow (p2 * (x0 - x[i][0]) - p1 * (y0 - x[i][1]), 2) +
+                           pow (p3 * (x0 - x[i][0]) - p1 * (z0 - x[i][2]), 2) +
+                           pow (p3 * (y0 - x[i][1]) - p2 * (z0 - x[i][2]), 2), 0.5)) /
+                   pow (p1 * p1 + p2 * p2 + p3 * p3, 1.5);
+    }
+    return ret;
+}
+
+void linear_kernel_search (double x0, double y0, double z0, double p1, double p2, double p3, std::vector< RVec > x, double epsi) {
+    double F = 1, FX = 0, FX0 = 0, FY0 = 0, FZ0 = 0, FP1 = 0, FP2 = 0, FP3 = 0;
+    double L1, L2, L3, L4, L5, L6;
+    while (F - FX >= epsi) {
+        F = FX;
+
+        FX = Fx(x0, y0, z0, p1, p2, p3, x);
+        FX0 = fx0(x0, y0, z0, p1, p2, p3, x);
+        FY0 = fy0(x0, y0, z0, p1, p2, p3, x);
+        FZ0 = fz0(x0, y0, z0, p1, p2, p3, x);
+        FP1 = fp1(x0, y0, z0, p1, p2, p3, x);
+        FP2 = fp2(x0, y0, z0, p1, p2, p3, x);
+        FP3 = fp3(x0, y0, z0, p1, p2, p3, x);
+
+        L1 = 1;
+        while (Fx(x0 - L1 * FX0, y0, z0, p1, p2, p3, x) - FX > 0) {
+            L1 /= 2;
+        }
+        L2 = 1;
+        while (Fx(x0, y0 - L2 * FY0, z0, p1, p2, p3, x) - FX > 0) {
+            L2 /= 2;
+        }
+        L3 = 1;
+        while (Fx(x0, y0, z0 - L3 * FZ0, p1, p2, p3, x) - FX > 0) {
+            L3 /= 2;
+        }
+        L4 = 1;
+        while (Fx(x0, y0, z0, p1 - L4 * FP1, p2, p3, x) - FX > 0) {
+            L4 /= 2;
+        }
+        L5 = 1;
+        while (Fx(x0, y0, z0, p1, p2 - L5 * FP2, p3, x) - FX > 0) {
+            L5 /= 2;
+        }
+        L6 = 1;
+        while (Fx(x0, y0, z0, p1, p2, p3 - L6 * FP3, x) - FX > 0) {
+            L6 /= 2;
+        }
+        x0 -= L1 * FX0;
+        y0 -= L2 * FY0;
+        z0 -= L3 * FZ0;
+        p1 -= L4 * FP1;
+        p2 -= L5 * FP2;
+        p3 -= L6 * FP3;
+    }
+}
+
+RVec kernel_pro (double x0, double y0, double z0, double p1, double p2, double p3, RVec x) {
+    double lambda = (p1 * (x[0] - x0) + p2 * (x[1] - y0) + p3 * (x[2] - z0)) / (p1 * p1 + p2 * p2 + p3 * p3);
+    RVec pro;
+    pro[0] = x0 + p1 * lambda;
+    pro[1] = y0 + p2 * lambda;
+    pro[2] = z0 + p3 * lambda;
+    return pro;
+}
+
+double left_right_turn () {
+
 }
 
 /*! \brief
@@ -131,11 +237,11 @@ class Spirals : public TrajectoryAnalysisModule
         AnalysisData                     data_;
         AnalysisDataAverageModulePointer avem_;
 
-        SelectionList                                       sel_;
-        int                                                 frames       = 0;
-        std::vector< std::vector< crcl > >                  kernel;
-        std::vector< std::vector< std::vector< int > > >    circles;
-        std::vector< std::vector< long double > >           spiral_dist;
+        SelectionList                           sel_;
+        int                                     frames      = 0;
+        double                                  epsi        = 0.01;
+
+        std::vector< std::vector< RVec > >      kernel;
 };
 
 Spirals::Spirals()
@@ -170,17 +276,13 @@ void
 Spirals::initAnalysis(const TrajectoryAnalysisSettings &settings,
                                const TopologyInformation         & /*top*/)
 {
-
+    kernel.resize(0);
 }
 
 void
 Spirals::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
                                TrajectoryAnalysisModuleData *pdata)
 {
-    kernel.resize(frames + 1);
-    circles.resize(frames + 1);
-    spiral_dist.resize(frames + 1);
-
     const SelectionList &sel = pdata->parallelSelections(sel_);
     std::vector< RVec > temp;
     temp.resize(sel_.size());
@@ -188,130 +290,41 @@ Spirals::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
         copy_rvec(sel[i].position(0).x(), temp[i]);
     }
 
-    // центры окружностей и их радиусы
-    for (int i = 0; i < temp.size() - 2; i++) {
-        kernel[frames].push_back(return_crcl(   temp[i][0],     temp[i][1],     temp[i][2],
-                                        temp[i + 1][0], temp[i + 1][1], temp[i + 1][2],
-                                        temp[i + 2][0], temp[i + 2][1], temp[i + 2][2]));
-        if (i == 0) {
-            kernel[frames].push_back(kernel[frames].front());
-        }
-        if (i == temp.size() - 3) {
-            kernel[frames].push_back(kernel[frames].back());
-        }
-    }
+    RVec mid = 0, arrow = 0;
 
-    // распределение точек по виткам
-    std::vector< long double > a, b, c;
-    a.resize(3, 0);
-    b.resize(3, 0);
-    c.resize(3, 0);
-
-    a[0] = temp[0][0] - kernel[frames].front().x;
-    a[1] = temp[0][1] - kernel[frames].front().y;
-    a[2] = temp[0][2] - kernel[frames].front().z;
-
-    c[0] = kernel[frames].back().x - kernel[frames].front().x;
-    c[1] = kernel[frames].back().y - kernel[frames].front().y;
-    c[2] = kernel[frames].back().z - kernel[frames].front().z;
-
-    std::vector< int > empty;
-    empty.resize(0);
-
-
-    circles[frames].push_back(empty);
-    circles[frames][0].push_back(0);
-
-    long double sign = 0, prev_sign = 0;
-    for (int i = 1; i < temp.size(); i++) {
-        if (i < kernel[frames].size()) {
-            b[0] = temp[i][0] - kernel[frames][i + 1].x;
-            b[1] = temp[i][1] - kernel[frames][i + 1].y;
-            b[2] = temp[i][2] - kernel[frames][i + 1].z;
-        } else {
-            b[0] = temp[i][0] - kernel[frames].back().x;
-            b[1] = temp[i][1] - kernel[frames].back().y;
-            b[2] = temp[i][2] - kernel[frames].back().z;
-        }
-        long double local_sign = lr_turn(a, b ,c);
-        if (sign == 0) {
-            sign = local_sign;
-            prev_sign = local_sign;
-            circles[frames].back().push_back(i);
-        } else {
-            if (std::signbit(local_sign) == std::signbit(prev_sign)) {
-                circles[frames].back().push_back(i);
-            } else {
-                if (std::signbit(local_sign) != std::signbit(sign)) {
-                    circles[frames].back().push_back(i);
-                } else {
-                    circles[frames].push_back(empty);
-                    circles[frames].back().push_back(i);
-                }
-            }
-            prev_sign = local_sign;
-        }
+    for (int i = 0; i < sel.size(); i++) {
+        copy_rvec(sel[i].position(0).x(), mid);
     }
+    mid[0] /= sel.size();
+    mid[1] /= sel.size();
+    mid[2] /= sel.size();
+    rvec_sub(temp.back(), temp.front(), arrow);
 
-    // шаг спирали
-    spiral_dist[frames].resize(circles[frames].size(), 0);
-    for (int i = 0; i < circles[frames].size(); i++) {
-        if (i == 0 && circles[frames].size() == 1) {
-            spiral_dist[frames][i] = kernel_dist(kernel[frames][circles[frames][i].front()], kernel[frames][circles[frames][i].back()]);
-        } else if (i == 0 && circles[frames].size() > 1) {
-            spiral_dist[frames][i] = kernel_dist(kernel[frames][circles[frames][i].front()], kernel[frames][circles[frames][i].back()]) +
-                             kernel_dist(kernel[frames][circles[frames][i].back()], kernel[frames][circles[frames][i + 1].front()]) / 2;
-        } else if (i != circles[frames].size() - 1) {
-            spiral_dist[frames][i] = kernel_dist(kernel[frames][circles[frames][i - 1].back()], kernel[frames][circles[frames][i].front()]) / 2 +
-                             kernel_dist(kernel[frames][circles[frames][i].front()], kernel[frames][circles[frames][i].back()]) +
-                             kernel_dist(kernel[frames][circles[frames][i].back()], kernel[frames][circles[frames][i + 1].front()]) / 2;
-        } else if (i == circles[frames].size() - 1) {
-            spiral_dist[frames][i] = kernel_dist(kernel[frames][circles[frames][i - 1].back()], kernel[frames][circles[frames][i].front()]) / 2 +
-                             kernel_dist(kernel[frames][circles[frames][i].front()], kernel[frames][circles[frames][i].back()]);
-        }
+    linear_kernel_search(mid[0], mid[1], mid[2], arrow[0], arrow[1], arrow[2], temp, epsi); //изменить формат функции для изменения значений переменных
+
+    kernel.resize(kernel.size() + 1);
+    for (int i = 0; i < sel.size(); i++) {
+        kernel.back().push_back(kernel_pro(mid[0], mid[1], mid[2], arrow[0], arrow[1], arrow[2], temp[i]));
     }
 
-   frames++;
+    frames++;
 }
 
 void
 Spirals::finishAnalysis(int /*nframes*/)
 {
-    /*
-     *  kernel.resize(frames + 1);
-        circles.resize(frames + 1);
-        spiral_dist.resize(frames + 1);
-     */
-    FILE *file;
-    file = std::fopen("kernel.txt", "w+");
-    for (int i = 0; i < kernel.size(); i++) {
-        for (int j = 1; j < kernel[i].size() - 1; j++) {
-            std::fprintf(file, "%3.2Lf %3.2Lf %3.2Lf %3.2Lf\n", kernel[i][j].x, kernel[i][j].y, kernel[i][j].z, kernel[i][j].r);
-        }
-        std::fprintf(file, "\n");
-    }
-    std::fclose(file);
-
-    file = std::fopen("circles.txt", "w+");
-    for (int i = 0; i < circles.size(); i++) {
-        for (int j = 0; j < circles[i].size(); j++) {
-            for (int k = 0; k < circles[i][j].size(); k++) {
-                std::fprintf(file, "%3.2d ", circles[i][j][k]);
-            }
-            std::fprintf(file, "\n");
-        }
-        std::fprintf(file, "\n");
-    }
-    std::fclose(file);
 
-    file = std::fopen("spiral_dist.txt", "w+");
+
+
+
+    /*file = std::fopen("spiral_dist.txt", "w+");
     for (int i = 0; i < spiral_dist.size(); i++) {
         for (int j = 0; j < spiral_dist[i].size(); j++) {
             std::fprintf(file, "%3.2Lf\n", spiral_dist[i][j]);
         }
         std::fprintf(file, "\n");
     }
-    std::fclose(file);
+    std::fclose(file);*/
 }
 
 void
@@ -328,3 +341,20 @@ main(int argc, char *argv[])
 {
     return gmx::TrajectoryAnalysisCommandLineRunner::runAsMain<Spirals>(argc, argv);
 }
+
+
+/*
+ * F = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) ((p2*(zi-z0)-p3*(yi-y0))^2+(p3*(xi-x0)-p1*(zi-z0))^2+(p1*(yi-y0)-p2*(xi-x0))^2)^(1/2)/(p1*p1+p2*p2+p3*p3)^(1/2);
+%diff(F, x0)
+fx0 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) (2*p2*(p2*(x0 - xi) - p1*(y0 - yi)) + 2*p3*(p3*(x0 - xi) - p1*(z0 - zi)))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2));
+%diff(F, y0)
+fy0 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) -(2*p1*(p2*(x0 - xi) - p1*(y0 - yi)) - 2*p3*(p3*(y0 - yi) - p2*(z0 - zi)))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2));
+%diff(F, z0)
+fz0 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) -(2*p1*(p3*(x0 - xi) - p1*(z0 - zi)) + 2*p2*(p3*(y0 - yi) - p2*(z0 - zi)))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2));
+%diff(F, p1)
+fp1 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) -(2*(p2*(x0 - xi) - p1*(y0 - yi))*(y0 - yi) + 2*(p3*(x0 - xi) - p1*(z0 - zi))*(z0 - zi))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2)) - (p1*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2))/(p1^2 + p2^2 + p3^2)^(3/2);
+%diff(F, p2)
+fp2 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) (2*(p2*(x0 - xi) - p1*(y0 - yi))*(x0 - xi) - 2*(p3*(y0 - yi) - p2*(z0 - zi))*(z0 - zi))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2)) - (p2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2))/(p1^2 + p2^2 + p3^2)^(3/2);
+%diff(F, p3)
+fp3 = @(x0, y0, z0, p1, p2, p3, xi, yi, zi) (2*(p3*(x0 - xi) - p1*(z0 - zi))*(x0 - xi) + 2*(p3*(y0 - yi) - p2*(z0 - zi))*(y0 - yi))/(2*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2)*(p1^2 + p2^2 + p3^2)^(1/2)) - (p3*((p2*(x0 - xi) - p1*(y0 - yi))^2 + (p3*(x0 - xi) - p1*(z0 - zi))^2 + (p3*(y0 - yi) - p2*(z0 - zi))^2)^(1/2))/(p1^2 + p2^2 + p3^2)^(3/2);
+*/