/*
* Javlov - a Java toolkit for reinforcement learning with multi-agent support.
*
* Copyright (c) 2009 Matthijs Snel
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
package net.javlov.world;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.awt.geom.Path2D;
import java.awt.geom.PathIterator;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.List;
public class DistanceSensor implements GroupedSensor {
private int numRays;
private double rayLength, aperture, interRayAngle;
private Point2D.Double origin;
private Path2D.Double rayPath;
//TODO speed and ease over proper OO design, for now. Bad: double coupling between
//agent body and sensors
private Body body;
private World world;
public DistanceSensor( int numRays, double rayLength, double aperture ) {
origin = new Point2D.Double();
this.numRays = numRays;
this.rayLength = rayLength;
this.aperture = aperture;
createRays();
}
public DistanceSensor( int numRays, double rayLength, double aperture, World world ) {
this(numRays, rayLength, aperture);
setWorld(world);
}
protected void createRays() {
interRayAngle = aperture / (numRays-1);
double angle = aperture/2;
rayPath = new Path2D.Double();
for ( int i = 0; i < numRays; i++ ) {
rayPath.moveTo(origin.x, origin.y);
rayPath.lineTo(rayLength*Math.cos(angle), rayLength*Math.sin(angle));
angle -= interRayAngle;
}
}
//// GETTERS & SETTERS
public void setWorld(World world) {
this.world = world;
}
/**
* Ugly, but quick fix for ignoring own body detection. Could be fixed more nicely if
* detection wasn't based on object bounding boxes. Done like this for now for performance
* reasons.
* @param b
*/
public void setBody(Body b) {
body = b;
}
//// INTERFACE METHODS ////
@Override
public void init() {}
@Override
public void reset() {}
@Override
public Shape getRangeArea() {
return rayPath;
}
@Override
public void rotate(double angle) {
rayPath.transform(AffineTransform.getRotateInstance(angle, origin.x, origin.y));
}
@Override
public void rotate(double angle, double x, double y) {
AffineTransform at = AffineTransform.getRotateInstance(angle, x, y);
at.transform(origin, origin);
rayPath.transform(at);
}
@Override
public void translate(double dx, double dy) {
origin.x += dx;
origin.y += dy;
rayPath.transform(AffineTransform.getTranslateInstance(dx, dy));
}
/**
* NOTE: assumes that there is only 1 object at the time intersecting a single ray of
* the sensor.
*/
@Override
public double[] getReading() {
List<Body> objectsInRange = world.getIntersectingObjects(rayPath.getBounds2D());
if ( objectsInRange.size() < 2 )
return new double[1];
//System.out.println(objectsInRange.size() + " objects in range");
return getReadingFromObjects(objectsInRange);
}
/**
* NOTE: assumes that there is only 1 object at the time intersecting a single ray of
* the sensor.
*/
@Override
public double[] getReadingFromObjects(List<Body> objectsInRange) {
double[] rayCoords = new double[6],
tempReading = new double[numRays];
PathIterator rays = rayPath.getPathIterator(null);
int i = 0;
while (!rays.isDone()) {
//we know that for this sensor there is always a LINETO followed by a MOVETO;
//only interested in the LINETO
rays.next();
rays.currentSegment(rayCoords);
rays.next();
//System.out.println(Arrays.toString(rayCoords));
for ( Body b : objectsInRange ) {
if ( (tempReading[i] = getRayReading(rayCoords[0], rayCoords[1], b)) > 0 ) {
i++;
break;
}
}
}
if ( numRays > 2 )
return new double[] {getWeightedAverageReading(tempReading, 1)};
else
return new double[] {getAverageReading(tempReading)};
}
/**
* Compute weighted average of ray readings according to Gaussian distribution of ray
* weights. The closer to the sensor axis a ray is, the larger its weight. The shape of
* the distribution is governed by {@code gaussianWidth}: smaller gaussianWidth means a
* higher peak (larger weight to rays around the centre).
*
* Formula is the same as the one used by the Cyberbotics Webots(tm) distance sensors.
*
* @param reading the raw readings from each ray
* @param gaussianWidth governs the shape of the ray weight distribution
* @return weighted average of the readings
*/
public double getWeightedAverageReading(double[] reading, double gaussianWidth) {
double vals[] = new double[numRays],
sumVals = 0,
avgReading = 0,
axisAngle = 0;
for ( int i = 0; i < numRays; i++ ) {
axisAngle = aperture/2 - i*interRayAngle;
vals[i] = Math.exp(-Math.pow((axisAngle/(aperture*gaussianWidth)), 2));
sumVals += vals[i];
}
if ( sumVals == 0 )
return 0;
for ( int i = 0; i < numRays; i++ ) {
avgReading += vals[i] * reading[i] / sumVals;
}
//System.out.println("Reading: " + Arrays.toString(reading) + ", avgread: " + avgReading);
return avgReading;
}
public double getAverageReading(double[] reading) {
double sum = 0;
for ( int i = 0; i < numRays; i++ )
sum += reading[i];
return sum/numRays;
}
/**
* Code adapted from Java Rectangle2D.intersectsLine, so that if a ray intersects we
* immediately have the point of intersection with the bounding box of the shape (repeat:
* the BOUNDING BOX of the shape, so this method is inaccurate for non-rectangular shapes
* or rotated rectangular shapes. The level of inaccuracy depends on how different from a
* rectangle the shape is).
*
* @param x2 x-coord of ray endpoint
* @param y2 y-coord of ray endpoint
* @param s the shape to check for intersection (only checks bounding box)
* @return 1 - d/r, where d is distance between the origin of the ray and the
* intersection point, and r is ray length; or 0 if the ray doesn't intersect the
* bounding box of the shape.
*/
protected double getRayReading(double x2, double y2, Body b) {
Rectangle2D bounds = b.getFigure().getBounds2D();
double x1 = origin.x,
y1 = origin.y;
//System.out.println("SINGLE-RAY READING");
//System.out.println(" Ray from " + x1 + "," + y1 + " to " + x2 + "," + y2);
//System.out.println(" Bounding box " + bounds);
int out1, out2 = bounds.outcode(x2, y2);
while ((out1 = bounds.outcode(x1, y1)) != 0) {
if ((out1 & out2) != 0) {
//System.out.println(" No intersection, returning");
return 0;
}
if ((out1 & (Rectangle2D.OUT_LEFT | Rectangle2D.OUT_RIGHT)) != 0) {
double x = bounds.getX();
if ((out1 & Rectangle2D.OUT_RIGHT) != 0) {
x += bounds.getWidth();
}
y1 = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
x1 = x;
} else {
double y = bounds.getY();
if ((out1 & Rectangle2D.OUT_BOTTOM) != 0) {
y += bounds.getHeight();
}
x1 = x1 + (y - y1) * (x2 - x1) / (y2 - y1);
y1 = y;
}
}
//it intersects, and (x1, y1) is point of intersection
double d = Point2D.distance(origin.x, origin.y, x1, y1);
//System.out.println(" Distance to intersection point " + d);
if ( d == 0 ) {
Point2D pos = body.getLocation();
if ( pos.getX() <= bounds.getCenterX() + 0.001 &&
pos.getX() >= bounds.getCenterX() - 0.001 &&
pos.getY() <= bounds.getCenterY() + 0.001 &&
pos.getY() >= bounds.getCenterY() - 0.001)
{
//System.out.println(" Detected own body, ignoring");
return 0;
}
}
//System.out.println(" READING: " + (1 - d / rayLength));
return 1 - d / rayLength;
}
@Override
public int getReadingDim() {
return 1;
}
}