-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathScene.java
More file actions
122 lines (108 loc) · 4.02 KB
/
Scene.java
File metadata and controls
122 lines (108 loc) · 4.02 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
import geometry.*;
import geometry.Point;
import javax.swing.*;
import java.awt.*;
import java.awt.event.*;
import java.util.ArrayList;
public class Scene extends JFrame {
private Point[] points;
private Plane[] planes;
private boolean showClosestPoint = false;
private Point lastMousePosition;
public Scene(String title, Plane[] planes, Point[] points) throws HeadlessException {
super(title);
this.points = points;
this.planes = planes;
setSize(500,520);
setResizable(false);
setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE);
add(new DrawingPanel());
addMouseMotionListener(new MouseMotionAdapter() {
@Override
public void mouseMoved(MouseEvent e) {
lastMousePosition = new Point(e.getX() + (int)(Scene.this.getContentPane().getSize().getWidth() - Scene.this.getWidth()), e.getY() + (int)(Scene.this.getContentPane().getSize().getHeight() - Scene.this.getHeight()));
showClosestPoint = true;
Scene.this.repaint();
}
});
addMouseListener(new MouseAdapter() {
@Override
public void mouseExited(MouseEvent e) {
showClosestPoint = false;
Scene.this.repaint();
}
});
JButton button = new JButton("Run ICP");
add(button, BorderLayout.SOUTH);
button.addActionListener(e -> new ICPWorker(new SimpleICP()));
setVisible(true);
}
public static Point findClosestPlane(Plane[] planes, Point p) {
ArrayList<Point> cPoints = new ArrayList<>();
for(Plane plane : planes) {
Point AP = new Point(p.getX() - plane.getX1(), p.getY() - plane.getY1());
Point AB = new Point(plane.getX2() - plane.getX1(), plane.getY2() - plane.getY1());
float ab2 = AB.getX()*AB.getX() + AB.getY()*AB.getY();
float ap_ab = AP.getX()*AB.getX() + AP.getY()*AB.getY();
float t = ap_ab / ab2;
t = t > 1 ? 1 : t < 0 ? 0 : t;
cPoints.add(new Point((int)(plane.getX1() + (AB.getX() * t)), (int)(plane.getY1() + (AB.getY() * t))));
}
int minDistance = 0;
Point minPoint = null;
for(Point a : cPoints) {
if(distanceBetweenPoints(a, p) < minDistance || minPoint == null) {
minDistance = distanceBetweenPoints(a, p);
minPoint = a;
}
}
return minPoint;
}
private static int distanceBetweenPoints(Point p1, Point p2) {
return (int)Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
}
private void drawTarget(Graphics2D g) {
for(Plane p : planes) {
p.draw(g);
}
}
private void drawSource(Graphics2D g) {
for(Point p : points) {
p.drawPoint(g);
}
}
private void drawToClosestPlane(Graphics2D g) {
Point end = findClosestPlane(planes, lastMousePosition);
g.drawLine(lastMousePosition.getX(), lastMousePosition.getY(), end.getX(), end.getY());
}
class DrawingPanel extends JComponent {
@Override
public void paint(Graphics g) {
super.paint(g);
g.setColor(Color.DARK_GRAY);
g.fillRect(0,0, getWidth(), getHeight());
g.setColor(Color.blue);
Scene.this.drawTarget((Graphics2D)g);
g.setColor(Color.red);
Scene.this.drawSource((Graphics2D)g);
if(showClosestPoint && lastMousePosition != null) {
g.setColor(Color.white);
Scene.this.drawToClosestPlane((Graphics2D)g);
}
}
}
class ICPWorker extends Thread {
private ICP icp;
public ICPWorker(ICP icp) {
this.icp = icp;
start();
}
@Override
public void run() {
Transformation t;
t = icp.iterate(points, planes);
System.out.println(t);
super.run();
}
}
}