using gmx::RVec;
-double F (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double F (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return sqrt( pow(aix + (biy + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - (biz + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * (bix + x), 2) +
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2) +
pow(aiz + sin(B) * (bix + x) - cos(A) * cos(B) * (biz + z) - cos(B) * sin(A) * (biy + y), 2) );
}
-double Fx (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double Fx (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return -(2 * cos(B) * cos(C) * (aix + (biy + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - (biz + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * (bix + x)) -
2 * sin(B) * (aiz + sin(B) * (bix + x) - cos(A) * cos(B) * (biz + z) - cos(B) * sin(A) * (biy + y)) +
2 * cos(B) * sin(C) * (aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x))) /
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-double Fy (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double Fy (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return -(2 * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) *
(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x)) -
2 * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) *
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-double Fz (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double Fz (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return -(2 * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) *
(aix + (biy + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - (biz + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * (bix + x)) -
2 * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) *
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-double FA (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double FA (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return -(2 * (cos(A) * cos(B) * (biy + y) - cos(B) * sin(A) * (biz + z)) * (aiz + sin(B) * (bix + x) - cos(A) * cos(B) * (biz + z) - cos(B) * sin(A) * (biy + y)) -
2 * ((biy + y) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) + (biz + z) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C))) *
(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x)) +
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-double FB (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double FB (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return -(2 * (cos(A) * cos(B) * sin(C) * (biz + z) - sin(B) * sin(C) * (bix + x) + cos(B) * sin(A) * sin(C) * (biy + y)) *
(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x)) +
2 * (cos(A) * cos(B) * cos(C) * (biz + z) - cos(C) * sin(B) * (bix + x) + cos(B) * cos(C) * sin(A) * (biy + y)) *
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-double FC (double aix, double aiy, double aiz, double bix, double biy, double biz, double x, double y, double z, double A, double B, double C) {
+long double FC (long double aix, long double aiy, long double aiz, long double bix, long double biy, long double biz, long double x, long double y, long double z, long double A, long double B, long double C) {
return (2 * ((biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) - (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) + cos(B) * sin(C) * (bix + x)) *
(aix + (biy + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) - (biz + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - cos(B) * cos(C) * (bix + x)) -
2 * ((biz + z) * (sin(A) * sin(C) + cos(A) * cos(C) * sin(B)) - (biy + y) * (cos(A) * sin(C) - cos(C) * sin(A) * sin(B)) + cos(B) * cos(C) * (bix + x)) *
pow(aiy - (biy + y) * (cos(A) * cos(C) + sin(A) * sin(B) * sin(C)) + (biz + z) * (cos(C) * sin(A) - cos(A) * sin(B) * sin(C)) - cos(B) * sin(C) * (bix + x), 2)));
}
-void ApplyFit (std::vector< RVec > &b, double x, double y, double z, double A, double B, double C) {
+void ApplyFit (std::vector< RVec > &b, long double x, long double y, long double z, long double A, long double B, long double C) {
RVec temp;
for (int i = 0; i < b.size(); i++) {
temp = b[i];
}
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;
- double f21 = 0, f22 = 0, f23 = 0, f24 = 0, f25 = 0, f26 = 0;
- double l1 = 1, l2 = 1, l3 = 1, l4 = 1, l5 = 1, l6 = 1, l = 1;
+ long double f1 = 0, f2 = 0, fx = 0, fy = 0, fz = 0, fa = 0, fb = 0, fc = 0;
+ long double f21 = 0, f22 = 0, f23 = 0, f24 = 0, f25 = 0, f26 = 0;
+ long double l1 = 1, l2 = 1, l3 = 1, l4 = 1, l5 = 1, l6 = 1, 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;
+ long double x = ma[0], y = ma[1], z = ma[2], A = 0, B = 0, C = 0;
int count;
while (true) {
fb += FB(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);
fc += FC(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);
}
- 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], b[pairs[i].second][1], b[pairs[i].second][2], x - l * fx, y - l * fy, z - l * fz, A - l * fa, B - l * fb, C - l * fc);
- /*
- f2 += F(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 - l1 * fx, y - l2 * fy, z - l3 * fz, A - l4 * fa, B - l5 * fb, C - l6 * fc);
- f21 += F(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 - l1 * fx, y, z, A, B, C);
- f22 += F(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 - l2 * fy, z, A, B, C);
- f23 += F(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 - l3 * fz, A, B, C);
- f24 += F(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 - l4 * fa, B, C);
- f25 += F(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 - l5 * fb, C);
- f26 += F(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 - l6 * fc);
- */
- }
- /*count++;
- if (count % 100 == 0) {
- std::cout << f1 << " " << f2 << "\n";
- }*/
- if (f2 < f1) {
- x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
- //x -= l1 * fx; y -= l2 * fy; z -= l3 * fz; A -= l4 * fa; B -= l5 * fb; C -= l6 * fc;
- fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
- break;
- } else {
- l *= 0.85;
- /*
- if (f21 > f1) {
- l1 /= 2;
- }
- if (f22 > f1) {
- l2 /= 2;
- }
- if (f23 > f1) {
- l3 /= 2;
- }
- if (f24 > f1) {
- l4 /= 2;
+ if (f1 != 0) {
+ 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], b[pairs[i].second][1], b[pairs[i].second][2], x - l * fx, y - l * fy, z - l * fz, A - l * fa, B - l * fb, C - l * fc);
+ /*
+ f2 += F(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 - l1 * fx, y - l2 * fy, z - l3 * fz, A - l4 * fa, B - l5 * fb, C - l6 * fc);
+ f21 += F(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 - l1 * fx, y, z, A, B, C);
+ f22 += F(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 - l2 * fy, z, A, B, C);
+ f23 += F(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 - l3 * fz, A, B, C);
+ f24 += F(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 - l4 * fa, B, C);
+ f25 += F(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 - l5 * fb, C);
+ f26 += F(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 - l6 * fc);
+ */
}
- if (f25 > f1) {
- l5 /= 2;
+ /*count++;
+ if (count % 100 == 0) {
+ std::cout << " " << f1 << " " << f2 << "\n";
+ }*/
+ if (f2 < f1) {
+ x -= l * fx; y -= l * fy; z -= l * fz; A -= l * fa; B -= l * fb; C -= l * fc;
+ //x -= l1 * fx; y -= l2 * fy; z -= l3 * fz; A -= l4 * fa; B -= l5 * fb; C -= l6 * fc;
+ fx = 0; fy = 0; fz = 0; fa = 0; fb = 0; fc = 0;
+ break;
+ } else {
+ l *= 0.85;
+ /*
+ if (f21 > f1) {
+ l1 /= 2;
+ }
+ if (f22 > f1) {
+ l2 /= 2;
+ }
+ if (f23 > f1) {
+ l3 /= 2;
+ }
+ if (f24 > f1) {
+ l4 /= 2;
+ }
+ if (f25 > f1) {
+ l5 /= 2;
+ }
+ if (f26 > f1) {
+ l6 /= 2;
+ }
+ */
}
- if (f26 > f1) {
- l6 /= 2;
- }
- */
}
}
- if (f1 - f2 > 0 && f1 - f2 < 0.00001) {
+ if (f1 - f2 >= 0 && f1 - f2 < 0.00001) {
break;
}
f1 = 0; f2 = 0;
}
}
-//Fitng -s '/home/toluk/Develop/samples/reca_rd/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_rd/reca_rd.mono.xtc' -select 'name CA'
+// Fitng -s '/home/toluk/Develop/samples/reca_rd/reca_rd.mono.tpr' -f '/home/toluk/Develop/samples/reca_rd/reca_rd.mono.xtc' -select 'name CA'
+// Fitng -s '/home/toluk/Develop/FitSamples/cube_1st_fr.pdb' -f '/home/toluk/Develop/FitSamples/cube.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/cube_with_termal_1st_fr.pdb' -f '/home/toluk/Develop/FitSamples/cube_with_termal.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/cube_with_small_cube_floating_1st_fr_D100.pdb' -f '/home/toluk/Develop/FitSamples/cube_with_small_cube_floating_D100.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/cube_with_small_cube_floating_1st_fr_D10.pdb' -f '/home/toluk/Develop/FitSamples/cube_with_small_cube_floating_D10.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/garmoshka_1st_fr.pdb' -f '/home/toluk/Develop/FitSamples/garmoshka.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/garmoshka_2nd_fr.pdb' -f '/home/toluk/Develop/FitSamples/garmoshka.pdb' -select 'name NA'
+// Fitng -s '/home/toluk/Develop/FitSamples/rubber_1st_fr.pdb' -f '/home/toluk/Develop/FitSamples/rubber.pdb' -select 'name NA'
+// rubber_1st_fr
void
Fitng::finishAnalysis(int nframes)
for (int i = 0; i < index.size(); i++) {
pairs.push_back(std::make_pair(i, i));
}
-
double dist1 = 0, dist2 = 0;
-
- for (int i = 0; i < index.size(); i++) {
- dist1 += F(trajectory[0][i][0], trajectory[0][i][1], trajectory[0][i][2], trajectory.back()[i][0], trajectory.back()[i][1], trajectory.back()[i][2], 0, 0, 0, 0, 0, 0);
- }
-
- std::cout << "\n\n\n";
- std::cout << "\n\n\n" << "old dist = " << dist1 << "\n";
-
- rvec *traj1, *traj2;
+ std::vector< real > w_rls;
+ w_rls.resize(index.size(), 1);
+ rvec *traj1, **traj2;
snew(traj1, index.size());
- snew(traj2, index.size());
+ snew(traj2, trajectory.size());
for (int i = 0; i < index.size(); i++) {
copy_rvec(trajectory[0][i].as_vec(), traj1[i]);
- copy_rvec(trajectory.back()[i].as_vec(), traj2[i]);
+ }
+ reset_x(index.size(), NULL, index.size(), NULL, traj1, &w_rls[0]);
+ for (int j = 0; j < trajectory.size(); j++) {
+ snew(traj2[j], index.size());
+ for (int i = 0; i < index.size(); i++) {
+ copy_rvec(trajectory[j][i].as_vec(), traj2[j][i]);
+ }
}
- //
- // My Fit
- //
+ for (int j = 1; j < trajectory.size(); j++) {
- MyFitNew(trajectory[0], trajectory.back(), pairs);
- for (int i = 0; i < index.size(); i++) {
- dist2 += F(trajectory[0][i][0], trajectory[0][i][1], trajectory[0][i][2], trajectory.back()[i][0], trajectory.back()[i][1], trajectory.back()[i][2], 0, 0, 0, 0, 0, 0);
- }
- std::cout << "\n" << "my fit dist = " << dist2 << "\n";
+ 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], trajectory[j][i][1], trajectory[j][i][2], 0, 0, 0, 0, 0, 0);
+ //std::cout << F(trajectory[0][i][0], trajectory[0][i][1], trajectory[0][i][2], trajectory[j][i][0], trajectory[j][i][1], trajectory[j][i][2], 0, 0, 0, 0, 0, 0);
+ }
+ std::cout << "\nbasic dist = " << dist1 / index.size() << "\n";
+ //
+ // My Fit
+ //
- dist2 = 0;
- std::vector< real > w_rls;
- w_rls.resize(index.size(), 1);
- reset_x(index.size(), NULL, index.size(), NULL, traj1, &w_rls[0]);
- reset_x(index.size(), NULL, index.size(), NULL, traj2, &w_rls[0]);
- //
- // Old Fit
- //
- do_fit(index.size(), &w_rls[0], traj1, traj2);
+ MyFitNew(trajectory[0], trajectory[j], pairs);
+ 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], trajectory[j][i][1], trajectory[j][i][2], 0, 0, 0, 0, 0, 0);
+ }
+ std::cout << "my fit dist = " << dist2 / index.size()<< "\n";
+
+ dist2 = 0;
+
+ reset_x(index.size(), NULL, index.size(), NULL, traj2[j], &w_rls[0]);
+ //
+ // Old Fit
+ //
+
+ /*for (int i = 0; i < index.size(); i++) {
+ std::cout << traj2[j][i][0] << " " << traj2[j][i][1] << " " << traj2[j][i][2] << "\n";
+ }*/
+ do_fit(index.size(), &w_rls[0], traj1, traj2[j]);
+ /*for (int i = 0; i < index.size(); i++) {
+ std::cout << traj2[j][i][0] << " " << traj2[j][i][1] << " " << traj2[j][i][2] << "\n";
+ }*/
+
+ for (int i = 0; i < index.size(); i++) {
+ dist2 += F(traj1[i][0], traj1[i][1], traj1[i][2], traj2[j][i][0], traj2[j][i][1], traj2[j][i][2], 0, 0, 0, 0, 0, 0);
+ //std::cout << F(traj1[i][0], traj1[i][1], traj1[i][2], traj2[j][i][0], traj2[j][i][1], traj2[j][i][2], 0, 0, 0, 0, 0, 0);
+ }
+ std::cout << "old fit dist = " << dist2 / index.size()<< "\n";
- for (int i = 0; i < index.size(); i++) {
- dist2 += F(traj1[i][0], traj1[i][1], traj1[i][2], traj2[i][0], traj2[i][1], traj2[i][2], 0, 0, 0, 0, 0, 0);
+ dist1 = 0;
+ dist2 = 0;
+
+ }
+
+ int co = 0;
+ freopen ("/home/toluk/Develop/FitSamples/old_fit_result.pdb","w",stdout);
+ for (int l = 1; l < trajectory.size(); l++) {
+ for (int i = 0; i < index.size(); i++) {
+ //cout << D*i + (float)l * 6;
+ printf("ATOM %5d NA NA %5d %8.3f%8.3f%8.3f%6.2f%6.2f %4s%2s \n", co, co, (traj2[l][i][0] * 10), (traj2[l][i][1] * 10), (traj2[l][i][2] * 10), 1.0, 20.0, " ", " ");
+ co++;
+ }
+ co = 0;
+ printf("ENDMDL\n");
+ }
+ printf("END\n");
+ freopen ("/home/toluk/Develop/FitSamples/new_fit_result.pdb","w",stdout);
+ for (int l = 1; l < trajectory.size(); l++) {
+ for (int i = 0; i < index.size(); i++) {
+ //cout << D*i + (float)l * 6;
+ printf("ATOM %5d NA NA %5d %8.3f%8.3f%8.3f%6.2f%6.2f %4s%2s \n", co, co, (trajectory[l][i][0] * 10), (trajectory[l][i][1] * 10), (trajectory[l][i][2] * 10), 1.0, 20.0, " ", " ");
+ co++;
+ }
+ co = 0;
+ printf("ENDMDL\n");
}
- std::cout << "\n" << "old fit dist = " << dist2 << "\n";
+ printf("END\n");
+ fclose(stdout);
sfree(traj1);
+ for (int i = 0; i < trajectory.size(); i++) {
+ sfree(traj2[i]);
+ }
sfree(traj2);
}