/*
* Initialise Method
* @params start and goal coordinates
*/
public void init(int sX, int sY, int gX, int gY) {
cellHash.clear();
path.clear();
openHash.clear();
while (!openList.isEmpty()) openList.poll();
k_m = 0;
s_start.x = sX;
s_start.y = sY;
s_goal.x = gX;
s_goal.y = gY;
CellInfo tmp = new CellInfo();
tmp.g = 0;
tmp.rhs = 0;
tmp.cost = C1;
cellHash.put(s_goal, tmp);
tmp = new CellInfo();
tmp.g = tmp.rhs = heuristic(s_start, s_goal);
tmp.cost = C1;
cellHash.put(s_start, tmp);
s_start = calculateKey(s_start);
s_last = s_start;
}
/*
* Update the position of the agent/robot.
* This does not force a replan.
*/
public void updateStart(int x, int y) {
s_start.x = x;
s_start.y = y;
k_m += heuristic(s_last, s_start);
s_start = calculateKey(s_start);
s_last = s_start;
}
/*
* updateCell as per [S. Koenig, 2002]
*/
public void updateCell(int x, int y, double val) {
State u = new State();
u.x = x;
u.y = y;
if ((u.eq(s_start)) || (u.eq(s_goal))) return;
makeNewCell(u);
cellHash.get(u).cost = val;
updateVertex(u);
}
/*
* This is somewhat of a hack, to change the position of the goal we
* first save all of the non-empty nodes on the map, clear the map, move the
* goal and add re-add all of the non-empty cells. Since most of these cells
* are not between the start and goal this does not seem to hurt performance
* too much. Also, it frees up a good deal of memory we are probably not
* going to use.
*/
public void updateGoal(int x, int y) {
List<Pair<ipoint2, Double>> toAdd = new ArrayList<Pair<ipoint2, Double>>();
Pair<ipoint2, Double> tempPoint;
for (Map.Entry<State, CellInfo> entry : cellHash.entrySet()) {
if (!close(entry.getValue().cost, C1)) {
tempPoint =
new Pair(new ipoint2(entry.getKey().x, entry.getKey().y), entry.getValue().cost);
toAdd.add(tempPoint);
}
}
cellHash.clear();
openHash.clear();
while (!openList.isEmpty()) openList.poll();
k_m = 0;
s_goal.x = x;
s_goal.y = y;
CellInfo tmp = new CellInfo();
tmp.g = tmp.rhs = 0;
tmp.cost = C1;
cellHash.put(s_goal, tmp);
tmp = new CellInfo();
tmp.g = tmp.rhs = heuristic(s_start, s_goal);
tmp.cost = C1;
cellHash.put(s_start, tmp);
s_start = calculateKey(s_start);
s_last = s_start;
Iterator<Pair<ipoint2, Double>> iterator = toAdd.iterator();
while (iterator.hasNext()) {
tempPoint = iterator.next();
updateCell(tempPoint.first().x, tempPoint.first().y, tempPoint.second());
}
}
/**
* 次の状態へ遷移する。 台車、棒の位置はシミュレーション計算で求める。
*
* @param action 力を加える方向。
*/
public void nextState(int action) {
// 台車の加速度
double xAcc;
// 棒の角加速度
double thetaAcc;
// 台車に加える力
double force;
// cosθ
double cosTheta;
// sinθ
double sinTheta;
double temp;
// actionに応じて力を加える方向を変える
force = 0;
switch (action) {
case 0:
force = -FORCE_MAG;
break;
case 1:
force = FORCE_MAG;
break;
}
// 台車と棒の運動方程式にしたがって次の状態を決める
// 台車の詳しい運動方程式はWebを参照してください
cosTheta = Math.cos(state.theta);
sinTheta = Math.sin(state.theta);
temp = (force + POLE_MASS_LENGTH * state.thetaDot * state.thetaDot * sinTheta) / TOTAL_MASS;
thetaAcc =
(GRAVITY * sinTheta - cosTheta * temp)
/ (LENGTH * (FOUR_THIRDS - MASS_POLE * cosTheta * cosTheta / TOTAL_MASS));
xAcc = temp - POLE_MASS_LENGTH * thetaAcc * cosTheta / TOTAL_MASS;
// 状態を更新する
state.x += TAU * state.xDot;
state.xDot += TAU * xAcc;
state.theta += TAU * state.thetaDot;
state.thetaDot += TAU * thetaAcc;
}