Exemple #1
0
 static void handleKeyTyped(KeyEvent e) {
   if (e.getKeyChar() == ' ') {
     currentSequenceNumDisplay++;
     scribbles = Collections.synchronizedList(new ArrayList<DrawObject>());
     currentScribbleNum = 0;
   }
   drawArea.repaint();
 }
  /**
   * Connects the figure to the new connectableConnector. If there is no new connectableConnector
   * the connection reverts to its original one.
   */
  public void trackEnd(Point anchor, Point lead, int modifiersEx) {
    ConnectionFigure f = getOwner();
    // Change node type
    if ((modifiersEx
                & (InputEvent.META_DOWN_MASK
                    | InputEvent.CTRL_DOWN_MASK
                    | InputEvent.ALT_DOWN_MASK
                    | InputEvent.SHIFT_DOWN_MASK))
            != 0
        && (modifiersEx & InputEvent.BUTTON2_DOWN_MASK) == 0) {
      f.willChange();
      int index = getBezierNodeIndex();
      BezierPath.Node v = f.getNode(index);
      if (index > 0 && index < f.getNodeCount()) {
        v.mask = (v.mask + 3) % 4;
      } else if (index == 0) {
        v.mask = ((v.mask & BezierPath.C2_MASK) == 0) ? BezierPath.C2_MASK : 0;
      } else {
        v.mask = ((v.mask & BezierPath.C1_MASK) == 0) ? BezierPath.C1_MASK : 0;
      }
      f.setNode(index, v);
      f.changed();
      fireHandleRequestSecondaryHandles();
    }

    Point2D.Double p = view.viewToDrawing(lead);
    view.getConstrainer().constrainPoint(p);
    Connector target = findConnectionTarget(p, view.getDrawing());
    if (target == null) {
      target = savedTarget;
    }

    setLocation(p);
    if (target != savedTarget) {
      disconnect();
      connect(target);
    }
    getOwner().setLiner(savedLiner);
    getOwner().updateConnection();
    connectableConnector = null;
    connectors = Collections.emptyList();
  }
Exemple #3
0
  public static SymbolAxis getBLMAxis(
      final String param,
      final Map<String, TypeDataSet> typeSeries,
      Map<String, NumberAxis> typeAxes) {

    final ArrayList<LossDetector> detectors = new ArrayList<LossDetector>();

    for (TypeDataSet v : typeSeries.values()) {
      if (v.isVisible()) {
        detectors.addAll(v.getDetectors());
      }
    }
    if (detectors.size() > 0) {
      Collections.sort(detectors, LossDetector.getComparator());
    }

    String[] names = new String[detectors.size()];
    final LossDetector[] dets = new LossDetector[detectors.size()];
    final ValueAxis[] axes = new ValueAxis[detectors.size()];

    int index = 0;
    for (LossDetector det : detectors) {
      names[index] = det.getShortName();
      dets[index] = det;
      axes[index] = typeAxes.get(det.getType());
      index++;
    }

    @SuppressWarnings("rawtypes")
    SymbolAxis axis =
        new SymbolAxis("", names) {
          private static final long serialVersionUID = -2911592900608877261L;

          protected void drawGridBandsHorizontal(
              Graphics2D g2,
              Rectangle2D plotArea,
              Rectangle2D dataArea,
              boolean firstGridBandIsDark,
              List ticks) {

            boolean currentGridBandIsDark = firstGridBandIsDark;
            double yy = dataArea.getY();
            double xx1, xx2;

            // gets the outline stroke width of the plot
            double outlineStrokeWidth;
            if (getPlot().getOutlineStroke() != null) {
              outlineStrokeWidth = ((BasicStroke) getPlot().getOutlineStroke()).getLineWidth();
            } else {
              outlineStrokeWidth = 1d;
            }

            Iterator iterator = ticks.iterator();
            ValueTick tick;
            Rectangle2D band;
            while (iterator.hasNext()) {
              tick = (ValueTick) iterator.next();

              int index = (int) tick.getValue();

              xx1 = valueToJava2D(tick.getValue() - 0.5d, dataArea, RectangleEdge.BOTTOM);
              xx2 = valueToJava2D(tick.getValue() + 0.5d, dataArea, RectangleEdge.BOTTOM);

              currentGridBandIsDark = !currentGridBandIsDark;
              if (dets.length > 0) {
                LossDetector detector = dets[index];
                Set<String> normalization = typeSeries.get(detector.getType()).getNormalization();
                boolean limitVisibility = typeSeries.get(detector.getType()).isLimitVisible();
                if (limitVisibility) {
                  ScalarSignalValue limit =
                      (ScalarSignalValue) detector.getValue(param, normalization);
                  if (limit != null) {
                    ValueAxis ax = axes[index];
                    double losslimit = Math.abs(limit.getValue());
                    double zz = ax.valueToJava2D(losslimit, dataArea, RectangleEdge.RIGHT);

                    g2.setPaint(Color.red);
                    Rectangle2D.Double zRect =
                        new Rectangle2D.Double(
                            xx1, yy + outlineStrokeWidth, xx2 - xx1, zz - yy - outlineStrokeWidth);
                    g2.fill(zRect);
                  }
                }

                if (STATUS_DECORATION) {
                  int status = detectors.get((int) tick.getValue()).getStatus();
                  Color color = statusColors.get(status);
                  if (color == null || status == LossDetector.STATUS_OK) {
                  } else {
                    g2.setPaint(color);
                    double x, y, w, h;
                    x = xx1;
                    y = yy + outlineStrokeWidth;
                    w = xx2 - xx1;
                    h = 10;
                    band = new Rectangle2D.Double(x, y, w, 10);
                    g2.fill(band);
                  }
                }
              }
            }
            g2.setPaintMode();
          }

          protected AxisState drawTickMarksAndLabels(
              Graphics2D g2,
              double cursor,
              Rectangle2D plotArea,
              Rectangle2D dataArea,
              RectangleEdge edge) {

            AxisState state = new AxisState(cursor);

            if (isAxisLineVisible()) {
              drawAxisLine(g2, cursor, dataArea, edge);
            }

            double ol = getTickMarkOutsideLength();
            double il = getTickMarkInsideLength();

            List ticks = refreshTicks(g2, state, dataArea, edge);
            state.setTicks(ticks);
            g2.setFont(getTickLabelFont());
            Iterator iterator = ticks.iterator();
            while (iterator.hasNext()) {
              ValueTick tick = (ValueTick) iterator.next();
              if (isTickLabelsVisible()) {
                /// status decorations

                if (STATUS_DECORATION && dets.length > 0) {
                  int status = detectors.get((int) tick.getValue()).getStatus();
                  Color color = statusColors.get(status);
                  if (color == null) {
                    g2.setPaint(getTickLabelPaint());
                  } else {
                    g2.setPaint(color);
                  }
                } else {
                  g2.setPaint(getTickLabelPaint());
                }

                float[] anchorPoint = calculateAnchorPoint(tick, cursor, dataArea, edge);

                TextUtilities.drawRotatedString(
                    tick.getText(),
                    g2,
                    anchorPoint[0],
                    anchorPoint[1],
                    tick.getTextAnchor(),
                    tick.getAngle(),
                    tick.getRotationAnchor());
              }

              if (isTickMarksVisible()) {
                float xx = (float) valueToJava2D(tick.getValue(), dataArea, edge);
                Line2D mark = null;
                g2.setStroke(getTickMarkStroke());
                g2.setPaint(getTickMarkPaint());
                if (edge == RectangleEdge.LEFT) {
                  mark = new Line2D.Double(cursor - ol, xx, cursor + il, xx);
                } else if (edge == RectangleEdge.RIGHT) {
                  mark = new Line2D.Double(cursor + ol, xx, cursor - il, xx);
                } else if (edge == RectangleEdge.TOP) {
                  mark = new Line2D.Double(xx, cursor - ol, xx, cursor + il);
                } else if (edge == RectangleEdge.BOTTOM) {
                  mark = new Line2D.Double(xx, cursor + ol, xx, cursor - il);
                }
                g2.draw(mark);
              }
            }

            // need to work out the space used by the tick labels...
            // so we can update the cursor...
            double used = 0.0;
            if (isTickLabelsVisible()) {
              if (edge == RectangleEdge.LEFT) {
                used += findMaximumTickLabelWidth(ticks, g2, plotArea, isVerticalTickLabels());
                state.cursorLeft(used);
              } else if (edge == RectangleEdge.RIGHT) {
                used = findMaximumTickLabelWidth(ticks, g2, plotArea, isVerticalTickLabels());
                state.cursorRight(used);
              } else if (edge == RectangleEdge.TOP) {
                used = findMaximumTickLabelHeight(ticks, g2, plotArea, isVerticalTickLabels());
                state.cursorUp(used);
              } else if (edge == RectangleEdge.BOTTOM) {
                used = findMaximumTickLabelHeight(ticks, g2, plotArea, isVerticalTickLabels());
                state.cursorDown(used);
              }
            }

            return state;
          }
        };
    //	axis.setTickLabelFont(axis.getTickLabelFont().deriveFont(Font.BOLD, 14.0f));
    return axis;
  }
Exemple #4
0
  static {
    points = Collections.synchronizedList(new ArrayList<DrawObject>());
    lines = Collections.synchronizedList(new ArrayList<DrawObject>());
    ovals = Collections.synchronizedList(new ArrayList<DrawObject>());
    rectangles = Collections.synchronizedList(new ArrayList<DrawObject>());
    images = Collections.synchronizedList(new ArrayList<DrawObject>());
    labels = Collections.synchronizedList(new ArrayList<DrawObject>());
    scribbles = Collections.synchronizedList(new ArrayList<DrawObject>());
    eqnLines = Collections.synchronizedList(new ArrayList<DrawObject>());

    animPoints = Collections.synchronizedList(new ArrayList<DrawObject>());
    animLines = Collections.synchronizedList(new ArrayList<DrawObject>());
    animOvals = Collections.synchronizedList(new ArrayList<DrawObject>());
    animRectangles = Collections.synchronizedList(new ArrayList<DrawObject>());
    animLabels = Collections.synchronizedList(new ArrayList<DrawObject>());

    df.setMaximumFractionDigits(4);
  }
 /** Ensures that the children are sorted in z-order sequence. */
 private void ensureSorted() {
   if (needsSorting) {
     Collections.sort(children, FigureLayerComparator.INSTANCE);
     needsSorting = false;
   }
 }
 @Override
 public java.util.List<Figure> getChildren() {
   return Collections.unmodifiableList(children);
 }
/**
 * AbstractConnectionHandle factors the common code for handles that can be used to change the
 * connection of a ConnectionFigure.
 *
 * <p>XXX - Undo/Redo is not implemented yet.
 *
 * @author Werner Randelshofer
 * @version 3.0 2007-05-18 Changed due to changes in the canConnect methods of the ConnectionFigure
 *     interface. Shortened the name from AbstractChangeConnectionHandle to
 *     AbstractConnectionHandle. <br>
 *     2.1 2006-02-16 Remove savedLiner from connection while tracking. <br>
 *     2.0 2006-01-14 Changed to support double coordinates. <br>
 *     1.0 2003-12-01 Derived from JHotDraw 5.4b1.
 */
public abstract class AbstractConnectionHandle extends AbstractHandle {
  private Connector savedTarget;
  private Connector connectableConnector;
  private Figure connectableFigure;
  private Point start;
  /**
   * We temporarily remove the Liner from the connection figure, while the handle is being moved. We
   * store the Liner here, and add it back when the user has finished the interaction.
   */
  private Liner savedLiner;
  /** All connectors of the connectable Figure. */
  protected Collection<Connector> connectors = Collections.emptyList();

  /** Initializes the change connection handle. */
  protected AbstractConnectionHandle(ConnectionFigure owner) {
    super(owner);
  }

  public ConnectionFigure getOwner() {
    return (ConnectionFigure) super.getOwner();
  }

  public boolean isCombinableWith(Handle handle) {
    return false;
  }
  /** Returns the connector of the change. */
  protected abstract Connector getTarget();

  /** Disconnects the connection. */
  protected abstract void disconnect();

  /** Connect the connection with the given figure. */
  protected abstract void connect(Connector c);

  /** Sets the location of the connectableConnector point. */
  protected abstract void setLocation(Point2D.Double p);
  /** Returns the start point of the connection. */
  protected abstract Point2D.Double getLocation();

  /** Gets the side of the connection that is unaffected by the change. */
  protected Connector getSource() {
    if (getTarget() == getOwner().getStartConnector()) {
      return getOwner().getEndConnector();
    }
    return getOwner().getStartConnector();
  }

  /** Disconnects the connection. */
  public void trackStart(Point anchor, int modifiersEx) {
    savedTarget = getTarget();
    start = anchor;
    savedLiner = getOwner().getLiner();
    getOwner().setLiner(null);
    // disconnect();
    fireHandleRequestSecondaryHandles();
  }

  /** Finds a new connectableConnector of the connection. */
  public void trackStep(Point anchor, Point lead, int modifiersEx) {
    Point2D.Double p = view.viewToDrawing(lead);
    view.getConstrainer().constrainPoint(p);
    connectableFigure = findConnectableFigure(p, view.getDrawing());
    if (connectableFigure != null) {
      Connector aTarget = findConnectionTarget(p, view.getDrawing());
      if (aTarget != null) {
        p = aTarget.getAnchor();
      }
    }
    getOwner().willChange();
    setLocation(p);
    getOwner().changed();
    repaintConnectors();
  }

  /**
   * Connects the figure to the new connectableConnector. If there is no new connectableConnector
   * the connection reverts to its original one.
   */
  public void trackEnd(Point anchor, Point lead, int modifiersEx) {
    ConnectionFigure f = getOwner();
    // Change node type
    if ((modifiersEx
                & (InputEvent.META_DOWN_MASK
                    | InputEvent.CTRL_DOWN_MASK
                    | InputEvent.ALT_DOWN_MASK
                    | InputEvent.SHIFT_DOWN_MASK))
            != 0
        && (modifiersEx & InputEvent.BUTTON2_DOWN_MASK) == 0) {
      f.willChange();
      int index = getBezierNodeIndex();
      BezierPath.Node v = f.getNode(index);
      if (index > 0 && index < f.getNodeCount()) {
        v.mask = (v.mask + 3) % 4;
      } else if (index == 0) {
        v.mask = ((v.mask & BezierPath.C2_MASK) == 0) ? BezierPath.C2_MASK : 0;
      } else {
        v.mask = ((v.mask & BezierPath.C1_MASK) == 0) ? BezierPath.C1_MASK : 0;
      }
      f.setNode(index, v);
      f.changed();
      fireHandleRequestSecondaryHandles();
    }

    Point2D.Double p = view.viewToDrawing(lead);
    view.getConstrainer().constrainPoint(p);
    Connector target = findConnectionTarget(p, view.getDrawing());
    if (target == null) {
      target = savedTarget;
    }

    setLocation(p);
    if (target != savedTarget) {
      disconnect();
      connect(target);
    }
    getOwner().setLiner(savedLiner);
    getOwner().updateConnection();
    connectableConnector = null;
    connectors = Collections.emptyList();
  }

  private Connector findConnectionTarget(Point2D.Double p, Drawing drawing) {
    Figure targetFigure = findConnectableFigure(p, drawing);

    if (getSource() == null && targetFigure != null) {
      return findConnector(p, targetFigure, getOwner());
    } else if (targetFigure != null) {
      Connector target = findConnector(p, targetFigure, getOwner());
      if ((targetFigure != null)
          && targetFigure.canConnect()
          && targetFigure != savedTarget
          && !targetFigure.includes(getOwner())
          && (canConnect(getSource(), target)
              || getTarget() != null && getTarget().getOwner() == targetFigure)) {
        return target;
      }
    }
    return null;
  }

  protected abstract boolean canConnect(Connector existingEnd, Connector targetEnd);

  protected Connector findConnector(Point2D.Double p, Figure f, ConnectionFigure prototype) {
    return f.findConnector(p, prototype);
  }

  /** Draws this handle. */
  public void draw(Graphics2D g) {
    Graphics2D gg = (Graphics2D) g.create();
    gg.transform(view.getDrawingToViewTransform());
    for (Connector c : connectors) {
      c.draw(gg);
    }

    gg.dispose();
    drawCircle(g, (getTarget() == null) ? Color.red : Color.green, Color.black);
  }

  private Figure findConnectableFigure(Point2D.Double p, Drawing drawing) {
    for (Figure f : drawing.getFiguresFrontToBack()) {
      if (!f.includes(getOwner()) && f.canConnect() && f.contains(p)) {
        return f;
      }
    }
    return null;
  }

  protected void setPotentialTarget(Connector newTarget) {
    this.connectableConnector = newTarget;
  }

  protected Rectangle basicGetBounds() {
    // if (connection.getPointCount() == 0) return new Rectangle(0, 0, getHandlesize(),
    // getHandlesize());
    Point center = view.drawingToView(getLocation());
    return new Rectangle(
        center.x - getHandlesize() / 2,
        center.y - getHandlesize() / 2,
        getHandlesize(),
        getHandlesize());
  }

  protected BezierFigure getBezierFigure() {
    return (BezierFigure) getOwner();
  }

  protected abstract int getBezierNodeIndex();

  @Override
  public final Collection<Handle> createSecondaryHandles() {
    LinkedList<Handle> list = new LinkedList<Handle>();
    if (((ConnectionFigure) getOwner()).getLiner() == null && savedLiner == null) {
      int index = getBezierNodeIndex();
      BezierFigure f = getBezierFigure();
      BezierPath.Node v = f.getNode(index);
      if ((v.mask & BezierPath.C1_MASK) != 0 && (index != 0 || f.isClosed())) {
        list.add(new BezierControlPointHandle(f, index, 1));
      }
      if ((v.mask & BezierPath.C2_MASK) != 0 && (index < f.getNodeCount() - 1 || f.isClosed())) {
        list.add(new BezierControlPointHandle(f, index, 2));
      }
      if (index > 0 || f.isClosed()) {
        int i = (index == 0) ? f.getNodeCount() - 1 : index - 1;
        v = f.getNode(i);
        if ((v.mask & BezierPath.C2_MASK) != 0) {
          list.add(new BezierControlPointHandle(f, i, 2));
        }
      }
      if (index < f.getNodeCount() - 2 || f.isClosed()) {
        int i = (index == f.getNodeCount() - 1) ? 0 : index + 1;
        v = f.getNode(i);
        if ((v.mask & BezierPath.C1_MASK) != 0) {
          list.add(new BezierControlPointHandle(f, i, 1));
        }
      }
    }
    return list;
  }

  protected BezierPath.Node getBezierNode() {
    int index = getBezierNodeIndex();
    return getBezierFigure().getNodeCount() > index ? getBezierFigure().getNode(index) : null;
  }

  public String getToolTipText(Point p) {
    ConnectionFigure f = (ConnectionFigure) getOwner();
    if (f.getLiner() == null && savedLiner == null) {
      ResourceBundleUtil labels = ResourceBundleUtil.getLAFBundle("org.jhotdraw.draw.Labels");
      BezierPath.Node node = getBezierNode();
      return (node == null)
          ? null
          : labels.getFormatted(
              "bezierNodeHandle.tip",
              labels.getFormatted(
                  (node.getMask() == 0)
                      ? "bezierNode.linearNode"
                      : ((node.getMask() == BezierPath.C1C2_MASK)
                          ? "bezierNode.cubicNode"
                          : "bezierNode.quadraticNode")));
    } else {
      return null;
    }
  }
  /**
   * Updates the list of connectors that we draw when the user moves or drags the mouse over a
   * figure to which can connect.
   */
  public void repaintConnectors() {
    Rectangle2D.Double invalidArea = null;
    for (Connector c : connectors) {
      if (invalidArea == null) {
        invalidArea = c.getDrawingArea();
      } else {
        invalidArea.add(c.getDrawingArea());
      }
    }
    connectors =
        (connectableFigure == null)
            ? new java.util.LinkedList<Connector>()
            : connectableFigure.getConnectors(getOwner());
    for (Connector c : connectors) {
      if (invalidArea == null) {
        invalidArea = c.getDrawingArea();
      } else {
        invalidArea.add(c.getDrawingArea());
      }
    }
    if (invalidArea != null) {
      view.getComponent().repaint(view.drawingToView(invalidArea));
    }
  }
}
Exemple #8
0
  // public Point2D.Double localize(double odo_x, double odo_y, double odo_theta, List<Fiducial>
  // fiducials){
  public Point2D.Double localize(double odo_x, double odo_y, List<Fiducial> fiducials) {
    double x_disp = odo_x - prev_odo_x;
    double y_disp = odo_y - prev_odo_y;

    prev_odo_x = odo_x;
    prev_odo_y = odo_y;

    // double theta_disp = odo_theta - prev_odo_theta;
    // prev_odo_theta = odo_theta;

    List<Point2D.Double> new_particles = new ArrayList<Point2D.Double>();
    List<Double> new_probs = new ArrayList<Double>();
    List<Double> cumulative_probs = new ArrayList<Double>();

    Point2D.Double curr_point;
    double curr_prob, distance, mapx, mapy;
    double probsum = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      // Motion Update
      curr_point = new_particles.get(i);
      new_particles.add(new Point2D.Double(curr_point.x + x_disp, curr_point.y + y_disp));

      // Sensor Update
      curr_prob = 1;
      for (Fiducial fid : fiducials) {
        mapx = fiducialPairs.get(fid.colors).x;
        mapy = fiducialPairs.get(fid.colors).y;

        distance =
            Math.sqrt(
                (curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
                    + (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));

        curr_prob *=
            (Math.exp(-0.5 * (fid.distance - distance) * (fid.distance - distance) / SIGMA)
                / Math.sqrt(2 * Math.PI * SIGMA * SIGMA));
      }

      probsum += curr_prob;
      new_probs.add(curr_prob);
      cumulative_probs.add(probsum);
    }

    particles.clear();
    double rand_val;
    int index;
    int max_index = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      rand_val = probsum * Math.random();
      index = Math.abs(Collections.binarySearch(cumulative_probs, rand_val));

      particles.add(new_particles.get(index));

      if (new_probs.get(max_index) < new_probs.get(i)) {
        max_index = i;
      }
    }

    localPoint = new_particles.get(max_index);

    return localPoint;
  }
  public OdoPoint localize(
      double odo_x, double odo_y, double odo_theta, List<FiducialBlob> fiducials) {
    double x_disp = odo_x - prev_odo_x;
    double y_disp = odo_y - prev_odo_y;
    double theta_disp = odo_theta - prev_odo_theta;

    prev_odo_x = odo_x;
    prev_odo_y = odo_y;
    prev_odo_theta = odo_theta;

    localPoint =
        new OdoPoint(localPoint.x + x_disp, localPoint.y + y_disp, localPoint.theta + theta_disp);

    List<FiducialBlob> revFiducials = new ArrayList<FiducialBlob>();
    double mapx, mapy;
    Color[] colors = new Color[2];
    Fiducial mapFid;

    double mapbearing;
    double localAngle = localPoint.theta;
    localAngle = localAngle - ((int) (localAngle / (2 * Math.PI)));
    if (localAngle > Math.PI) {
      localAngle -= 2 * Math.PI;
    }

    List<OdoPoint> new_particles = new ArrayList<OdoPoint>();
    List<Double> new_probs = new ArrayList<Double>();
    List<Double> cumulative_probs = new ArrayList<Double>();

    OdoPoint curr_point;
    double curr_prob, distance, fid_distance, fid_bearing;
    double probsum = 0;

    // Filter out unreasonable fiducials
    for (FiducialBlob fid : fiducials) {
      colors[0] = fid.colors[0];
      colors[1] = fid.colors[1];

      if (findFiducial(colors[0], colors[1]) != null) {
        mapFid = findFiducial(colors[0], colors[1]);
        mapx = mapFid.getPosition().x;
        mapy = mapFid.getPosition().y;

        mapbearing = Math.atan2(mapy - localPoint.y, mapx - localPoint.x);

        distance =
            Math.sqrt(
                (localPoint.x - mapx) * (localPoint.x - mapx)
                    + (localPoint.y - mapy) * (localPoint.y - mapy));

        fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());

        System.out.println(
            "Localization: map and camera bearing = " + mapbearing + ", " + localAngle);
        System.out.println(
            "Localization: map and camera distance = " + distance + ", " + fid_distance);

        if ((Math.abs(mapbearing - localAngle) < Math.PI / 6
                || Math.abs(mapbearing - localAngle + 2 * Math.PI) < Math.PI / 6
                || Math.abs(mapbearing - localAngle - 2 * Math.PI) < Math.PI / 6)
            && fid_distance < distance * 1.3
            && distance < fid_distance * 1.3) {

          System.out.println("Localization: added fiducial");
          revFiducials.add(fid);
        } else {
          System.out.println("Localization: eliminated fiducial from bearing and distance");
        }
      } else {
        System.out.println("Localization: eliminated fiducial from colors");
      }
    }

    /*// Use Odometry if no fiducials
    if (revFiducials.size() == 0){
    	System.out.println("Localization: no fiducials to localize with");
    	particles.clear();
    	OdoPoint curr_point;
    	for (int i = 0; i < NUM_PARTICLES; i++){
    		curr_point = particles.get(i);
    		particles.add(new OdoPoint(curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
    	}
    	return localPoint;
    }*/

    for (int i = 0; i < NUM_PARTICLES; i++) {
      // Motion Update
      curr_point = particles.get(i);
      new_particles.add(
          new OdoPoint(
              curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));

      // Sensor Update
      curr_prob = 1;
      for (FiducialBlob fid : revFiducials) {
        colors[0] = fid.colors[0];
        colors[1] = fid.colors[1];

        mapFid = findFiducial(colors[0], colors[1]);
        mapx = mapFid.getPosition().x;
        mapy = mapFid.getPosition().y;

        distance =
            Math.sqrt(
                (curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
                    + (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));

        fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());

        curr_prob *=
            NORMALIZATION
                * (Math.exp(
                        -0.5 * (fid_distance - distance) * (fid_distance - distance) / DIST_SIGMA)
                    / Math.sqrt(2 * Math.PI * DIST_SIGMA * DIST_SIGMA));

        fid_bearing = curr_point.theta + theta_disp - ImageBlob.computeBearing(fid.x);
        fid_bearing = fid_bearing - ((int) (fid_bearing / (2 * Math.PI)));
        if (fid_bearing > Math.PI) {
          fid_bearing -= 2 * Math.PI;
        }
        mapbearing = Math.atan2(mapy - curr_point.y - y_disp, mapx - curr_point.x - x_disp);

        curr_prob *=
            NORMALIZATION
                * (Math.exp(
                        -0.5 * (mapbearing - fid_bearing) * (mapbearing - fid_bearing) / ROT_SIGMA)
                    / Math.sqrt(2 * Math.PI * ROT_SIGMA * ROT_SIGMA));
      }

      probsum += curr_prob;
      new_probs.add(curr_prob);
      cumulative_probs.add(probsum);
    }

    if (revFiducials.size() > 0) {
      System.out.println("Localization: localizing with fiducials");
      particles.clear();
      double rand_val;
      int index;
      int max_index = 0;
      for (int i = 0; i < NUM_PARTICLES; i++) {
        rand_val = probsum * Math.random();
        index = Collections.binarySearch(cumulative_probs, rand_val);
        if (index < 0) {
          index = -(index + 1);
        }

        particles.add(new_particles.get(index));

        if (new_probs.get(max_index) < new_probs.get(i)) {
          max_index = i;
        }
      }

      localPoint = new_particles.get(max_index);
    } else {
      System.out.println("Localization: no fiducials to localize with");
      particles = new_particles;
    }

    return localPoint;
  }
 long finishBuild() {
   Collections.sort(graph_objects, new HistoryCompare());
   return last_time;
 }