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(); }