1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package motejx.adapters;
17
18 import java.awt.geom.Point2D;
19
20 import motej.IrPoint;
21 import motej.event.IrCameraEvent;
22 import motej.event.IrCameraListener;
23
24
25
26
27
28
29 public abstract class IrDistanceAdapter implements IrCameraListener {
30
31 private double radiansPerPixel = Math.toRadians(45) / Point2D.distance(1024d, 768d, 0d, 0d);
32
33 private double distanceBetweenIrLightSourcesInMillimeter = 150d;
34
35 public IrDistanceAdapter(double distanceBetweenIrLightSourcesInMillimeter) {
36 this.distanceBetweenIrLightSourcesInMillimeter = distanceBetweenIrLightSourcesInMillimeter;
37 }
38
39 public void irImageChanged(IrCameraEvent evt) {
40 IrPoint p0 = null;
41 IrPoint p1 = null;
42
43 if (evt.getIrPoint(0).x != 1023) {
44 p0 = evt.getIrPoint(0);
45 }
46
47 if (evt.getIrPoint(1).x != 1023) {
48 if (p0 == null)
49 p0 = evt.getIrPoint(1);
50 else
51 p1 = evt.getIrPoint(1);
52 }
53
54 if (evt.getIrPoint(2).x != 1023) {
55 if (p0 == null)
56 p0 = evt.getIrPoint(2);
57 else if (p1 == null)
58 p1 = evt.getIrPoint(2);
59 }
60
61 if (evt.getIrPoint(3).x != 1023) {
62 if (p0 == null)
63 p0 = evt.getIrPoint(3);
64 else if (p1 == null)
65 p1 = evt.getIrPoint(3);
66
67 }
68
69 if (p0 == null || p1 == null)
70 return;
71
72 double avgX = (p0.x + p1.x) / 2;
73 double avgY = (p0.y + p1.y) / 2;
74 double distanceBetweenIrLights = p0.distance(p1);
75
76
77 double radiansBetweenIrDots = radiansPerPixel * distanceBetweenIrLights;
78 double z = (distanceBetweenIrLightSourcesInMillimeter / 2d) / Math.tan(radiansBetweenIrDots / 2d);
79
80
81 double x = Math.sin(radiansPerPixel * (avgX - 512)) * z;
82 double y = Math.sin(radiansPerPixel * (avgY - 384)) * z;
83
84 positionChanged(x,y,z);
85 }
86
87 public abstract void positionChanged(final double x, final double y, final double z);
88
89 }