ObjectLocator's purpose is to locate physical objects closest to specified one. It uses a tile array over the two-dimensional "world". The search is done over closest tiles in spiral manner.
Invoked to detect which agents should interact each with other, it notably reduces the closest/fittest agents search time (compared to one-to-one search). This implementation gives real-time performance for over 10000 uniformly spread interacting units (for 200x200 tiles). I may append performance charts later.
package yarangi.ui.bidim.geom;
import java.awt.Color;
import java.util.Comparator;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.Map;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;
import yarangi.ui.ProcessVisualizer;
/**
* Object locator provides a set of methods to search for objects inside two-dimensional square.
*
* Proximity functions are implemented by tile search.
*
* To use the locator, all object locations must be registered using {@link #updateLocation(ObjectLocation2D)} method.
*
* TODO: is not thread-safe
* @author Dve Yarangi
*/
public class ObjectLocator2D
{
/** approximation grid detection size. */
private int gridDim;
/** makes calculations faster. */
private int halfGridDim;
/** makes calculations faster. */
private int halfWorldWidth, halfWorldHeight;
/**
* Tile array cell holds a list of ObjectLocation objects, allowing
* quickly extract all objects that are located in this tile.
*/
@SuppressWarnings("unchecked")
public LinkedList [][] blocks;
/**
* Maps objects to their tile coordinates, allowing quickly locate the tale
* that contains a specified object.
*/
public HashMap objects = new HashMap ();
/**
* Defines the ratio between world coordinate and tile ingex.
*/
private double widthScale, heightScale;
/**
* Dimensions of a single tile.
*/
private double blockWidth, blockHeight;
/**
* Creates a new object locator.
* @param worldHeight worlds height
* @param worldWidth world's width
* @param gridDim The finess of the search tile overlay.
*/
@SuppressWarnings("unchecked")
public ObjectLocator2D(int worldHeight, int worldWidth, int gridDim)
{
this.gridDim = gridDim;
halfGridDim = gridDim / 2; // will be useful later
halfWorldWidth = worldHeight/2;
halfWorldHeight = worldHeight/2;
blockWidth = worldWidth / ((double)gridDim);
blockHeight = worldHeight / ((double)gridDim);
widthScale = 1 / blockWidth;
heightScale = 1 / blockHeight;
// initiating tiles:
blocks = new LinkedList [gridDim][gridDim];
for(int i = 0; i < gridDim; i ++)
for(int j = 0; j < gridDim; j ++)
blocks[i][j] = new LinkedList();
}
/**
* This method is used to inform the ObjectLocator about object reposition.
* @param location
*/
@SuppressWarnings("unchecked")
public void updateLocation(ObjectLocation2D location)
{
ObjectIndex index = objects.get(location);
if(index == null) // this object is not registered with the object locator
{
index = new ObjectIndex();
objects.put(location, index);
}
else
blocks[index.i][index.j].remove(location); // dropping the location
// calculating new tile:
index.i = (int)(location.getX() * heightScale) + halfGridDim;
index.j = (int)(location.getY() * widthScale) + halfGridDim;
// TODO: somehow handle the object that drops out the tile coverage:
if(index.i < 0 || index.i >= gridDim || index.j < 0 || index.j >= gridDim)
throw new IllegalArgumentException("Location [" + location.getX() + "," + location.getY() + "] is out of world dimensions.");
// updating the tile objects' list:
blocks[index.i][index.j].add(location);
}
public void deleteLocation(ObjectLocation2D location) {
ObjectIndex index = objects.get(location);
if(index == null)
return;
objects.remove(location);
blocks[index.i][index.j].remove(location);
}
/**
* Removes the specified location from the locator registry.
* @param location
*/
public void removeLocation(ObjectLocation2D location)
{
ObjectIndex index = objects.get(location);
if(index == null) // this object is not registered with the object locator
return;
blocks[index.i][index.j].remove(location);
objects.remove(location);
}
/**
* Retrieves the closes object to given location that fits with the filter.
*
*/
public ProximityPair2D getClosestObject(ObjectLocation2D location, ObjectLocation2DFilter filter)
{
Set set = getClosestObjects(location, 1, Double.MAX_VALUE, filter, null);
return set.size() == 0 ? null : set.iterator().next();
}
/**
* Retrieve a specified number of object locations inside the specified range around the specified location that
* fit the specified filter. The returned object locations will be the ones with the highest characteristics values.
*
* @param location The source location
* @param objNum Maximal number of retrieved locations
* @param range Maximal range of the search
* @param filter Further restricts the location selections.
* @param ch Location selection priority
* @return Set of locations that fit the specified parameters.
*/
public SortedSet getClosestObjects(ObjectLocation2D location, int objNum, double range, ObjectLocation2DFilter filter, ObjectLocation2DCharacteristics ch)
{
// most intensely created temporary variables:
double distance, dx, dy;
ObjectLocation2D l;
double farestDistance;
// setting the "minimal distances array" size cap:
int objectsNum = Math.min(objNum, objects.size());
// preparing the results' set:
SortedSet result = new TreeSet (ch == null ? new DistanceComparator() : new CharacteristicsComparator());
// getting the object index:
ObjectIndex index = objects.get(location);
if(index == null) // we don't have the object, go away
return result;
// preparing the shortest allowed to block out the tiles that are too far:
double shortestDistance = range;
// location relative to the tile - we will need it later:
double inblockX = location.getX() - index.i * blockWidth + halfWorldWidth;
double inblockY = location.getY() - index.j * blockHeight + halfWorldHeight;
// this is the blocks search radius. OOO
// each radius loop we check the tiles that form a square that O O
// have side equals to 2*radius+1 blocks around the target location: OOO
for(int radius = 0; radius < gridDim; radius ++)
{
// this will hold number of to far tiles in current tile square.
// if all of them are to far, there is no reason to check
// the squares with bigger radius
int toFarBlocks = 0;
for(int di = -radius; di <= radius; di ++)
for(int dj = -radius; dj <= radius; dj ++)
{
int blockI = index.i + di;
if(blockI >= gridDim || blockI < 0)
continue; // outside of the world
int blockJ = index.j + dj;
if(blockJ >= gridDim || blockJ < 0)
continue; // outside of the world
// now scanning the block:
// at first, we determine the minimal distance from the target location to
// current tile. If the minimal distance to the tile is longer than the distance
// to the all the closest locations found, searching current tile is useless.
if(radius != 0) // yeah-yeah
{
double toBlockX = blockI > index.i ?
(blockWidth - inblockX) + (blockI - index.i - 1) * blockWidth:
blockI < index.i ?
inblockX + (index.i - blockI - 1) * blockWidth:
0;
double toBlockY = blockJ > index.j ?
(blockHeight - inblockY) + (blockJ - index.j - 1) * blockHeight:
blockJ < index.j ?
inblockY + (index.j - blockJ - 1) * blockHeight:
0;
// System.out.println(toBlockX + " ::: " + toBlockY);
// System.out.println(shortestDistance + " ::: " + toBlockX * toBlockX + toBlockY * toBlockY);
if(shortestDistance < toBlockX * toBlockX + toBlockY * toBlockY)
{
toFarBlocks ++; // counting useless tiles to interrupt the radius loop later
continue; // going to the next tile
}
}
// ProcessVisualizer.addRect((blockI-halfGridDim)/widthScale-blockWidth/2, (blockJ-halfGridDim)/heightScale-blockHeight/2,
// (blockI-halfGridDim)/widthScale+blockWidth/2, (blockJ-halfGridDim)/heightScale+blockHeight/2);
// System.out.println(halfWorldDim);
// then scanning all objects inside the tile:
for(Object o : blocks[blockI][blockJ])
{
if(o == location) // not searching for self just yet
continue;
l = (ObjectLocation2D) o;
if(!filter.accept(l)) // filter says it is a bad object
continue;
dx = l.getX()-location.getX();
dy = l.getY()-location.getY();
distance = dx*dx + dy*dy;
if(distance > shortestDistance) // out of range or farer that any other found locations
continue;
// System.out.println("distance:" + distance + ", range: " + range + ", sd: " + shortestDistance);
double characteristics = ch == null ? distance : ch.getRelativeCharacteristics(location, l);
if(result.size() < objectsNum) // result set is not full yet, adding everything
{
result.add(new ProximityPair2D(location, l, distance, characteristics));
continue;
}
// now going over the locations that are already in the result set
// to determine which one should be kicked out:
if(result.last().getCharacteristics() > characteristics)
{
result.remove(result.last());
result.add(new ProximityPair2D(location, l, distance, characteristics));
farestDistance = result.last().getCharacteristics();
// System.out.println(fd2);
}
else
farestDistance = Double.MAX_VALUE;
// setting the maximal relevant distance to a tile:
if(ch == null) // this only happens if no characteristics is provided
shortestDistance = Math.min(farestDistance, shortestDistance);
//////
}
}
// System.out.println(radius);
if(radius != 0 && toFarBlocks >= radius*8)
break;
}
return result;
}
public Map > getProximityMap(double range, ObjectLocation2DFilter sourceFilter, ObjectLocation2DFilter targetFilter)
{
// System.out.println(objects.size());
Map > map = new HashMap > ();
for(ObjectLocation2D object : objects.keySet())
{
if(sourceFilter.accept(object))
{
SortedSet pairs = getClosestObjects(object, Integer.MAX_VALUE, range, targetFilter, null);
// System.out.println(pairs.size());
map.put(object, pairs);
}
}
// System.out.println(ProximityPair2D._object_count);
return map;
}
public void visualizeDensity()
{
for(int blockI = 0; blockI < gridDim; blockI ++)
for(int blockJ = 0; blockJ < gridDim; blockJ ++)
{
if(blocks[blockI][blockJ].size() < 5)
continue;
float alpha = blocks[blockI][blockJ].size() > 20 ? 1 : blocks[blockI][blockJ].size() / 20.f;
Color color = new Color(0.3f, 0.3f, 1.f, alpha);
double di = (blockI-halfGridDim)/widthScale;
double dj = (blockJ-halfGridDim)/heightScale;
double minX = di <= 0 ? di-blockWidth : di;
double minY = dj <= 0 ? dj-blockHeight : dj;
double maxX = minX + blockWidth;
double maxY = minY + blockWidth;
ProcessVisualizer.addRect(minX, minY, maxX, maxY, color, true);
// blocks[i][j] = new LinkedList();
}
}
/**
*
*
* @author DveYarangi
*/
class ObjectIndex {
public int i = -1;
public int j = -1;
}
public static class DistanceComparator implements Comparator
{
public int compare(ProximityPair2D o1, ProximityPair2D o2) {
return (int)(o1.getSquaredDistance() - o2.getSquaredDistance());
}
}
public static class CharacteristicsComparator implements Comparator
{
public int compare(ProximityPair2D o1, ProximityPair2D o2) {
return (int)(o1.getCharacteristics() - o2.getCharacteristics());
}
}
}
Now working on using this to implement obstacle detection. The required functionality: having set of poly-line obstacle features, retrieve new polylines that are inside agent's sight range.
No comments:
Post a Comment