private void testMultiplication()
{
DirtySet d = new DirtySet(N);
d.dirtySet = new int[] { 1, 0, 0, 0, 1 };
int[] robots = new int[] { 4, 1 };
int[][] Ahat = modifyAdjacency(A, robots);
/*
* for (int i=0; i<N; i++) {
* string log = "";
* for (int j=0; j<N; j++) {
* log += Ahat[i][j] + " ";
* }
* Debug.Log(log);
* }*/
DirtySet Dhat = d.multiplication(Ahat);
Dhat.print();
}
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;
* }*/
}
}
}
}
}