public Rectangle2D.Double calculateLayout(
CompositeFigure compositeFigure, Point2D.Double anchor, Point2D.Double lead) {
Rectangle2D.Double bounds = null;
for (Figure child : compositeFigure.getChildren()) {
Locator locator = getLocator(child);
Rectangle2D.Double r;
if (locator == null) {
r = child.getBounds();
} else {
Point2D.Double p = locator.locate(compositeFigure);
Dimension2DDouble d = child.getPreferredSize();
r = new Rectangle2D.Double(p.x, p.y, d.width, d.height);
}
if (!r.isEmpty()) {
if (bounds == null) {
bounds = r;
} else {
bounds.add(r);
}
}
}
return (bounds == null) ? new Rectangle2D.Double() : bounds;
}
public AngleLocalization(
List<PolygonObstacle> cSpaceObstacles,
Rectangle2D.Double cSpaceWorld,
Point2D.Double robotStart,
Fiducial[] fiducialPairs) {
this.cSpaceObstacles = cSpaceObstacles;
this.cSpaceWorld = cSpaceWorld;
this.robotStart = robotStart;
this.fiducialPairs = fiducialPairs;
particles = new ArrayList<OdoPoint>();
probabilities = new ArrayList<Double>();
double delta_x = 0;
double delta_y = 0;
double delta_theta = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (1 - 2 * Math.random());
delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (1 - 2 * Math.random());
delta_theta = 2 * Math.PI * INITIAL_NOISE * (1 - 2 * Math.random());
particles.add(new OdoPoint(robotStart.x + delta_x, robotStart.y + delta_y, delta_theta));
probabilities.add(1.0 / NUM_PARTICLES);
}
prev_odo_x = robotStart.x;
prev_odo_y = robotStart.y;
prev_odo_theta = 0;
localPoint = new OdoPoint(robotStart.x, robotStart.y, 0);
}
public Rectangle2D.Double getBounds() {
if (cachedBounds == null) {
cachedBounds = new Rectangle2D.Double();
cachedBounds.setRect(getTextShape().getBounds2D());
}
return (Rectangle2D.Double) cachedBounds.clone();
}
// robot reference point is the origin
public CSpace(Polygon robot, PolygonMap map) {
Rectangle2D.Double worldRect = map.getWorldRect();
constructor(
robot, worldRect.getMinX(), worldRect.getMinY(), worldRect.getMaxX(), worldRect.getMaxY());
for (PolygonObstacle obstacle : map.getObstacles()) {
addObstacle(obstacle);
}
}
private void writeTextAreaElement(IXMLElement parent, SVGTextAreaFigure f) throws IOException {
IXMLElement elem = parent.createElement("AREA");
Rectangle2D.Double rect = f.getBounds();
double grow = getPerpendicularHitGrowth(f);
rect.x -= grow;
rect.y -= grow;
rect.width += grow;
rect.height += grow;
if (writeRectAttributes(elem, f, rect)) {
parent.addChild(elem);
}
}
public Rectangle2D.Double layout(
CompositeFigure compositeFigure, Point2D.Double anchor, Point2D.Double lead) {
Rectangle2D.Double bounds = null;
for (Figure child : compositeFigure.getChildren()) {
Locator locator = getLocator(child);
Rectangle2D.Double r;
if (locator == null) {
r = child.getBounds();
} else {
Point2D.Double p = locator.locate(compositeFigure, child);
Dimension2DDouble d = child.getPreferredSize();
r = new Rectangle2D.Double(p.x, p.y, d.width, d.height);
}
child.willChange();
child.setBounds(
new Point2D.Double(r.getMinX(), r.getMinY()),
new Point2D.Double(r.getMaxX(), r.getMaxY()));
child.changed();
if (!r.isEmpty()) {
if (bounds == null) {
bounds = r;
} else {
bounds.add(r);
}
}
}
return (bounds == null) ? new Rectangle2D.Double() : bounds;
}
public void testOutcode() {
assertEquals(Rectangle2D.OUT_LEFT | Rectangle2D.OUT_TOP, r.outcode(0, 0));
assertEquals(Rectangle2D.OUT_TOP, r.outcode(2, 0));
assertEquals(Rectangle2D.OUT_RIGHT | Rectangle2D.OUT_TOP, r.outcode(5, 0));
assertEquals(Rectangle2D.OUT_RIGHT, r.outcode(5, 3));
assertEquals(Rectangle2D.OUT_RIGHT | Rectangle2D.OUT_BOTTOM, r.outcode(5, 7));
assertEquals(Rectangle2D.OUT_BOTTOM, r.outcode(2, 7));
assertEquals(Rectangle2D.OUT_LEFT | Rectangle2D.OUT_BOTTOM, r.outcode(0, 7));
assertEquals(Rectangle2D.OUT_LEFT, r.outcode(0, 3));
assertEquals(0, r.outcode(2, 3));
}
public void testIsEmpty() {
assertFalse(r.isEmpty());
assertTrue(new Rectangle2D.Double(1, 2, -1, 1).isEmpty());
assertTrue(new Rectangle2D.Double(3, 4, 1, -1).isEmpty());
assertTrue(new Rectangle2D.Double(3, 4, 1, 0).isEmpty());
assertTrue(new Rectangle2D.Double(5, 6, -1, -1).isEmpty());
}
@Override
public Rectangle2D.Double getDrawingArea() {
Rectangle2D.Double r = super.getDrawingArea();
if (getNodeCount() > 1) {
if (get(START_DECORATION) != null) {
Point2D.Double p1 = getPoint(0, 0);
Point2D.Double p2 = getPoint(1, 0);
r.add(get(START_DECORATION).getDrawingArea(this, p1, p2));
}
if (get(END_DECORATION) != null) {
Point2D.Double p1 = getPoint(getNodeCount() - 1, 0);
Point2D.Double p2 = getPoint(getNodeCount() - 2, 0);
r.add(get(END_DECORATION).getDrawingArea(this, p1, p2));
}
}
return r;
}
public ArrayList<Vertex> getVertecesInArea(Rectangle2D.Double area, byte dim1, byte dim2) {
ArrayList<Vertex> temp = new ArrayList<Vertex>();
for (Vertex ver : vertex) {
// Point2D.Double p = new Point(ver.getCoords(dim1),ver.getCoords(dim2))
if (area.contains(ver.getCoord(dim1), ver.getCoord(dim2))) {
temp.add(ver);
}
}
return temp;
}
@Override
public Rectangle2D.Double getDrawingArea() {
if (cachedDrawingArea == null) {
Rectangle2D rx = getBounds();
Rectangle2D.Double r =
(rx instanceof Rectangle2D.Double)
? (Rectangle2D.Double) rx
: new Rectangle2D.Double(rx.getX(), rx.getY(), rx.getWidth(), rx.getHeight());
double g = SVGAttributeKeys.getPerpendicularHitGrowth(this);
Geom.grow(r, g, g);
if (TRANSFORM.get(this) == null) {
cachedDrawingArea = r;
} else {
cachedDrawingArea = new Rectangle2D.Double();
cachedDrawingArea.setRect(TRANSFORM.get(this).createTransformedShape(r).getBounds2D());
}
}
return (Rectangle2D.Double) cachedDrawingArea.clone();
}
public void testCreateUnion() {
// assertEquals(r.createUnion(new Rectangle2D.Double()), new Rectangle2D.Double(1, 2, 3,
// 4)); // Empty
assertEquals(
new Rectangle2D.Float(1, 2, 3, 4),
r.createUnion(new Rectangle2D.Float(1, 2, 3, 4))); // The same
assertEquals(
new Rectangle2D.Double(1, 2, 3, 4),
r.createUnion(new Rectangle2D.Double(2, 3, 1, 2))); // Inside
assertEquals(
new Rectangle2D.Float(1, 2, 5, 7),
r.createUnion(new Rectangle2D.Float(5, 7, 1, 2))); // Outside
assertEquals(
new Rectangle2D.Double(1, 2, 6, 7),
r.createUnion(new Rectangle2D.Double(2, 3, 5, 6))); // Intersect
assertEquals(
new Rectangle2D.Float(0, 0, 5, 6),
r.createUnion(new Rectangle2D.Float(0, 0, 5, 6))); // Cover
}
@Override
public void draw(Graphics2D g) {
double opacity = get(OPACITY);
opacity = Math.min(Math.max(0d, opacity), 1d);
if (opacity != 0d) {
if (opacity != 1d) {
Rectangle2D.Double drawingArea = getDrawingArea();
Rectangle2D clipBounds = g.getClipBounds();
if (clipBounds != null) {
Rectangle2D.intersect(drawingArea, clipBounds, drawingArea);
}
if (!drawingArea.isEmpty()) {
BufferedImage buf =
new BufferedImage(
(int) ((2 + drawingArea.width) * g.getTransform().getScaleX()),
(int) ((2 + drawingArea.height) * g.getTransform().getScaleY()),
BufferedImage.TYPE_INT_ARGB);
Graphics2D gr = buf.createGraphics();
gr.scale(g.getTransform().getScaleX(), g.getTransform().getScaleY());
gr.translate((int) -drawingArea.x, (int) -drawingArea.y);
gr.setRenderingHints(g.getRenderingHints());
drawFigure(gr);
gr.dispose();
Composite savedComposite = g.getComposite();
g.setComposite(AlphaComposite.getInstance(AlphaComposite.SRC_OVER, (float) opacity));
g.drawImage(
buf,
(int) drawingArea.x,
(int) drawingArea.y,
2 + (int) drawingArea.width,
2 + (int) drawingArea.height,
null);
g.setComposite(savedComposite);
}
} else {
drawFigure(g);
}
}
}
public ArrayList<TVertex> getTVertecesInArea(Rectangle2D.Double area, int layerId) {
ArrayList<TVertex> temp = new ArrayList<TVertex>();
for (int i = 0; i < vertex.size(); i++) {
TVertex ver = vertex.get(i).getTVertex(layerId);
// Point2D.Double p = new Point(ver.getCoords(dim1),ver.getCoords(dim2))
if (area.contains(ver.getX(), ver.getY())) {
temp.add(ver);
}
}
return temp;
}
/** Gets the drawing area without taking the decorator into account. */
@Override
protected Rectangle2D.Double getFigureDrawingArea() {
if (getText() == null) {
return getBounds();
} else {
TextLayout layout = getTextLayout();
Rectangle2D.Double r =
new Rectangle2D.Double(origin.x, origin.y, layout.getAdvance(), layout.getAscent());
Rectangle2D lBounds = layout.getBounds();
if (!lBounds.isEmpty() && !Double.isNaN(lBounds.getX())) {
r.add(
new Rectangle2D.Double(
lBounds.getX() + origin.x,
(lBounds.getY() + origin.y + layout.getAscent()),
lBounds.getWidth(),
lBounds.getHeight()));
}
// grow by two pixels to take antialiasing into account
Geom.grow(r, 2d, 2d);
return r;
}
}
public java.util.List<Figure> findFiguresWithin(Rectangle2D.Double bounds) {
LinkedList<Figure> contained = new LinkedList<Figure>();
for (Figure f : children) {
Rectangle2D r = f.getBounds();
if (AttributeKeys.TRANSFORM.get(f) != null) {
r = AttributeKeys.TRANSFORM.get(f).createTransformedShape(r).getBounds2D();
}
if (f.isVisible() && bounds.contains(r)) {
contained.add(f);
}
}
return contained;
}
/**
* All other write methods delegate their work to here.
*/
public void write(OutputStream out, java.util.List<Figure> figures) throws IOException {
Rectangle2D.Double drawingRect = null;
for (Figure f : figures) {
if (drawingRect == null) {
drawingRect = f.getBounds();
} else {
drawingRect.add(f.getBounds());
}
}
AffineTransform drawingTransform = new AffineTransform();
drawingTransform.translate(
-Math.min(0, drawingRect.x),
-Math.min(0, drawingRect.y)
);
write(out, figures, drawingTransform,
new Dimension(
(int) (Math.abs(drawingRect.x) + drawingRect.width),
(int) (Math.abs(drawingRect.y) + drawingRect.height))
);
}
public Localization(
List<PolygonObstacle> cSpaceObstacles,
Rectangle2D.Double cSpaceWorld,
Point2D.Double robotStart,
Map<int[], Point2D.Double> fiducialPairs) {
this.cSpaceObstacles = cSpaceObstacles;
this.cSpaceWorld = cSpaceWorld;
this.robotStart = robotStart;
this.fiducialPairs = fiducialPairs;
particles = new ArrayList<Point2D.Double>();
probabilities = new ArrayList<Double>();
double delta_x = 0;
double delta_y = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (2 * Math.random() - 1);
delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (2 * Math.random() - 1);
particles.add(new Point2D.Double(robotStart.x + delta_x, robotStart.y + delta_y));
probabilities.add(1.0 / NUM_PARTICLES);
}
}
private void writeRectElement(IXMLElement parent, SVGRectFigure f) throws IOException {
IXMLElement elem = parent.createElement("AREA");
boolean isContained;
if (f.getArcHeight() == 0 && f.getArcWidth() == 0) {
Rectangle2D.Double rect = f.getBounds();
double grow = getPerpendicularHitGrowth(f);
rect.x -= grow;
rect.y -= grow;
rect.width += grow;
rect.height += grow;
isContained = writeRectAttributes(elem, f, rect);
} else {
isContained = writePolyAttributes(elem, f,
new GrowStroke((float) (getStrokeTotalWidth(f) / 2d), (float) getStrokeTotalWidth(f)).
createStrokedShape(new RoundRectangle2D.Double(
f.getX(), f.getY(), f.getWidth(), f.getHeight(),
f.getArcWidth(), f.getArcHeight()
)));
}
if (isContained) {
parent.addChild(elem);
}
}
/**
* 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));
}
}
@Override
public Rectangle2D.Double getDrawingArea() {
if (cachedDrawingArea == null) {
if (get(TRANSFORM) == null) {
cachedDrawingArea = path.getBounds2D();
} else {
BezierPath p2 = (BezierPath) path.clone();
p2.transform(get(TRANSFORM));
cachedDrawingArea = p2.getBounds2D();
}
double strokeTotalWidth = AttributeKeys.getStrokeTotalWidth(this);
double width = strokeTotalWidth / 2d;
if (get(STROKE_JOIN) == BasicStroke.JOIN_MITER) {
width *= get(STROKE_MITER_LIMIT);
} else if (get(STROKE_CAP) != BasicStroke.CAP_BUTT) {
width += strokeTotalWidth * 2;
}
Geom.grow(cachedDrawingArea, width, width);
}
return (Rectangle2D.Double) cachedDrawingArea.clone();
}
public void testCreateIntersection() {
assertEquals(
new Rectangle2D.Float(1, 2, -1, -2),
r.createIntersection(new Rectangle2D.Float())); // Empty
assertEquals(
new Rectangle2D.Double(1, 2, 3, 4),
r.createIntersection(new Rectangle2D.Double(1, 2, 3, 4))); // The same
assertEquals(
new Rectangle2D.Float(2, 3, 1, 2),
r.createIntersection(new Rectangle2D.Float(2, 3, 1, 2))); // Inside
assertEquals(
new Rectangle2D.Double(5, 7, -1, -1),
r.createIntersection(new Rectangle2D.Double(5, 7, 1, 2))); // Outside
assertEquals(
new Rectangle2D.Float(2, 3, 2, 3),
r.createIntersection(new Rectangle2D.Float(2, 3, 5, 6))); // Intersect
assertEquals(
new Rectangle2D.Double(1, 2, 3, 4),
r.createIntersection(new Rectangle2D.Double(0, 0, 5, 6))); // Cover
}
public CSpace(Polygon robot, Rectangle2D.Double worldRect) {
constructor(
robot, worldRect.getMinX(), worldRect.getMinY(), worldRect.getMaxX(), worldRect.getMaxY());
}
public void testGetY() {
assertEquals(2.0f, r.getY(), 0.0f);
}
public void testGetWidth() {
assertEquals(3.0f, r.getWidth(), 0.0f);
}
public void testGetHeight() {
assertEquals(4.0f, r.getHeight(), 0.0f);
}
public void testSetRect2() {
r.setRect(5.0f, 6.0f, 7.0f, 8.0f);
assertEquals(new Rectangle2D.Double(5, 6, 7, 8), r);
}
public void testGetX() {
assertEquals(1.0f, r.getX(), 0.0f);
}
public void testGetBounds2D() {
assertEquals(new Rectangle2D.Double(1, 2, 3, 4), r.getBounds2D());
}
public void testSetRect3() {
r.setRect(new Rectangle(5, 6, 7, 8));
assertEquals(new Rectangle2D.Double(5, 6, 7, 8), r);
}
