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; }
private KeyFrame interpolateStraightLine(KeyFrame start, KeyFrame end, double timestamp) { Point2D startPoint = start.getPosition(); Point2D endPoint = end.getPosition(); double ds = start.getTimestamp(); double duration = end.getTimestamp() - ds; double d = (timestamp - ds) / duration; double x1 = startPoint.getX(); double y1 = startPoint.getY(); double x2 = endPoint.getX(); double y2 = endPoint.getY(); double x = x1 + d * (x2 - x1); double y = y1 + d * (y2 - y1); Double p = new Point2D.Double(x, y); KeyFrame frame = new KeyFrame( TrajectoryMovement.LINE, start.getMovementSpeed(), end.getOrientation(), 0, 0, p, timestamp, start.getSourceLine()); return frame; }
private KeyFrame getKeyFrame(double timestamp) { if (keyFrame == null || keyFrame.getTimestamp() != timestamp) { KeyFrameInterpolator interpolator = new KeyFrameInterpolator(); keyFrame = interpolator.interpolate(frames, timestamp); } return keyFrame; }
private KeyFrame interpolateBezier(KeyFrame start, KeyFrame end, double timestamp) { double ds = start.getTimestamp(); double duration = end.getTimestamp() - ds; double d = (timestamp - ds) / duration; Point2D[] points = end.getControlPoints(); Point2D s = start.getPosition(); Point2D e = end.getPosition(); Point2D p = Bezier.getPoint(d, s, points[0], points[1], e); double angle = Bezier.getAngle(d, s, points[0], points[1], e); KeyFrame frame = new KeyFrame( TrajectoryMovement.BEZIER, start.getMovementSpeed(), angle, 0, 0, p, timestamp, start.getSourceLine(), points); 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(); }