public Path PEstep(int MaxIteration)
{
inizializzation();
// ALGORITHM
int iteration = 0;
while (Vbest > 0 && iteration < MaxIteration)
{
iteration++;
// for each configuration of the robot
for (int n1 = 0; n1 < N; n1++)
{
for (int n2 = 0; n2 < N; n2++)
{
// for each future configuration of the robot
for (int m1 = 0; m1 < N; m1++)
{
if (isVisibleFrom(n1, m1))
{
for (int m2 = 0; m2 < N; m2++)
{
if (isVisibleFrom(n2, m2))
{
// Calculate next step and, if it is a better value, update the matrix d, v and path
int[][] Ahat = modifyAdjacency(A, r);
DirtySet Dhat = d[n1][n2].multiplication(Ahat);
int Vhat = Dhat.cardinality();
if (Vhat < v[m1][m2])
{
path[m1][m2].add(m1, m2);
d[m1][m2] = Dhat;
v[m1][m2] = Vhat;
}
// check for the best path so far
if (v[m1][m2] < Vbest)
{
Vbest = v[m1][m2];
Pbest = path[m1][m2];
}
}
}
}
}
}
}
}
return(Pbest);
}
public void inizializzation()
{
// Assign first value to all data
DirtySet d = new DirtySet(N);
d.calculateDirtySet(r);
d.updateVisibility(r, A);
//d.print ();
Q.Add(d);
//for (int i = 0; i < Np2; i++) {
x [0] [0] = r [0];
x [0] [1] = r [1];
//path[0].add (r);
v[0] = d.cardinality();
Vbest = v[0];
Pbest = path[0];
}
public void PEOneStep()
{
// for each configuration of the robot
for (int n1 = 0; n1 < N; n1++)
{
for (int n2 = 0; n2 < N; n2++)
{
// for each future configuration of the robot
for (int m1 = 0; m1 < N; m1++)
{
if (isVisibleFrom(n1, m1))
{
for (int m2 = 0; m2 < N; m2++)
{
if (isVisibleFrom(n2, m2))
{
// Calculate next step and, if it is a better value, update the matrix d, v and path
int[][] Ahat = modifyAdjacency(A, r);
DirtySet Dhat = d[n1][n2].multiplication(Ahat);
int Vhat = Dhat.cardinality();
if (Vhat < v[m1][m2])
{
path[m1][m2].add(m1, m2);
d[m1][m2] = Dhat;
v[m1][m2] = Vhat;
}
// check for the best path so far
if (v[m1][m2] < Vbest)
{
Vbest = v[m1][m2];
Pbest = path[m1][m2];
}
}
}
}
}
}
}
}
public void PEOneStep()
{
int Qsize = Q.Count;
//Debug.Log ("Q size "+Qsize);
// for each configuratioQ.Countn of the robot
for (int d = 0; d < Qsize; d++)
{
DirtySet D = (DirtySet)Q[d];
// try to move one robot at a time
for (int i = 0; i < 2; i++)
{
int[] Xhat = new int[2] {
x[d][0], x[d][1]
};
if (debugOn)
{
Debug.Log("X hat (" + Xhat[0] + ", " + Xhat[1] + ")");
}
// find the next node for the robot i
for (int m = 0; m < N; m++)
{
if (x[d][i] != m && isVisibleFrom(x[d][i], m))
{
Xhat[i] = m;
int[][] Ahat = modifyAdjacency(A, Xhat);
//Debug.Log("new robots: "+Xhat[0]+" "+Xhat[1]);
/***************/
/*
* string log = "";
* for (int k=0; k<N; k++) {
* for (int j=0; j<N; j++) {
* log += Ahat[k][j] + " ";
* }
* log += "\n";
* }
* Debug.Log(log);*/
/*******/
DirtySet Dhat = D.multiplication(Ahat);
if (debugOn)
{
Debug.Log("M: " + m + " Dhat: ");
}
if (debugOn)
{
Dhat.print();
}
int Vhat = Dhat.cardinality();
int Dindex = find(Dhat, Q);
if (Dindex == -1)
{
// new dirty set
x[Qsize] = Xhat;
path[Qsize] = new Path(path[d]);
path[Qsize].add(Xhat);
//Debug.Log("Q: "+Qsize);
//path[Qsize].print();
v[Qsize] = Vhat;
Q.Add(Dhat);
Dindex = Qsize;
if (Vhat == Vbest && path[Dindex].path.Count < Pbest.path.Count)
{
Pbest = new Path(path[Dindex]);
Vbest = Vhat;
}
else if (Vhat < Vbest)
{
Pbest = new Path(path[Dindex]);
Vbest = Vhat;
}
}
else if (Vhat < v[Dindex] || (Vhat == v[Dindex] &&
dist(Xhat, Dhat.dirtySet) < dist(x[d], Dhat.dirtySet)))
{
// update old dirty set with new info
//Debug.Log(dist(Xhat, Dhat.dirtySet));
x[Dindex] = Xhat;
//Debug.Log("Before: ");
//path[Qsize].print();
path[Dindex].add(Xhat);
//Debug.Log("Q: "+Dindex);
//path[Qsize].print();
v[Dindex] = Vhat;
if (Vhat == Vbest && path[Dindex].path.Count < Pbest.path.Count)
{
Pbest = new Path(path[Dindex]);
Vbest = Vhat;
}
else if (Vhat < Vbest)
{
Pbest = new Path(path[Dindex]);
Vbest = Vhat;
}
}
/*
* if ( Vhat == Vbest) {
* if (path[Dindex].path.Count < Pbest.path.Count) {
* Pbest = new Path(path[Dindex]);
* Vbest = Vhat;
* }
* } else if ( Vhat < Vbest) {
* Pbest = new Path(path[Dindex]);
* Vbest = Vhat;
* }*/
}
}
}
}
}