public void SolveContinuumCrowdsForTile(CC_Tile tile, List <Location> goal)
{
f = tile.f;
C = tile.C;
g = tile.g;
N = f.GetLength(0);
M = f.GetLength(1);
if (N == 0 || M == 0)
{
Debug.Log("Eikonal Solver initiated with 0-dimension");
}
Phi = new float[N, M];
dPhi = new Vector2[N, M];
velocity = new Vector2[N, M];
accepted = new bool[N, M];
this.goal = new bool[N, M];
considered = new FastPriorityQueue <FastLocation>(N * M);
neighbor = new FastLocation(0, 0);
computeContinuumCrowdsFields(goal);
}
// 0000000000000000000000000000000000000000000000000000000000
// 0000000000000000000000000000000000000000000000000000000000
public void drawRhoOnTile(Location l)
{
CC_Tile cct = getLocalTile(l);
NavSystem.S._DEBUG_VISUAL_plotTileFields(new Vector2(cct.myLoc.x, cct.myLoc.y), cct.rho);
// NavSystem.S.theMapAnalyzer.printOutMatrix (cct.f);
}
private bool initiateTiles(List <MapTile> tiles)
{
if (tiles.Count == 0)
{
throw new NavSystemException("CCDynamicGlobalFields: No Tiles");
}
tileSize = tiles[0].TileSize;
// instantiate all our tiles
for (int i = 0; i < tiles.Count; i++)
{
// create a new tile based on this location
var cct = new CC_Tile(tiles[i]);
// save the tile
_tiles.Add(cct.Corner, cct);
}
// initialize some tile values
foreach (CC_Tile cct in _tiles.Values)
{
// initialize the cost and speed field
computeSpeedField(cct);
computeCostField(cct);
// store the current data in memory
cct.StoreCurrentSpeedAndCostFields();
}
return(true);
}
private void writeDataToPoint_vAve(int xGlobal, int yGlobal, Vector2 val)
{
Location l = new Location((int)Math.Floor(((double)xGlobal) / ((double)tileSize)),
(int)Math.Floor(((double)yGlobal) / ((double)tileSize)));
CC_Tile localTile = getLocalTile(l);
int xTile = xGlobal - l.x * tileSize;
int yTile = yGlobal - l.y * tileSize;
_tiles [localTile.myLoc].writeData_vAve(xTile, yTile, val);
}
private void writeDataToPoint_gP(int xGlobal, int yGlobal, float val)
{
Location l = new Location((int)Math.Floor(((double)xGlobal) / ((double)tileSize)),
(int)Math.Floor(((double)yGlobal) / ((double)tileSize)));
CC_Tile localTile = getLocalTile(l);
int xTile = xGlobal - l.x * tileSize;
int yTile = yGlobal - l.y * tileSize;
localTile.writeData_gP(xTile, yTile, val);
_tiles [localTile.myLoc] = localTile;
}
private Vector4 readDataFromPoint_C(int xGlobal, int yGlobal)
{
Location l = new Location((int)Math.Floor(((double)xGlobal) / ((double)tileSize)),
(int)Math.Floor(((double)yGlobal) / ((double)tileSize)));
CC_Tile localTile = getLocalTile(l);
int xTile = xGlobal - l.x * tileSize;
int yTile = yGlobal - l.y * tileSize;
Vector4 f = localTile.readData_C(xTile, yTile);
return(f);
}
private void computeSpeedField(CC_Tile cct)
{
for (int n = 0; n < tileSize; n++)
{
for (int m = 0; m < tileSize; m++)
{
for (int d = 0; d < DIR_ENWS.Length; d++)
{
cct.f [n, m] [d] = computeSpeedFieldPoint(n, m, cct, DIR_ENWS [d]);
}
}
}
}
private void computeSpeedField(CC_Tile cct)
{
for (int n = 0; n < tileSize; n++)
{
for (int m = 0; m < tileSize; m++)
{
for (int d = 0; d < CCValues.S.ENSW.Length; d++)
{
cct.f[n, m][d] = computeSpeedFieldPoint(n, m, cct, CCValues.S.ENSW[d]);
}
}
}
}
private void computeCostField(CC_Tile cct)
{
for (int n = 0; n < tileSize; n++)
{
for (int m = 0; m < tileSize; m++)
{
for (int d = 0; d < DIR_ENWS.Length; d++)
{
cct.C [n, m] [d] = computeCostFieldValue(n, m, d, DIR_ENWS [d], cct);
}
}
}
}
private void computeCostField(CC_Tile cct)
{
for (int n = 0; n < tileSize; n++)
{
for (int m = 0; m < tileSize; m++)
{
for (int d = 0; d < CCValues.S.ENSW.Length; d++)
{
cct.C[n, m][d] = computeCostFieldValue(n, m, d, CCValues.S.ENSW[d], cct);
}
}
}
}
private float computeSpeedFieldPoint(int tileX, int tileY, CC_Tile cct, Vector2 direction)
{
int xLocalInto = tileX + (int)direction.x;
int yLocalInto = tileY + (int)direction.y;
int xGlobalInto = cct.Corner.x * tileSize + xLocalInto;
int yGlobalInto = cct.Corner.y * tileSize + yLocalInto;
// otherwise, run the speed field calculation
float ff, ft, fv;
float r;
// test to see if the point we're looking INTO is in another tile, and if so, pull it
if (xLocalInto < 0 || xLocalInto > tileSize - 1 || yLocalInto < 0 || yLocalInto > tileSize - 1)
{
// if we're looking off the map, dont store this value
if (!isPointValid(xGlobalInto, yGlobalInto))
{
return(CCValues.S.f_speedMin);
}
r = readDataFromPoint_rho(xGlobalInto, yGlobalInto);
}
else
{
r = cct.rho[xLocalInto, yLocalInto];
}
// test the density INTO WHICH we move:
if (r < CCValues.S.f_rhoMin)
{
// rho < rho_min calc
ft = computeTopographicalSpeed(readDataFromPoint_dh(xGlobalInto, yGlobalInto), direction);
ff = ft;
}
else if (r > CCValues.S.f_rhoMax)
{
// rho > rho_max calc
fv = computeFlowSpeed(xGlobalInto, yGlobalInto, direction);
ff = fv;
}
else
{
// rho in-between calc
fv = computeFlowSpeed(xGlobalInto, yGlobalInto, direction);
ft = computeTopographicalSpeed(readDataFromPoint_dh(xGlobalInto, yGlobalInto), direction);
ff = ft + (r - CCValues.S.f_rhoMin) / (CCValues.S.f_rhoMax - CCValues.S.f_rhoMin) * (fv - ft);
}
ff = Mathf.Clamp(ff, CCValues.S.f_speedMin, CCValues.S.f_speedMax);
return(Math.Max(CCValues.S.f_speedMin, ff));
}
// IMPORTANT: in this function call, x and y are LOCAL to the tile
private float computeSpeedFieldPoint(int tileX, int tileY, CC_Tile cct, Vector2 direction)
{
int xLocalInto = tileX + (int)direction.x;
int yLocalInto = tileY + (int)direction.y;
int xGlobalInto = cct.myLoc.x * tileSize + xLocalInto;
int yGlobalInto = cct.myLoc.y * tileSize + yLocalInto;
// otherwise, run the speed field calculation
float ff = 0, ft = 0, fv = 0;
float r;
// test to see if the point we're looking INTO is in another tile, and if so, pull it
if ((xLocalInto < 0) || (xLocalInto > tileSize - 1) || (yLocalInto < 0) || (yLocalInto > tileSize - 1))
{
// if we're looking off the map, dont store this value
if (!isPointValid(xGlobalInto, yGlobalInto))
{
return(CCvals.f_speedMin);
}
r = readDataFromPoint_rho(xGlobalInto, yGlobalInto);
}
else
{
r = cct.rho [xLocalInto, yLocalInto];
}
// test the density INTO WHICH we move:
if (r < CCvals.f_rhoMin) // rho < rho_min calc
{
ft = computeTopographicalSpeed(tileX, tileY, theMapData.getHeightGradientMap(tileX, tileY), direction);
ff = ft;
}
else if (r > CCvals.f_rhoMax) // rho > rho_max calc
{
fv = computeFlowSpeed(xGlobalInto, yGlobalInto, direction);
ff = fv;
}
else // rho in-between calc
{
fv = computeFlowSpeed(xGlobalInto, yGlobalInto, direction);
ft = computeTopographicalSpeed(tileX, tileY, theMapData.getHeightGradientMap(tileX, tileY), direction);
ff = ft + (r - CCvals.f_rhoMin) / (CCvals.f_rhoMax - CCvals.f_rhoMin) * (fv - ft);
}
return(Math.Max(CCvals.f_speedMin, ff));
}
// average velocity fields will just iterate over each tile, since information
// doesnt 'bleed' into or out from nearby tiles
private void computeAverageVelocityField(CC_Tile cct)
{
for (int n = 0; n < tileSize; n++)
{
for (int m = 0; m < tileSize; m++)
{
Vector2 v = cct.vAve [n, m];
float r = cct.rho [n, m];
if (r != 0)
{
v /= r;
}
cct.vAve [n, m] = v;
}
}
}
// ******************************************************************************************
// functions used for reading and writing to tiles
// ******************************************************************************************
private CC_Tile getLocalTile(Location l)
{
if (current_Tile_Loc == l)
{
return(current_Tile);
}
if (_tiles.ContainsKey(l))
{
current_Tile = _tiles [l];
current_Tile_Loc = current_Tile.myLoc;
return(current_Tile);
}
else
{
return(new CC_Tile(0, l));
}
}
public bool initiateTiles()
{
// take map dimensions
// if tileSize and map dimensions dont fit perfectly, drop a flag
// otherwise, create all the tiles
// make sure the map dimensions are divisible by tileSize
if ((((float)_mapX) % ((float)tileSize) != 0) ||
(((float)_mapY) % ((float)tileSize) != 0))
{
// this should NEVER HAPPEN, so send an error if it does
return(false);
}
else
{
Location loc;
int numTilesX = _mapX / tileSize;
int numTilesY = _mapY / tileSize;
// instantiate all our tiles
for (int x = 0; x < numTilesX; x++)
{
for (int y = 0; y < numTilesY; y++)
{
// create a new tile based on this location
loc = new Location(x, y);
CC_Tile cct = new CC_Tile(tileSize, loc);
_tiles.Add(loc, cct);
}
}
if (_tiles.Keys.Count > 0)
{
current_Tile = _tiles[new Location(0, 0)];
current_Tile_Loc = current_Tile.myLoc;
}
}
return(true);
}
private float computeCostFieldValue(int tileX, int tileY, int d, Vector2 direction, CC_Tile cct)
{
int xLocalInto = tileX + (int)direction.x;
int yLocalInto = tileY + (int)direction.y;
int xGlobalInto = cct.myLoc.x * tileSize + xLocalInto;
int yGlobalInto = cct.myLoc.y * tileSize + yLocalInto;
// if we're looking in an invalid direction, dont store this value
if (cct.f [tileX, tileY] [d] == 0)
{
return(Mathf.Infinity);
}
else if (!isPointValid(xGlobalInto, yGlobalInto))
{
return(Mathf.Infinity);
}
// test to see if the point we're looking INTO is in a DIFFERENT tile, and if so, pull it
float gP;
if ((xLocalInto < 0) || (xLocalInto > tileSize - 1) || (yLocalInto < 0) || (yLocalInto > tileSize - 1))
{
gP = readDataFromPoint_gP(xGlobalInto, yGlobalInto);
}
else
{
gP = cct.gP [xLocalInto, yLocalInto];
}
float cost = (cct.f [tileX, tileY] [d] * CCvals.C_alpha + CCvals.C_beta + gP * CCvals.C_gamma) / cct.f [tileX, tileY] [d];
return(cost);
}
private float computeCostFieldValue(int tileX, int tileY, int d, Vector2 direction, CC_Tile cct)
{
int xLocalInto = tileX + (int)direction.x;
int yLocalInto = tileY + (int)direction.y;
int xGlobalInto = cct.Corner.x * tileSize + xLocalInto;
int yGlobalInto = cct.Corner.y * tileSize + yLocalInto;
// if we're looking in an invalid direction, dont store this value
if (cct.f[tileX, tileY][d] == 0 || !isPointValid(xLocalInto, yLocalInto))
{
return(Mathf.Infinity);
}
// initialize g as the map discomfort data value
float g = readDataFromPoint_g(xGlobalInto, yGlobalInto);
// test to see if the point we're looking INTO is in a DIFFERENT tile, and if so, pull it
if (xLocalInto < 0 || xLocalInto > tileSize - 1 ||
yLocalInto < 0 || yLocalInto > tileSize - 1
)
{
g += readDataFromPoint_g(xGlobalInto, yGlobalInto);
}
else
{
g += cct.g[xLocalInto, yLocalInto];
}
// clamp g to make sure it's not > 1
if (g > 1)
{
g = 1;
}
else if (g < 0)
{
g = 0;
}
// compute the cost weighted by our coefficients
var f = cct.f[tileX, tileY][d];
float cost = CCValues.S.C_alpha
+ CCValues.S.C_beta * 1 / f
+ CCValues.S.C_gamma * g / f;
return(cost);
}
