public void Test(Map map, Measurements measurements, List <Vector2> moves,
int iExpectedX, int iExpectedZ)
{
MonteCarloLocalizer loc = new MonteCarloLocalizer();
loc.Init(map);
int iX = (int)moves[0].x;
int iY = (int)moves[0].y;
loc.Move(iX, iY, 1f);
loc.Sense(measurements, 1f);
int mlX, mlZ;
loc.MostLikelyCell(out mlX, out mlZ);
if (iExpectedX == mlX && iExpectedZ == mlZ)
{
Debug.Log("Correct!");
}
else
{
Debug.LogError("Oops");
}
}
void SenseEnv(int iMoveX, int iMoveZ)
{
Marker[] allMarkers = GameObject.FindObjectsOfType <Marker>();
Vector3 pos = transform.position;
Measurements ma = new Measurements();
ma.Init(100);
foreach (Marker marker in allMarkers)
{
Vector3 delta = (marker.transform.position - pos);
if (delta.magnitude < radiusSense)
{
int offX = Mathf.RoundToInt(delta.x / mm.map.dX);
int offZ = Mathf.RoundToInt(delta.z / mm.map.dZ);
Measurement m = new Measurement(marker.id, offX, offZ);
ma.m.Add(m);
}
}
//first move is huge and not needed.
if (prevPos != Vector3.zero)
{
loc.Move(iMoveX, iMoveZ, probMoveExact);
}
loc.Sense(ma, probSenseExact);
Vector3 likelyPos = Vector3.zero;
likelyTM.position = likelyPos;
}