private KeyFrame interpolateRotation(KeyFrame start, KeyFrame end, double timestamp) {
double startAngle = start.getOrientation();
double endAngle = end.getOrientation();
double theta = Angle.getRotationAngle(startAngle, endAngle);
if (end.useRelativeAngle()) {
theta = endAngle - startAngle;
}
double ds = start.getTimestamp();
double duration = end.getTimestamp() - ds;
double d = (timestamp - ds) / duration;
double angle = startAngle + d * theta;
Point2D p = start.getPosition();
KeyFrame frame =
new KeyFrame(
TrajectoryMovement.ROTATION,
0,
angle,
0,
start.getRotationSpeed(),
p,
timestamp,
start.getSourceLine());
return frame;
}
@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();
}
