public void move(Path path, boolean checkStock) throws SOSActionException {
if (!(me.me() instanceof Human)) {
log().warn("can't use move in Center Agent " + me + "\n");
return;
}
if (path == null) {
log().error("move to null??....doing random walk");
((PlatoonAgent<?>) me).randomWalk(true);
}
log().debug("MOVE TO : " + path + "\n");
if (checkStock) checkStock(path);
Point2D xy = path.getDestination().second();
me.informationStacker.addInfo(me.model(), new MoveAction(path));
me.send(
new AKMove(me.getID(), me.model().time(), path.getIds(), (int) xy.getX(), (int) xy.getY()));
throw new SOSActionException("MoveXY(" + path + ", " + xy + ")");
}
private void computeCenter(int[] apexes) {
rescuecore2.geometry.Point2D p = SOSGeometryTools.computeCentroid(apexes);
centerX = (int) p.getX();
centerY = (int) p.getY();
if (apexes.length < 6) return;
try {
Point2D point1 = null, point2 = null, middlePoint = null;
ArrayList<rescuecore2.geometry.Point2D> twoPointOutOfLine = null;
if (!contains(centerX, centerY)) { // if the point is not inside the polygon
/////////////////////// finding a point inside polygon////////////////
FOR:
for (int i = 0; i < apexes.length - 2; i += 2) {
point1 = new rescuecore2.geometry.Point2D(apexes[i], apexes[i + 1]);
point2 = new rescuecore2.geometry.Point2D(apexes[i + 2], apexes[i + 3]);
middlePoint =
new rescuecore2.geometry.Point2D(
(apexes[i] + apexes[i + 2]) / 2, (apexes[i + 1] + apexes[i + 3]) / 2);
twoPointOutOfLine =
Utility.get2PointsAroundAPointOutOfLine(point1, point2, middlePoint, 2);
for (rescuecore2.geometry.Point2D point2d : twoPointOutOfLine) {
if (contains(point2d.getX(), point2d.getY())) {
centerX = (int) point2d.getX();
centerY = (int) point2d.getY();
break FOR;
}
}
}
/*
if (!contains(centerX, centerY)) {
debug.show("SOS Shape", new ShapeDebugFrame.AWTShapeInfo(this, "apexes=" + Arrays.toString(apexes), Color.blue, false),
new ShapeDebugFrame.Point2DShapeInfo(point1, "Point 1", Color.red, true),
new ShapeDebugFrame.Point2DShapeInfo(point2, "Point 2", Color.green, true),
new ShapeDebugFrame.Point2DShapeInfo(middlePoint, "middlePoint", Color.pink, true),
new ShapeDebugFrame.Point2DShapeInfo(twoPointOutOfLine.get(0), "twoPointOutOfLine 1", Color.magenta, true),
new ShapeDebugFrame.Point2DShapeInfo(twoPointOutOfLine.get(1), "twoPointOutOfLine 2", Color.black, true),
new ShapeDebugFrame.DetailInfo("area:" + SOSGeometryTools.computeAreaUnsigned(apexes))
);
}
*/
///////////////////////////////////////////////////////////////////////
}
} catch (Exception e) {
e.printStackTrace();
System.err.println(this.getClass().getSimpleName() + " have error" + e.getMessage());
}
}
@Override
public int getWeightFromXYToXY(Area area, Point2D start, Point2D end) {
int dstlength = ((int) start.distance(end)) / MoveConstants.DIVISION_UNIT;
if (area instanceof Building || !area.isBlockadesDefined()) return dstlength;
ReachablityState rs = Reachablity.isReachable((Road) area, start, end);
switch (rs) {
case Close:
return MoveConstants.UNREACHABLE_COST_FOR_GRAPH_WEIGTHING;
case FoggyClose:
return dstlength * 5;
case FoggyOpen:
return dstlength * 2;
default:
return dstlength;
}
}
private void check2CycleStock() throws SOSActionException {
CycleInformations cycleinfo1 = me.informationStacker.getInformations(1);
CycleInformations cycleinfo2 = me.informationStacker.getInformations(2);
if (!(cycleinfo1.getAct() instanceof MoveAction)
|| cycleinfo1.getAct() instanceof StockMoveAction) return;
if (!(cycleinfo2.getAct() instanceof MoveAction)
|| cycleinfo1.getAct() instanceof StockMoveAction) return;
Path path1 = ((MoveAction) cycleinfo1.getAct()).getPath();
// Path path2 = ((MoveAction) cycleinfo2.getAct()).getPath();
log().info("Two(2) cycle move ....");
log().debug("current position:" + me.me().getAreaPosition());
log().debug("1 cycle ago position:" + cycleinfo1.getPositionPair());
log().debug("2 cycle ago position:" + cycleinfo2.getPositionPair());
if (!cycleinfo1.getPositionPair().first().equals(cycleinfo2.getPositionPair().first())) return;
if (!cycleinfo1.getPositionPair().first().equals(me.me().getAreaPosition())) return;
if (SOSGeometryTools.getDistance(
cycleinfo1.getPositionPair().second(), cycleinfo2.getPositionPair().second())
> MoveConstants.TRAFFIC_CHECKING_DISTANCE) return;
//////////////// Stock Occured
log().debug("2 cycle stock occured!!!!!!!");
ArrayList<EntityID> entityPath = new ArrayList<EntityID>();
Area area = me.me().getAreaPosition();
log().debug("Current Position:" + area);
entityPath.add(area.getID());
Edge ed;
Area ne;
if (path1.getIds().size() > 0 && !path1.getIds().get(0).equals(area.getID())) {
ne = (Area) me.model().getEntity(path1.getIds().get(0));
ed = area.getEdgeTo(ne);
} else if (path1.getIds().size() > 1 && path1.getIds().get(0).equals(area.getID())) {
ne = (Area) me.model().getEntity(path1.getIds().get(1));
ed = area.getEdgeTo(ne);
} else {
ne = area.getNeighbours().get(0);
ed = area.getEdgeTo(ne);
}
log().debug("ne:" + ne + " ed:" + ed + " last move path:" + path1);
if (ed == null) {
log().error("[Move]edge is null!!!!!some problem!!!!");
ne = area.getNeighbours().get(0);
ed = area.getEdgeTo(ne);
log().debug("changed!!!! ne:" + ne + " ed:" + ed + " last move path:" + path1);
}
Point2D destXYAPointNearEdge;
if (ed != null) {
// if (area instanceof Road) {
Pair<Point2D, Point2D> points = getAPointInReachblePartEdges(area, ed);
destXYAPointNearEdge = points.second();
// } else {
// Line2D wallLine = ed.getLine();// new Line2D(edge.getStartX(), edge.getStartY(),
// edge.getEndX() - edge.getStartX(), edge.getEndY() - edge.getStartY());
// // ppp.add(new ShapeDebugFrame.Line2DShapeInfo(wallLine, "edge", Color.white.darker(),
// false, false));
// Vector2D offset;
// if (lastStockTime < me.time() - 4) {
// if (AliGeometryTools.havecorrectDirection(area)) {
// offset = wallLine.getDirection().getNormal().normalised().scale(1500);
// } else {
// offset = wallLine.getDirection().getNormal().normalised().scale(-1500);
// }
// } else {
// if (!AliGeometryTools.havecorrectDirection(area)) {
// offset = wallLine.getDirection().getNormal().normalised().scale(1500);
// } else {
// offset = wallLine.getDirection().getNormal().normalised().scale(-1500);
// }
// }
// destXYAPointNearEdge = ed.getMidPoint().plus(offset);
// }
} else {
log().error("[Move]edge is null!!!!!some problem!!!!");
destXYAPointNearEdge = area.getPositionPair().second();
}
log().debug("dest area for stock:" + area + " dst point:" + destXYAPointNearEdge);
me.send(
new AKMove(
me.getID(),
me.time(),
entityPath,
(int) destXYAPointNearEdge.getX(),
(int) destXYAPointNearEdge.getY()));
Path path =
new Path(
null,
null,
entityPath,
me.me().getPositionPair(),
new Pair<Area, Point2D>(area, destXYAPointNearEdge),
false);
me.informationStacker.addInfo(me.model(), new StockMoveAction(path));
// log().warn("Traffic Handeling should be change due to server doesn't support AKMotion");
lastStockTime = me.time();
throw new SOSActionException("Move Stock(" + area + " " + destXYAPointNearEdge + ")");
}
private void checkEarlyStock(Path path) throws SOSActionException {
Area source = me.me().getAreaPosition();
if (me.informationStacker.getInformations(1).getAct() instanceof MoveAction
&& me.informationStacker.getInformations(1).getPositionPair().first() instanceof Building) {
log()
.debug(
"last act is move and current position is in building ==> don't check early stock");
return;
}
if (me.informationStacker.getInformations(1).getAct() instanceof StockMoveAction) {
log().debug("last act is stock move ==> don't check early stock");
return;
}
if (source instanceof Building) {
log().debug("early stock detected!!! agent is in " + source);
ArrayList<EntityID> entityPath = new ArrayList<EntityID>();
entityPath.add(source.getID());
Area second = null;
Area third = null;
if (path.getIds().size() > 0 && !path.getIds().get(0).equals(source.getID())) {
second = (Area) me.model().getEntity(path.getIds().get(0));
if (path.getIds().size() > 1) third = (Area) me.model().getEntity(path.getIds().get(1));
} else if (path.getIds().size() > 1 && path.getIds().get(0).equals(source.getID())) {
second = (Area) me.model().getEntity(path.getIds().get(1));
if (path.getIds().size() > 2) third = (Area) me.model().getEntity(path.getIds().get(2));
} else {
log().warn("move path size is 0 can't checkEarlyStock!!!");
return;
}
if (second == null) {
log().warn("second is null can't checkEarlyStock!!!");
return;
} else {
ArrayList<Edge> edges = second.getEdgesTo(source);
int minDistance = Integer.MAX_VALUE;
for (Edge edge : edges) {
minDistance =
(int)
Math.min(
minDistance, SOSGeometryTools.distance(edge, me.me().getPositionPoint()));
}
// Edge fedge = second.getEdgeTo(source);
// if (SOSGeometryTools.distance(fedge, me.me().getPositionPoint()) <
// MoveConstants.ENTRACE_DISTANCE_MM+1000)
// return;
if (minDistance < MoveConstants.ENTRACE_DISTANCE_MM + 1000) {
log().debug("I'm too neat to edge in building... no need to check early stock");
return;
}
entityPath.add(second.getID());
}
if (third == null) {
log().debug("Move path is too small!!! no need to early stock handle");
return;
} else {
Edge ed = second.getEdgeTo(third);
Point2D destXYAPointNearEdge;
if (ed != null) {
Line2D wallLine =
ed
.getLine(); // new Line2D(edge.getStartX(), edge.getStartY(), edge.getEndX() -
// edge.getStartX(), edge.getEndY() - edge.getStartY());
// ppp.add(new ShapeDebugFrame.Line2DShapeInfo(wallLine, "edge", Color.white.darker(),
// false, false));
Vector2D offset;
if (AliGeometryTools.havecorrectDirection(second)) {
offset = wallLine.getDirection().getNormal().normalised().scale(15);
} else {
offset = wallLine.getDirection().getNormal().normalised().scale(-15);
}
destXYAPointNearEdge = ed.getMidPoint().plus(offset);
} else {
log().error("[Move]edge is null!!!!!some problem!!!!");
destXYAPointNearEdge = second.getPositionPair().second();
}
log()
.debug(
"source:"
+ source
+ " dest area for stock:"
+ second
+ " dst point:"
+ destXYAPointNearEdge);
me.send(
new AKMove(
me.getID(),
me.time(),
entityPath,
(int) destXYAPointNearEdge.getX(),
(int) destXYAPointNearEdge.getY()));
path =
new Path(
null,
null,
entityPath,
me.me().getPositionPair(),
new Pair<Area, Point2D>(second, destXYAPointNearEdge),
false);
me.informationStacker.addInfo(me.model(), new StockMoveAction(path));
throw new SOSActionException(
"Move Stock(" + source + "," + second + " " + destXYAPointNearEdge + ")");
}
}
}
@Override
public void write(OutputStream out) throws IOException {
writeDouble(value.getX(), out);
writeDouble(value.getY(), out);
}
// Morteza2011*****************************************************************
public static ArrayList<SOSArea> sortEdges(
StandardEntity a,
ArrayList<Edge> removedExtraEdgesBlock1,
ArrayList<Edge> removedExtraEdgesBlock2,
boolean setDirection)
throws ReachablityException {
ArrayList<SOSArea> areaList = new ArrayList<SOSArea>();
ArrayList<ArrayList<Edge>> edgeList = new ArrayList<ArrayList<Edge>>(2);
ArrayList<Edge> sortedEdges = new ArrayList<Edge>();
edgeList.add(new ArrayList<Edge>(removedExtraEdgesBlock1));
edgeList.add(new ArrayList<Edge>(removedExtraEdgesBlock2));
short index = 0;
short[] indexes = {0, 0};
boolean firstTime = true;
while (indexes[0] < edgeList.get(0).size() || indexes[1] < edgeList.get(1).size()) {
Point2D start, end;
if (indexes[0] < edgeList.get(0).size()) {
start = edgeList.get(0).get(indexes[0]).getStart();
end = edgeList.get(0).get(indexes[0]).getEnd();
sortedEdges.add(edgeList.get(0).get(indexes[0]));
indexes[0]++;
index = 0;
} else {
start = edgeList.get(1).get(indexes[1]).getStart();
end = edgeList.get(1).get(indexes[1]).getEnd();
sortedEdges.add(edgeList.get(1).get(indexes[1]));
indexes[1]++;
index = 1;
}
while (!end.isEqualTo(start)) {
if (indexes[index] < edgeList.get(index).size()
&& edgeList.get(index).get(indexes[index]).getStart().equals(end)) {
sortedEdges.add(edgeList.get(index).get(indexes[index]));
indexes[index]++;
} else if (indexes[1 - index] < edgeList.get(1 - index).size()
&& edgeList.get(1 - index).get(indexes[1 - index]).getStart().equals(end)) {
firstTime = false;
index = (short) (1 - index);
sortedEdges.add(edgeList.get(index).get(indexes[index]));
indexes[index]++;
} else {
boolean found = false;
index = (short) (1 - index);
short listSize = (short) (edgeList.get(index).size() - indexes[index]);
for (short i = 0; i < listSize; i++) {
if (!edgeList.get(index).get(indexes[index]).getStart().equals(end)) {
edgeList.get(index).add(edgeList.get(index).get(indexes[index]));
indexes[index]++;
} else {
found = true;
firstTime = false;
break;
}
}
if (!found) {
if (areaList.size() == 0) {
if (firstTime && !setDirection) {
ArrayList<Edge> newList = new ArrayList<Edge>(removedExtraEdgesBlock2);
setEqualDirection(newList);
try {
return sortEdges(a, removedExtraEdgesBlock1, newList, true);
} catch (ReachablityException e) {
throw new ReachablityException("need ReExpand!!!");
}
} else throw new ReachablityException("need ReExpand!!!");
} else return areaList;
}
sortedEdges.add(edgeList.get(index).get(indexes[index]));
indexes[index]++;
}
end = sortedEdges.get(sortedEdges.size() - 1).getEnd();
}
SOSArea area = new SOSArea(sortedEdges);
if (sortedEdges.size() > 2) areaList.add(area);
sortedEdges.clear();
}
if (edgeList.get(0).size() != indexes[0]) {
log(a).warn("Sorting has been completed but 1List has elements yet!!!");
}
if (edgeList.get(1).size() != indexes[1]) {
log(a).warn("Sorting has been completed but 1List has elements yet!!!");
}
return areaList;
}