/**
*
*/
package iamrescue.routing.costs;
import iamrescue.execution.command.IPath;
import iamrescue.util.PositionXY;
import java.util.List;
import org.apache.log4j.Logger;
import rescuecore2.standard.entities.Area;
import rescuecore2.standard.entities.StandardEntity;
import rescuecore2.standard.entities.StandardWorldModel;
import rescuecore2.worldmodel.EntityID;
/**
* @author Sebastian
*
*/
public abstract class AbstractRoutingCostFunction implements
IRoutingCostFunction {
public abstract double getTravelCost(Area area, PositionXY from,
PositionXY to);
private static final Logger LOGGER = Logger
.getLogger(AbstractRoutingCostFunction.class);
@Override
public double getCost(IPath path, StandardWorldModel worldModel) {
if (!path.isValid()) {
LOGGER.error("This path is not valid.");
return Double.POSITIVE_INFINITY;
}
// assert path.getLocations().get(0).equals(sourceID) :
// "Path does not start with source";
if (path.getLocations().size() == 1) {
// No movement
return 0;
}
List<EntityID> entities = path.getLocations();
List<PositionXY> positions = path.getXYPath();
double cost = 0;
for (int i = 0; i < entities.size(); i++) {
PositionXY lastPosition = positions.get(i);
PositionXY nextPosition = positions.get(i + 1);
Area entity = (Area) retrieveEntity(entities.get(i), worldModel);
if (lastPosition.equals(nextPosition)) {
continue;
} else {
cost += getTravelCost(entity, lastPosition, nextPosition);
}
}
return cost;
}
protected StandardEntity retrieveEntity(EntityID id,
StandardWorldModel worldModel) {
StandardEntity entity = worldModel.getEntity(id);
if (entity == null) {
throw new IllegalArgumentException(
"Could not find entity in world model: " + id);
}
return entity;
}
}