public void GenerateMMatrices()
{
CarNavigationEnvironment testws = new CarNavigationEnvironment();
Ma = new SparseMatrix[CarNavigationQStore.LENACTION];
for (int i = 0; i < CarNavigationQStore.LENACTION; ++i)
{
Ma[i] = new SparseMatrix(statenum);
}
for (int ii = 0; ii < statenum; ++ii)
{
int l = ii / (CarNavigationQStore.LENXY * CarNavigationQStore.LENXY);
int iil = ii - l * (CarNavigationQStore.LENXY * CarNavigationQStore.LENXY);
int k = iil / CarNavigationQStore.LENXY;
int j = iil % CarNavigationQStore.LENXY;
CarNavigationState state;
CarNavigationQStore.GetState(j, k, l, out state);
for (int i = 0; i < CarNavigationQStore.LENACTION; ++i)
{
CarNavigationAction action;
CarNavigationQStore.GetAction(i, out action);
for (int i1 = 0; i1 < 100; ++i1)
{
testws.x = state.x + r.NextDouble() * (CarNavigationQStore.MAXXY - CarNavigationQStore.MINXY) / CarNavigationQStore.LENXY;
testws.y = state.y + r.NextDouble() * (CarNavigationQStore.MAXXY - CarNavigationQStore.MINXY) / CarNavigationQStore.LENXY;
testws.alpha = state.alpha + r.NextDouble() * (2 * Math.PI) / CarNavigationQStore.LENANG;
testws.Step(action.ang);
int j2, k2, l2;
CarNavigationState state2 = new CarNavigationState(testws.x, testws.y, testws.alpha);
CarNavigationQStore.GetStateIndices(state2, out j2, out k2, out l2);
int ii2 = l2 * CarNavigationQStore.LENXY * CarNavigationQStore.LENXY + k2 * CarNavigationQStore.LENXY + j2;
if (ii2 > statenum - 1) throw new Exception();
Ma[i][ii, ii2] += 0.01f;
}
}
}
SaveMMatrices("mmatrices.dat");
}
public static void GetStateAndAction(int i, int j, int k, int l, out CarNavigationState state, out CarNavigationAction action)
{
GetState(j, k, l, out state);
GetAction(i, out action);
}
public static void GetIndices(CarNavigationState state, CarNavigationAction action, out int i, out int j, out int k, out int l)
{
GetStateIndices(state, out j, out k, out l);
GetActionIndices(action, out i);
}
public static void GetStateIndices(CarNavigationState state, out int j, out int k, out int l)
{
j = (int)ComMath.NormalLim(state.x, MINXY, MAXXY, 0, LENXY - 1);
k = (int)ComMath.NormalLim(state.y, MINXY, MAXXY, 0, LENXY - 1);
l = (int)ComMath.NormalLim(state.alpha, -Math.PI, Math.PI, 0, LENANG - 1);
}
public static void GetState(int j, int k, int l, out CarNavigationState state)
{
double x = ComMath.Normal(j, 0, LENXY - 1, MINXY, MAXXY);
double y = ComMath.Normal(k, 0, LENXY - 1 , MINXY, MAXXY);
double alpha = ComMath.Normal(l, 0, LENANG - 1, -Math.PI, Math.PI);
state = new CarNavigationState(x, y, alpha);
}
