*/
#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
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()
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());
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
{
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);
+*/