49 lines
1.5 KiB
Java
49 lines
1.5 KiB
Java
|
import java.util.ArrayList;
|
||
|
|
|
||
|
|
public class Movement {
|
||
|
|
double move[][]; // move = { {t0,y0}, {t1,y1}, ..., {tn, yn} }
|
||
|
|
Matrix HermiteMatrix = new Matrix();
|
||
|
|
double fourIn[][]; // four[i] = { p1, p4, r1, r4 }
|
||
|
|
double fourOut[][]; // four[i] = { a, b, c, d }
|
||
|
|
double slope_at_time[];
|
||
|
|
int innerI;
|
||
|
|
double t;
|
||
|
|
|
||
|
|
public Movement(double move[][]) {
|
||
|
|
this.move = move;
|
||
|
|
HermiteMatrix.hermiteMatrix();
|
||
|
|
// calculate p1, p4, r1, r4
|
||
|
|
innerI = move.length-1;
|
||
|
|
fourIn = new double[innerI][4];
|
||
|
|
fourOut = new double[innerI][4];
|
||
|
|
slope_at_time = new double[innerI+1];
|
||
|
|
slope_at_time[innerI] = 2.0 * (move[innerI][1]-move[innerI-1][1]) / (move[innerI][0]-move[innerI-1][0]);
|
||
|
|
|
||
|
|
for (int i=0; i<innerI; i++) {
|
||
|
|
fourIn[i][0] = move[i][1];
|
||
|
|
fourIn[i][1] = move[i+1][1];
|
||
|
|
if (i == 0) {
|
||
|
|
slope_at_time[i] = 2.0 * (move[i+1][1]-move[i][1]) / (move[i+1][0]-move[i][0]);
|
||
|
|
} else {
|
||
|
|
slope_at_time[i] = (move[i+1][1]-move[i-1][1]) / (move[i+1][0]-move[i-1][0]);
|
||
|
|
}
|
||
|
|
fourIn[i][2] = slope_at_time[i] / (move[i+1][0]-move[i][0]);
|
||
|
|
fourIn[i][3] = slope_at_time[i+1] / (move[i+1][0]-move[i][0]);
|
||
|
|
HermiteMatrix.transform(fourIn[i],fourOut[i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public double getMove(double time) {
|
||
|
|
double thisMove = 0;
|
||
|
|
for (int i=0; i<innerI; i++) {
|
||
|
|
if (time <= move[i+1][0] && time > move[i][0]) {
|
||
|
|
t = (time-move[i][0]) / (move[i+1][0]-move[i][0]);
|
||
|
|
// System.out.println("t is: "+t);
|
||
|
|
thisMove = fourOut[i][0]*Math.pow(t,3) + fourOut[i][1]*Math.pow(t,2) + fourOut[i][2]*t + fourOut[i][3];
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return thisMove;
|
||
|
|
}
|
||
|
|
}
|