package org.samcrow.sensor;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.awt.geom.Line2D;
import java.awt.geom.PathIterator;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.Collection;
import java.util.HashSet;
import org.lekan.graphics.SGLine;
import org.lekan.graphics.SGObject;
import org.samcrow.environment.Obstacle;
import org.samcrow.vehicle.Vehicle;
/**
* A rangefinder that calculates range to an obstacle in one direction
*
* @author Sam Crow
*
*/
public class SingleRangefinder extends Sensor {
/** Distance in pixels in which this sensor can detect an object */
public static final double RANGE = 1000;
private double lastDistance = 0;
/**
* Construct a new SingleRangefinder with a given location and rotation
* relative to the Vehicle that uses it
*
* @param inLocalX
* The X-axis (toward the right of the car) locaton relative to
* the car origin
* @param inLocalY
* The Y-axis (toward the front of the car) location relative to
* the car origin
* @param inLocalRotation
* The rotation, clockwise looking from above, relative to the
* vehicle front, of this sensor.<br />
* This is rotation is applied after the X and Y translations
* described by localX and localY.
* @param inParent
* The Vehicle that this sensor belongs to
* @param obstacles
* A Collection of Obstacles to detect
*/
public SingleRangefinder(double inLocalX, double inLocalY,
double inLocalRotation, Vehicle inParent,
Collection<Obstacle> obstacles) {
super(inLocalX, inLocalY, inLocalRotation, inParent, obstacles);
}
@Override
public void run() {
double distance = getDistance();
if (distance != lastDistance) {
if (listener != null) {
SensorData data = new SensorData(this, distance);
listener.sensorDataReceived(data);
}
}
lastDistance = distance;
}
/*
* (non-Javadoc)
*
* @see org.samcrow.sensor.Sensor#getDrawObjects()
*/
@Override
public Collection<SGObject> getDrawObjects() {
Collection<SGObject> objects = new HashSet<SGObject>();
Line2D.Double line = getRangeLine();
objects.add(new SGLine(line.x1, line.y1, line.x2, line.y2));
return objects;
}
/**
* Get a Line representing the location, heading, and range of this
* rangefinder.
*
* @return A Line representing the location, heading, and range of this
* rangefinder.
*/
protected Line2D.Double getRangeLine() {
double absoluteHeading = parent.getHeading() + localRotation;
Line2D.Double line = new Line2D.Double(0, 0, 0, RANGE);
AffineTransform transform = new AffineTransform();
transform.translate(parent.getX(), parent.getY());
transform.rotate(Math.toRadians(absoluteHeading));
transform.translate(localX, -localY);
Shape result = transform.createTransformedShape(line);
PathIterator iterator = result.getPathIterator(null);
double[] coords = new double[8];
iterator.currentSegment(coords);
Point2D.Double p1 = new Point2D.Double(coords[0], coords[1]);
iterator.next();
coords = new double[8];
iterator.currentSegment(coords);
Point2D.Double p2 = new Point2D.Double(coords[0], coords[1]);
line = new Line2D.Double(p1, p2);
return line;
}
/**
* Get the distance from this rangefinder to the nearest obstacle. The
* accuracy of the reading decreases linearly with the distance from the
* sensor to the obstacle. This means that the further the obstacle is from
* the sensor, the more likely the sensor is to over- or underestimate the
* distance or to detect an object that is not directly in its path.
*
* @return The approximate distance from the sensor to the nearest object
*/
/* package */double getDistance() {
Line2D.Double line = getRangeLine();
double distance = RANGE;
double xDiff = line.getX2() - line.getX1();
double yDiff = line.getY2() - line.getY1();
// iterate through RANGE rectagles, starting at the sensor location and
// ending at the end of the line
for (double i = 0; i < RANGE; i += 1) {
double ratio = i / RANGE;
Point2D.Double currentPoint = new Point2D.Double(line.getP1()
.getX() + xDiff * ratio, line.getP1().getY() + yDiff
* ratio);
final double boxSize = i / 20;// half side length of the rectangle
// to check
Rectangle2D.Double rect = new Rectangle2D.Double(
currentPoint.getX() - boxSize, currentPoint.getY()
- boxSize, 2 * boxSize, 2 * boxSize);
// check if this rectangle interferes with any obstacle
synchronized (obstacles) {
for (Obstacle obstacle : obstacles) {
if (obstacle.collides(rect)) {
// System.out.println("Collision at "+i+".");
if (i < distance) {
distance = i;
}
}
}
}
}
return distance;
}
}