public void mouseDragged(MouseEvent e) {
int mods = e.getModifiersEx();
Point dragEnd = e.getPoint();
boolean shift = (mods & MouseEvent.SHIFT_DOWN_MASK) > 0;
boolean ctrl = (mods & MouseEvent.CTRL_DOWN_MASK) > 0;
boolean alt = shift & ctrl;
ctrl = ctrl & (!alt);
shift = shift & (!alt);
boolean nomods = !(shift | ctrl | alt);
if (dragBegin == null) dragBegin = dragEnd;
nodrag = false;
if ((mods & InputEvent.BUTTON3_DOWN_MASK) > 0 || true) {
double dx = dragEnd.getX() - dragBegin.getX();
double dy = dragEnd.getY() - dragBegin.getY();
synchronized (JImage.this) {
t.preConcatenate(AffineTransform.getTranslateInstance(dx, dy));
}
dragBegin = dragEnd;
repaint();
}
}
public Point componentToImage(Point p) {
float r = Math.min(getWidth() / (float) width, getHeight() / (float) height);
int x = (int) (p.getX() / r);
int y = (int) (p.getY() / r);
if ((x > width - 1) || (y > height - 1)) return new Point(-1, -1);
return new Point(x, y);
}
/**
* Draw a string at a given point.
*
* <p>The method returns the point where the next string should be drawn to seamlessly continue
* the current string.
*
* @param s the string
* @param point the position where to draw the string
* @param fontname the font's name, should preferably be a logical name
* @param fontsize the font size
* @param color the color
* @return the point where outputting string text should continue
*/
public Point drawString(String s, Point point, String fontname, int fontsize, Color color) {
Graphics g = this.image.getGraphics();
g.setColor(color);
g.setFont(new Font(fontname, Font.PLAIN, fontsize));
g.drawString(s, point.x, point.y);
FontMetrics f = g.getFontMetrics();
Point newPoint = new Point(point);
newPoint.x += f.stringWidth(s);
this.repaint();
return (newPoint);
}
public void paint(Graphics g1) {
Graphics2D g = (Graphics2D) g1;
Component c;
Point p;
paintComponent(g);
for (int i = 0; i < getComponentCount(); i++) {
AffineTransform save = g.getTransform();
c = getComponent(i);
p = c.getLocation();
g.translate((int) p.getX(), (int) p.getY());
c.paint(g);
g.setTransform(save);
}
}
public void mousePressed(MouseEvent e) {
requestFocus();
Point p = e.getPoint();
int size =
Math.min(
MAX_SIZE,
Math.min(
getWidth() - imagePadding.left - imagePadding.right,
getHeight() - imagePadding.top - imagePadding.bottom));
p.translate(-(getWidth() / 2 - size / 2), -(getHeight() / 2 - size / 2));
if (mode == ColorPicker.BRI || mode == ColorPicker.SAT) {
// the two circular views:
double radius = ((double) size) / 2.0;
double x = p.getX() - size / 2.0;
double y = p.getY() - size / 2.0;
double r = Math.sqrt(x * x + y * y) / radius;
double theta = Math.atan2(y, x) / (Math.PI * 2.0);
if (r > 1) r = 1;
if (mode == ColorPicker.BRI) {
setHSB((float) (theta + .25f), (float) (r), bri);
} else {
setHSB((float) (theta + .25f), sat, (float) (r));
}
} else if (mode == ColorPicker.HUE) {
float s = ((float) p.x) / ((float) size);
float b = ((float) p.y) / ((float) size);
if (s < 0) s = 0;
if (s > 1) s = 1;
if (b < 0) b = 0;
if (b > 1) b = 1;
setHSB(hue, s, b);
} else {
int x2 = p.x * 255 / size;
int y2 = p.y * 255 / size;
if (x2 < 0) x2 = 0;
if (x2 > 255) x2 = 255;
if (y2 < 0) y2 = 0;
if (y2 > 255) y2 = 255;
if (mode == ColorPicker.RED) {
setRGB(red, x2, y2);
} else if (mode == ColorPicker.GREEN) {
setRGB(x2, green, y2);
} else {
setRGB(x2, y2, blue);
}
}
}
public void paint(Graphics g) {
Graphics2D g2 = (Graphics2D) g;
Dimension asz = this.getSize();
if (fullRefresh) {
g2.clearRect(0, 0, asz.width, asz.height);
fullRefresh = false;
}
g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
big.setColor(Color.black);
offset.x = (int) (asz.width - iw) / 2;
offset.y = (int) (asz.height - ih) / 2;
big.drawImage(img, 0, 0, this);
big.setPaint(Color.red);
if ((rect.width > 0) && (rect.height > 0)) big.draw(rect);
if (selected == 1) shadeExt(big, 0, 0, 0, 64);
else if (selected == 2) {
shadeExt(big, 0, 0, 0, 255);
selected = 1;
}
g2.drawImage(bi, offset.x, offset.y, this);
}
public void processPolygonSpline(boolean shiftDown) {
mCurrPC.setBatchModeOn();
if (!shiftDown) mCurrPC.unselectAll();
// loop on spline pts to construct a Polygon
Polygon srcArea = new Polygon();
for (int s = 0; s < mSplinePoints.size(); s++) {
Point p = (Point) mSplinePoints.elementAt(s);
int x = p.x - mLeft - 5;
int y = p.y - mTop - 5;
srcArea.addPoint(x, y);
}
mSplinePoints.removeAllElements();
// search for matches
byte[] currFilterResults = fdm.getResults();
double[] yArray1 = getYArray1();
double[] xArray1 = getXArray1();
double[] yArray2 = getYArray2();
double[] xArray2 = getXArray2();
Point p1 = new Point();
Point p2 = new Point();
for (int i = 0; i < mCurrPC.getSize(); i++) {
if (!mIgnoreFilter
&& (mCurrPC.isDeleted(i) || currFilterResults[i] != 4 || !mCurrPC.isSelectedLayer(i)))
continue;
double yi = yArray1[i];
double xi = xArray1[i];
double xi2 = Float.NaN;
double yi2 = Float.NaN;
double xx2 = Float.NaN;
double yy2 = Float.NaN;
// correct the X value if necessary
xi = correctX(xi);
double xx1 = (xi - winXOrigin) * winXScale;
double yy1 = (yi - winYOrigin) * winYScale;
// correct the Y coordinate if necessary
yy1 = correctY(yy1);
if (!isYAxisScaler()) {
yi2 = yArray2[i];
yy2 = (yi2 - winYOrigin) * winYScale;
;
yy2 = correctY(yy2);
} else yy2 = yy1;
if (!isXAxisScaler()) {
xi2 = xArray2[i];
xi2 = correctX(xi2);
xx2 = (xi2 - winXOrigin) * winXScale;
} else xx2 = xx1;
p1.setLocation((int) xx1, (int) yy1);
p2.setLocation((int) xx2, (int) yy2);
if (srcArea.contains(p1) || srcArea.contains(p2)) {
mCurrPC.select(i);
}
}
mViewManager.invalidateAllViews();
mCurrPC.setBatchModeOff();
}
public void rubberbandEnded(Rubberband rb, boolean shiftDown) {
// get the limits and zoom the plot
if (mCurrPC == null) return;
Rectangle rbRect = rb.getBounds();
if (rbRect.width == 0 || rbRect.height == 0) {
// click selection
if (toolMode == Constants.SELECT_MODE) {
// find any matches at the mouseclick location
int x = rbRect.x - mLeft - 5;
int y = rbRect.y - mTop - 5;
// construct a search region;
int[] xpoints = {x - 1, x + 1, x + 1, x - 1, x - 1};
int[] ypoints = {y - 1, y - 1, y + 1, y + 1, y - 1};
Polygon sr = new Polygon(xpoints, ypoints, 5);
if (!shiftDown) mCurrPC.unselectAll();
mCurrPC.setBatchModeOn();
byte[] currFilterResults = fdm.getResults();
double[] yArray1 = getYArray1();
double[] xArray1 = getXArray1();
double[] yArray2 = getYArray2();
double[] xArray2 = getXArray2();
for (int i = 0; i < mCurrPC.getSize(); i++) {
if (!mIgnoreFilter && (mCurrPC.isDeleted(i) || currFilterResults[i] != 4)
|| !mCurrPC.isSelectedLayer(i)) continue;
// search for matches
double yi = yArray1[i];
double xi = xArray1[i];
double xi2 = Float.NaN;
double yi2 = Float.NaN;
double x2 = Float.NaN;
double y2 = Float.NaN;
// correct the X value if necessary
xi = correctX(xi);
double x1 = (xi - winXOrigin) * winXScale;
double y1 = (yi - winYOrigin) * winYScale;
// correct the Y coordinate if necessary
y1 = correctY(y1);
if (!isYAxisScaler()) {
yi2 = yArray2[i];
y2 = (yi2 - winYOrigin) * winYScale;
y2 = correctY(y2);
} else y2 = y1;
if (!isXAxisScaler()) {
xi2 = xArray2[i];
xi2 = correctX(xi2);
x2 = (xi2 - winXOrigin) * winXScale;
} else x2 = x1;
if (sr.contains(new Point((int) x1, (int) y1))
|| sr.contains(new Point((int) x2, (int) y2))) {
mCurrPC.select(i);
}
}
mViewManager.invalidateAllViews();
mCurrPC.setBatchModeOff();
} else if (toolMode == Constants.ZOOM_MODE) {
if (!shiftDown) {
doZoomIn(rbRect);
} else {
// zoom out
double xInc = getXZoomIncrement();
double yInc = getYZoomIncrement();
double[] oldYs = {getMaxYVal(), getMinYVal()};
double[] newYs = {
getMinYVal() < 0 ? getMinYVal() - yInc : getMinYVal() + yInc,
getMaxYVal() < 0 ? getMaxYVal() - yInc : getMaxYVal() + yInc
};
double[] oldXs = {getMinXVal(), getMaxXVal()};
double[] newXs = {
getMinXVal() < 0 ? getMinXVal() - xInc : getMinXVal() + xInc,
getMaxXVal() < 0 ? getMaxXVal() - xInc : getMaxXVal() + xInc
};
this.zoomDomain(oldYs, newYs, oldXs, newXs);
}
mViewManager.invalidateAllViews();
}
} else {
// region selected
int x = rbRect.x - mLeft - 5;
int y = rbRect.y - mTop - 5;
int x2 = x + rbRect.width;
int y2 = y + rbRect.height;
Point p1 = new Point();
Point p2 = new Point();
if (toolMode == Constants.SELECT_MODE) {
if (!shiftDown) mCurrPC.unselectAll();
// construct a search region;
int[] xpoints = {x, x2, x2, x, x};
int[] ypoints = {y, y, y2, y2, y};
Polygon sr = new Polygon(xpoints, ypoints, 5);
// search for matches
byte[] currFilterResults = fdm.getResults();
double[] yArray1 = getYArray1();
double[] xArray1 = getXArray1();
double[] yArray2 = getYArray2();
double[] xArray2 = getXArray2();
mCurrPC.setBatchModeOn();
for (int i = 0; i < mCurrPC.getSize(); i++) {
if (!mIgnoreFilter
&& (mCurrPC.isDeleted(i) || currFilterResults[i] != 4 || !mCurrPC.isSelectedLayer(i)))
continue;
double yi = yArray1[i];
double xi = xArray1[i];
double xi2 = Float.NaN;
double yi2 = Float.NaN;
double xx2 = Float.NaN;
double yy2 = Float.NaN;
// correct the X value if necessary
xi = correctX(xi);
double xx1 = (xi - winXOrigin) * winXScale;
double yy1 = (yi - winYOrigin) * winYScale;
// correct the Y coordinate if necessary
yy1 = correctY(yy1);
if (!isYAxisScaler()) {
yi2 = yArray2[i];
yy2 = (yi2 - winYOrigin) * winYScale;
yy2 = correctY(yy2);
} else yy2 = yy1;
if (!isXAxisScaler()) {
xi2 = xArray2[i];
xi2 = correctX(xi2);
xx2 = (xi2 - winXOrigin) * winXScale;
} else xx2 = xx1;
p1.setLocation((int) xx1, (int) yy1);
p2.setLocation((int) xx2, (int) yy2);
if (sr.contains(p1) || sr.contains(p2)) {
mCurrPC.select(i);
}
}
mViewManager.invalidateAllViews();
mCurrPC.setBatchModeOff();
} else if (toolMode == Constants.ZOOM_MODE) {
// correct the x value if necessary
double xx = correctX((double) x);
double xx2 = correctX((double) x2);
// correct the Y coordinate if necessary
double yy = correctY((double) y);
double yy2 = correctY((double) y2);
double newX = (xx / winXScale) + winXOrigin;
double newX2 = (xx2 / winXScale) + winXOrigin;
double newY = (yy / winYScale) + winYOrigin;
double newY2 = (yy2 / winYScale) + winYOrigin;
double[] oldYs = {getMaxYVal(), getMinYVal()};
if (this.toString().indexOf("Depth") >= 0) {
double temp = newY2;
newY2 = newY;
newY = temp;
}
double[] newYs = {newY2, newY};
double[] oldXs = {getMinXVal(), getMaxXVal()};
double[] newXs = {newX, newX2};
this.zoomDomain(oldYs, newYs, oldXs, newXs);
mViewManager.invalidateAllViews();
}
}
}
public void run() {
while (true) {
BALL = null;
ball_t ballLCM = new ball_t();
synchronized (depthMonitor) {
try {
depthMonitor.wait();
} catch (Exception e) {
e.printStackTrace();
}
}
ballLCM.nanoTime = depthStream.latestTimestamp;
ArrayList<Statistics> blobs;
depthStream.getReadLock().lock();
try {
blobs = finder.analyze2(depthStream.getValidImageArray());
} finally {
depthStream.getReadLock().unlock();
}
Statistics ball = null;
Statistics robot = null;
int minSize = pg.gi("blobThresh");
// find robot and ball by blob size
Collections.sort(blobs, ComparatorFactory.getStatisticsCompareSize());
// find robot and ball by y pixel
// Collections.sort(blobs, ComparatorFactory.getStatisticsCompareYPix());
// if (tracking) {
// System.out.println("num blobs: " + blobs.size());
// System.out.println("biggest blob size: " + blobs.get(0).N);
// }
// for (Statistics blob : blobs) {
// if (blob.N > 10) {
// System.out.println("blob size: " + blob.N);
// }
// else {
// break;
// }
// }
if (blobs.size() == 1) {
Statistics first = blobs.get(0);
if (first.N > minSize) {
ball = first;
}
} else if (blobs.size() >= 2) {
Statistics first = blobs.get(0);
Statistics second = blobs.get(1);
if (first.N > minSize) {
ball = first;
}
if (second.N > minSize) {
robot = first;
ball = second;
}
}
// System.out.println("balls points " + depthStream.trajectory.size());
// if not tracking keep kv.depthStream.trajectory to just one index
if (!tracking) {
depthStream.trajectory.clear();
}
if (ball != null) {
depthStream.trajectory.add(ball);
Point depthPix = ball.center();
Point depthCoord = new Point();
depthCoord.x = depthPix.x - KinectVideo.C_X;
depthCoord.y = KinectVideo.C_Y - depthPix.y;
// System.out.println("avg depth " + ball.Uz());
// get depth from average depth of blob
double realDepth = raw_depth_to_meters(ball.Uz());
Point3D coord = depthStream.getWorldCoords(depthCoord, realDepth);
if (depthPix != null) {
for (int y = depthPix.y - 3; y < depthPix.y + 3; y++) {
for (int x = depthPix.x - 3; x < depthPix.x + 3; x++) {
try {
depthImg.setRGB(x, y, 0xFFFF0000);
} catch (Exception e) {
// System.out.println(x + " " + y);
}
;
}
}
// if (tracking) {
// //save image
// depthStream.getReadLock().lock();
// try {
// File imgFile = new File("image" + ballNum++ + ".png");
// try {
// ImageIO.write(depthImg, "png", imgFile);
// }
// catch(Exception e) {
// System.out.println("can't save img");
// }
// }
// finally {
// depthStream.getReadLock().unlock();
// }
// }
if (tracking) {
ballLCM.x = coord.x;
ballLCM.y = coord.y;
ballLCM.z = coord.z;
// if(tracking)
System.out.println("updating new ball (" + System.currentTimeMillis() + ")");
// if (ballLCM.x > CatchController.TARGET_MAX_X)
lcm.publish("6_BALL", ballLCM);
// else
// System.out.println("ball past target zone");
}
}
}
}
}
