View Javadoc

1   /*
2    * Copyright 2007-2008 Volker Fritzsch
3    *
4    * Licensed under the Apache License, Version 2.0 (the "License");
5    * you may not use this file except in compliance with the License.
6    * You may obtain a copy of the License at
7    *
8    *     http://www.apache.org/licenses/LICENSE-2.0
9    *
10   * Unless required by applicable law or agreed to in writing, software
11   * distributed under the License is distributed on an "AS IS" BASIS,
12   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13   * See the License for the specific language governing permissions and
14   * limitations under the License. 
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   * <p>
27   * @author <a href="mailto:vfritzsch@users.sourceforge.net">Volker Fritzsch</a>
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  		// distance calculation
77  		double radiansBetweenIrDots = radiansPerPixel * distanceBetweenIrLights;
78  		double z = (distanceBetweenIrLightSourcesInMillimeter / 2d) / Math.tan(radiansBetweenIrDots / 2d);// / screenWidth;
79  
80  		// calculation of x and y position (relative to 512, 384)
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  }