private KeyFrame interpolate(KeyFrame start, KeyFrame end, double timestamp) {
switch (end.getMovement()) {
case ROTATION:
return interpolateRotation(start, end, timestamp);
case LINE:
return interpolateStraightLine(start, end, timestamp);
case BEZIER:
return interpolateBezier(start, end, timestamp);
default:
return start;
}
}
public StraightLine(
String name, double initialAngle, double finalAngle, ArrayList<KeyFrame> frames) {
super(name, new Point2D.Double(), null, null, initialAngle, finalAngle);
this.frames = frames;
order = 1000;
int n = frames.size();
start = frames.get(0).getPosition();
end = frames.get(n - 1).getPosition();
boolean forward = true;
path = new Path2D.Double();
path.moveTo(start.getX(), start.getY());
for (int i = 1; i < n; i++) {
KeyFrame frame = frames.get(i);
TrajectoryMovement movement = frame.getMovement();
switch (movement) {
case LINE:
// handles backward moves by adding an extra moveto instruction
if (forward == frame.getMovementSpeed() < 0) {
Point2D p = path.getCurrentPoint();
path.moveTo(p.getX(), p.getY());
forward = !forward;
}
Point2D p = frame.getPosition();
path.lineTo(p.getX(), p.getY());
break;
case BEZIER:
p = frame.getPosition();
Point2D[] cp = frame.getControlPoints();
path.curveTo(cp[0].getX(), cp[0].getY(), cp[1].getX(), cp[1].getY(), p.getX(), p.getY());
break;
default:
break;
}
}
String description = getTrajectoryDescription();
setProperty(KEY_DESCRIPTION, description);
}
@Override
public String getTrajectoryDescription() {
StringBuilder sb = new StringBuilder();
Point2D last = null;
double lastOrientation = 0;
double lastTimestamp = 0;
Map<String, String> params = new HashMap<String, String>();
for (KeyFrame frame : frames) {
writeLine(sb, frame);
if (frame.hasComments()) {
ArrayList<String> comments = frame.getComments();
for (String s : comments) {
addParameters(params, s);
}
}
TrajectoryMovement movement = frame.getMovement();
switch (movement) {
case START:
last = frame.getPosition();
lastOrientation = frame.getOrientation();
sb.append(String.format("// start at %s\r\n", last.toString()));
break;
case BEZIER:
break;
case CLOTHOID:
break;
case LINE:
Point2D p = frame.getPosition();
double distance = p.distance(last);
double speed = frame.getMovementSpeed();
String direction = speed > 0 ? "forward" : "backward";
addComments(
params,
String.format(
"// move %s of %.0f mm (%.0f)",
direction, distance, 9.557 * distance * Math.signum(speed)),
true);
addValue(params, KEY_DISTANCE, 9.557 * distance * Math.signum(speed));
last = p;
break;
case NONE:
// double delay = frame.getTimestamp() - lastTimestamp;
// addComments(params, String.format("delay_ms(%.0f);", delay * 1000), true);
break;
case ROTATION:
double o = frame.getOrientation();
double angle = Angle.getRotationAngle(lastOrientation, o);
if (frame.useRelativeAngle()) {
// the angle is a relative angle that can be > 180°
angle = frame.getRelativeAngle();
}
angle = Math.toDegrees(angle);
addComments(
params,
String.format("// rotation of %.0f° (%.0f)", angle, 22527.5d / 360d * angle),
true);
addValue(params, KEY_ANGLE, 22527.5d / 360d * angle);
if (params.containsKey(KEY_ORIENTATION)) {
double opposite = getDoubleValue(params, KEY_ORIENTATION);
opposite = Math.toDegrees(opposite);
addValue(params, KEY_ORIENTATION, 22527.5d / 360d * opposite);
}
lastOrientation = o;
break;
default:
break;
}
lastTimestamp = frame.getTimestamp();
writeCommands(sb, params);
}
return sb.toString();
}
