private void getSpeedOne(int i) {
double vxtmp, vytmp, vztmp;
if (i < length - 1) {
vxtmp = (x.getXi(i + 1) - x.getXi(i)) / (x.getTi(i + 1) - x.getTi(i));
vytmp = (y.getXi(i + 1) - y.getXi(i)) / (x.getTi(i + 1) - x.getTi(i));
vztmp = (z.getXi(i + 1) - z.getXi(i)) / (x.getTi(i + 1) - x.getTi(i));
} else {
vxtmp = (x.getXi(i - 1) - x.getXi(i)) / (x.getTi(i - 1) - x.getTi(i));
vytmp = (y.getXi(i - 1) - y.getXi(i)) / (x.getTi(i - 1) - x.getTi(i));
vztmp = (z.getXi(i - 1) - z.getXi(i)) / (x.getTi(i - 1) - x.getTi(i));
}
minClass.tnow = x.getTi(i);
minClass.index_min = x.getFloorIndex(minClass.tnow - 1.5 * minClass.dt);
minClass.index_max = x.getFloorIndex(minClass.tnow + 1.5 * minClass.dt) + 1;
double[] xinit = {0.0, vxtmp, vytmp, vztmp, x.getXi(i), y.getXi(i), z.getXi(i)};
Uncmin_f77.optif0_f77(
6,
xinit,
minClass,
minClass.xpls,
minClass.fpls,
minClass.gpls,
minClass.itrmcd,
minClass.a,
minClass.udiag);
vx.set(i, minClass.tnow, minClass.xpls[1]);
vy.set(i, minClass.tnow, minClass.xpls[2]);
vz.set(i, minClass.tnow, minClass.xpls[3]);
vamp.set(
i,
minClass.tnow,
Math.sqrt(
minClass.xpls[1] * minClass.xpls[1]
+ minClass.xpls[2] * minClass.xpls[2]
+ minClass.xpls[3] * minClass.xpls[3]));
xbar.set(i, minClass.tnow, minClass.xpls[4]);
ybar.set(i, minClass.tnow, minClass.xpls[5]);
zbar.set(i, minClass.tnow, minClass.xpls[6]);
}
public void getSpeed(double delta_t, double delta_x) {
minClass.xdata = x;
minClass.ydata = y;
minClass.zdata = z;
minClass.dx = delta_x;
minClass.dt = delta_t;
minClass.dxData = dx;
for (int i = 0; i < length; ++i) getSpeedOne(i);
// ---
vx.setInterp();
vy.setInterp();
vz.setInterp();
vamp.setInterp();
xbar.setInterp();
ybar.setInterp();
zbar.setInterp();
}