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); }