/*
* 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.phys2d;
import java.awt.Shape;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import net.javlov.world.Body;
import net.javlov.world.grid.*;
import net.phys2d.math.ROVector2f;
//import net.phys2d.raw.Body;
import net.phys2d.raw.strategies.BruteCollisionStrategy;
public class Phys2DGridWorld extends Phys2DWorld implements IGridWorld {
protected Grid grid;
//map of each *moving* body to cells it currently intersects
protected Map<Body, Set<GridCell>> currentCells;
public Phys2DGridWorld(int iterations, float timestep, int width, int height, double cellWidth, double cellHeight ) {
super(iterations, new BruteCollisionStrategy(), timestep);
grid = new Grid(width, height, cellWidth, cellHeight);
currentCells = new HashMap<Body, Set<GridCell>>(15);
}
@Override
public boolean addBody(Body b) {
if ( super.addBody(b) ) {
addToCells((Phys2DBody)b);
return true;
}
return false;
}
@Override
public boolean removeBody(Body b) {
if ( super.removeBody(b) ) {
removeFromCells((Phys2DBody)b);
return true;
}
return false;
}
/*@Override
public void init() {
super.init();
grid.clear();
currentCells.clear();
for ( Body b : bodies )
addToCells(b);
}*/
public Grid getGrid() {
return grid;
}
/**
* @inheritDoc
*/
@Override
public List<Body> getIntersectingObjects(Shape s) {
ArrayList<Body> objects = new ArrayList<Body>();
Rectangle2D bounds = s.getBounds2D();
Set<Phys2DBody> occupiers;
if ( bounds.getWidth() > grid.getCellWidth() || bounds.getHeight() > grid.getCellHeight() )
occupiers = getOccupiers( getIntersectingCellsLarge(s) );
else
occupiers = getOccupiers( getIntersectingCells(s) );
for ( Body b : occupiers ) {
if ( s.intersects(b.getFigure().getBounds2D()) )
objects.add((net.javlov.world.Body)b);
}
return objects;
}
/**
* @inheritDoc
*/
@Override
public boolean intersectsObject(Shape s) {
for ( Body b : getOccupiers( getIntersectingCells(s) ) )
if ( s.intersects(b.getFigure().getBounds2D()) )
return true;
return false;
}
protected List<GridCell> getIntersectingCellsLarge(Shape s) {
Rectangle2D bounds = s.getBounds2D();
List<GridCell> intersectingCells = new ArrayList<GridCell>();
double minx = Math.max(bounds.getMinX(), 0),
maxx = Math.min(bounds.getMaxX(), grid.getWidth()*grid.getCellWidth()),
miny = Math.max(bounds.getMinY(), 0),
maxy = Math.min(bounds.getMaxY(), grid.getHeight()*grid.getCellHeight());
for ( double x = minx; x < maxx; x += grid.getCellWidth() )
for ( double y = miny; y < maxy; y += grid.getCellHeight() )
intersectingCells.add( grid.getCell(x, y) );
return intersectingCells;
}
protected List<GridCell> getIntersectingCells(Shape s) {
//IMPORTANT: the following assumes the shape is SMALLER
//than the width & height of a cell!
Rectangle2D bounds = s.getBounds2D();
double cx = bounds.getCenterX(),
cy = bounds.getCenterY();
List<GridCell> intersectingCells = new ArrayList<GridCell>(5);
GridCell cell = grid.getCell(cx, cy);
intersectingCells.add(cell);
//if the shape is not completely contained within the current cell, check for other
//intersecting cells
if ( !cell.contains(bounds) ) {
GridCell[] neighbours = cell.getQuadrantNeightbours(cx, cy);
for ( int j = 0; j < neighbours.length; j++ )
if ( neighbours[j].intersects(bounds) )
intersectingCells.add(neighbours[j]);
}
return intersectingCells;
}
protected Set<Phys2DBody> getOccupiers(Collection<? extends GridCell> cells) {
Set<Phys2DBody> occupiers = new HashSet<Phys2DBody>( (int) Math.ceil(cells.size() / 0.75) );
for ( GridCell c : cells )
for ( Body b : c.getOccupiers() )
occupiers.add((Phys2DBody) b);
return occupiers;
}
public void resolve(List<? extends net.phys2d.raw.Body> bodies, float dt) {
Phys2DBody p2db;
List<GridCell> intersectingCells;
/*for ( int i = bodies.size()-1; i >= 0; i-- ) {
b = (Phys2DBody) bodies.get(i);
System.out.println(b);
if ( !b.isStatic() ) {
intersectingCells = getIntersectingCells(b.getShape());
updateCells(intersectingCells, b);
}
}*/
for ( Body b : currentCells.keySet() ) {
intersectingCells = getIntersectingCells(b.getFigure());
updateCells(intersectingCells, b);
}
resolveFromCells(dt);
}
protected void updateCells(Collection<? extends GridCell> intersectingCells, Body b) {
//IMPORTANT: the following assumes the shape of dynamic bodies is SMALLER
//than the width & height of a cell!
Set<GridCell> cells = currentCells.get(b);
//nothing changed
if ( cells.size() == intersectingCells.size() )
return;
List<GridCell> buffer = new ArrayList<GridCell>();
//intersecting is a subset of last cells: need to remove some cells
if ( cells.size() > intersectingCells.size() ) {
for ( GridCell cell : cells )
if ( !intersectingCells.contains(cell) ) {
cell.removeBody((net.javlov.world.Body) b);
buffer.add(cell);
}
cells.removeAll(buffer);
}
//last cells is a subset of intersecting: need to add some cells
else {
for ( GridCell cell : intersectingCells )
if ( !cells.contains(cell) ) {
cell.addBody((net.javlov.world.Body) b);
buffer.add(cell);
}
cells.addAll(buffer);
}
}
protected void resolveFromCells(float dt) {
List<Phys2DBody> toResolve = new ArrayList<Phys2DBody>();
for ( Set<GridCell> cellSet : currentCells.values() ) {
for ( Phys2DBody b : getOccupiers(cellSet) )
toResolve.add(b);
super.resolve(toResolve, dt);
toResolve.clear();
}
}
protected void addToCells(Phys2DBody b) {
Rectangle2D.Float bounds = b.getShape().getBounds2D();
if ( bounds.width > grid.getCellWidth() || bounds.height > grid.getCellHeight() ) {
if ( !b.isStatic() )
throw new IllegalArgumentException(getClass() + " will behave incorrectly " +
"for dynamic bodies with a size larger than the cell size");
GridCell[][] cells = grid.getCells();
for(int h = 0; h < cells[0].length; h++)
for(int w = 0; w < cells.length; w++)
if ( cells[w][h].intersects(bounds) )
cells[w][h].addBody(b);
}
else {
ROVector2f pos = b.getPosition();
GridCell cell = grid.getCell(pos.getX(), pos.getY());
cell.addBody(b);
if ( !b.isStatic() ) {
Set<GridCell> cells = new HashSet<GridCell>(6);
cells.add(cell);
currentCells.put(b, cells);
}
if ( !cell.contains(bounds) ) {
GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
for ( int i = 0; i < neighbours.length; i++ )
if ( neighbours[i].intersects(bounds) ) {
neighbours[i].addBody(b);
if ( !b.isStatic() )
currentCells.get(b).add(neighbours[i]);
}
}
}
}
protected void removeFromCells(Phys2DBody b) {
Rectangle2D.Float bounds = b.getShape().getBounds2D();
if ( bounds.width > grid.getCellWidth() || bounds.height > grid.getCellHeight() ) {
GridCell[][] cells = grid.getCells();
for(int h = 0; h < cells[0].length; h++)
for(int w = 0; w < cells.length; w++)
if ( cells[w][h].intersects(bounds) )
cells[w][h].removeBody((net.javlov.world.Body)b);
}
else {
ROVector2f pos = b.getPosition();
GridCell cell = grid.getCell(pos.getX(), pos.getY());
cell.removeBody((net.javlov.world.Body)b);
if ( !b.isStatic() )
currentCells.remove(b);
if ( !cell.contains(bounds) ) {
GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
for ( int i = 0; i < neighbours.length; i++ )
if ( neighbours[i].intersects(bounds) )
neighbours[i].removeBody((net.javlov.world.Body)b);
}
}
}
/*
public void resolve(List<Body> bodies, float dt) {
Body b;
ROVector2f pos;
GridCell cell;
Rectangle2D.Float bounds;
Set<GridCell> cells;
List<GridCell> tempCells = new ArrayList<GridCell>(); //temp list to store cells to remove
for ( int i = bodies.size()-1; i >= 0; i-- ) {
b = bodies.get(i);
if ( b.isStatic() ) {
//System.out.println("Breaking at " + i);
break;
}
bounds = b.getShape().getBounds2D();
pos = b.getPosition();
//the cell that currently contains the centre of the body
cell = grid.getCell(pos.getX(), pos.getY());
//double check that the cell contains the body, there could've been a reset
if ( !cell.hasBody(b) )
cell.addBody(b);
//the list of cells that the body previously intersected
cells = currentCells.get(b);
cells.add(cell);
//if the body is completely contained within the current cell
if ( cell.contains(bounds) ) {
if ( cells.size() > 1 ) {
for ( GridCell c : cells ) {
if ( !c.equals(cell) ) {
c.removeBody(b);
tempCells.add(c);
}
}
cells.removeAll(tempCells);
}
}
//if the body is also intersecting some other cells
else {
tempCells.add(cell);
//get the cells neighbouring the cell quadrant in which the body is (3 cells)
GridCell[] neighbours = cell.getQuadrantNeightbours(pos.getX(), pos.getY());
for ( int j = 0; j < neighbours.length; j++ ) {
if ( neighbours[j].intersects(bounds) ) {
cells.add(neighbours[j]);
tempCells.add(neighbours[j]);
if ( !neighbours[j].hasBody(b) )
neighbours[j].addBody(b);
}
}
//tempCells contains the cells that are intersecting the body
for ( GridCell c : cells ) {
if ( !tempCells.contains(c) )
c.removeBody(b);
}
cells.retainAll(tempCells);
}
tempCells.clear();
}
//now currentCells contains the cells (usually just 1) that each body is currently
//intersecting
resolveFromCells(dt);
}*/
}