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