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);
}
private bool isEikonalLocationInsideLocalGrid(FastLocation l)
{
if (l.x < 0 || l.y < 0 || l.x > N - 1 || l.y > M - 1)
{
return(false);
}
return(true);
}
private bool isEikonalLocationAcceptedandValid(FastLocation l)
{
if (isLocationAccepted(l))
{
return(false);
}
if (!isPointValid(l))
{
return(false);
}
return(true);
}
private bool isEikonalLocationValidToMoveInto(FastLocation l)
{
// location must be tested to ensure that it does not attempt to assess a point
// that is not valid to move into. a valid point is:
// 1) not outisde the local grid
// 2) NOT accepted
// 3) NOT on a global discomfort grid
// (this occurs in isPointValid() )
// 4) NOT outside the global grid
// (this occurs in isPointValid() )
if (!isEikonalLocationInsideLocalGrid(l))
{
return(false);
}
return(isEikonalLocationAcceptedandValid(l));
}
private bool isEikonalLocationValidAsNeighbor(FastLocation l)
{
// A valid neighbor point is:
// 1) not outisde the local grid
// 3) NOT in the goal
// (everything below this is checked elsewhere)
// 2) NOT accepted
// 4) NOT on a global discomfort grid
// (this occurs in isPointValid() )
// 5) NOT outside the global grid
// (this occurs in isPointValid() )
if (!isEikonalLocationInsideLocalGrid(l))
{
return(false);
}
if (isLocationInGoal(l))
{
return(false);
}
return(isEikonalLocationAcceptedandValid(l));
}
// *************************************************************************
// THE EIKONAL SOLVER
// *************************************************************************
/// <summary>
/// The Eikonal Solver using the Fast Marching approach
/// </summary>
/// <param name="fields"></param>
/// <param name="goal"></param>
private void EikonalSolver(List <Location> goal)
{
// start by assigning all values of potential a huge number to in-effect label them 'far'
for (int n = 0; n < N; n++)
{
for (int m = 0; m < M; m++)
{
Phi[n, m] = Mathf.Infinity;
}
}
// initiate by setting potential to 0 in the goal, and adding all goal locations to the considered list
// goal locations will naturally become accepted nodes, as their 0-cost gives them 1st priority, and this
// way, they are quickly added to accepted, while simultaneously propagating their lists and beginning the
// algorithm loop
FastLocation loc;
foreach (Location l in goal)
{
if (isPointValid(l.x, l.y))
{
Phi[l.x, l.y] = 0f;
loc = new FastLocation(l.x, l.y);
markGoal(loc);
considered.Enqueue(loc, 0f);
}
}
/// THE EIKONAL UPDATE LOOP
// next, we start the eikonal update loop, by initiating it with each goal point as 'considered'.
// this will check each neighbor to see if it's a valid point (EikonalLocationValidityTest==true)
// and if so, update if necessary
while (considered.Count > 0)
{
FastLocation current = considered.Dequeue();
EikonalUpdateFormula(current);
markAccepted(current);
}
}
public static bool Equals(FastLocation l1, FastLocation l2)
{
return(l1.Equals(l2));
}
public bool Equals(FastLocation fast)
{
return((fast != null) && (fast == this));
}
private bool isPointValid(FastLocation l)
{
return(isPointValid(l.x, l.y));
}
private bool isLocationInGoal(FastLocation l)
{
return(goal[l.x, l.y]);
}
private void markGoal(FastLocation l)
{
goal[l.x, l.y] = true;
}
private bool isLocationAccepted(FastLocation l)
{
return(accepted[l.x, l.y]);
}
private void markAccepted(FastLocation l)
{
accepted[l.x, l.y] = true;
}
/// <summary>
/// The EikonalUpdateFormula computes the actual solution potential
/// </summary>
/// <param name="l"></param>
private void EikonalUpdateFormula(FastLocation l)
{
float phi_proposed = Mathf.Infinity;
// cycle through directions to check all neighbors and perform the eikonal
// update cycle on them
for (int d = 0; d < DIR_ENWS.Length; d++)
{
int xInto = l.x + (int)DIR_ENWS[d].x;
int yInto = l.y + (int)DIR_ENWS[d].y;
neighbor = new FastLocation(xInto, yInto);
if (isEikonalLocationValidAsNeighbor(neighbor))
{
// The point is valid. Now, we pull values from THIS location's
// 4 neighbors and use them in the calculation
Vector4 phi_m;
phi_m = Vector4.one * Mathf.Infinity;
// track cost of moving into each nearby space
for (int dd = 0; dd < DIR_ENWS.Length; dd++)
{
int xIInto = neighbor.x + (int)DIR_ENWS[dd].x;
int yIInto = neighbor.y + (int)DIR_ENWS[dd].y;
if (isEikonalLocationValidToMoveInto(new FastLocation(xIInto, yIInto)))
{
phi_m[dd] = Phi[xIInto, yIInto] + C[neighbor.x, neighbor.y][dd];
}
}
// select out cheapest
float phi_mx = Math.Min(phi_m[0], phi_m[2]);
float phi_my = Math.Min(phi_m[1], phi_m[3]);
// now assign C_mx based on which direction was chosen
float C_mx = phi_mx == phi_m[0] ? C[neighbor.x, neighbor.y][0] : C[neighbor.x, neighbor.y][2];
// now assign C_my based on which direction was chosen
float C_my = phi_my == phi_m[1] ? C[neighbor.x, neighbor.y][1] : C[neighbor.x, neighbor.y][3];
// solve for our proposed Phi[neighbor] value using the quadratic solution to the
// approximation of the eikonal equation
float C_mx_Sq = C_mx * C_mx;
float C_my_Sq = C_my * C_my;
float phi_mDiff_Sq = (phi_mx - phi_my) * (phi_mx - phi_my);
float valTest = C_mx_Sq + C_my_Sq - 1f / (C_mx_Sq * C_my_Sq);
//float valTest = C_mx_Sq + C_my_Sq - 1f;
// test the quadratic
if (phi_mDiff_Sq > valTest)
{
// use the simplified solution for phi_proposed
float phi_min = Math.Min(phi_mx, phi_my);
float cost_min = phi_min == phi_mx ? C_mx : C_my;
phi_proposed = cost_min + phi_min;
}
else
{
// solve the quadratic
var radical = Math.Sqrt(C_mx_Sq * C_my_Sq * (C_mx_Sq + C_my_Sq - phi_mDiff_Sq));
var soln1 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my + radical) / (C_mx_Sq + C_my_Sq);
var soln2 = (C_my_Sq * phi_mx + C_mx_Sq * phi_my - radical) / (C_mx_Sq + C_my_Sq);
// max - prefers diagonals
//phi_proposed = (float)Math.Max(soln1, soln2);
// min - prefers cardinals
//phi_proposed = (float)Math.Min(soln1, soln2);
// mean - better mix but still prefer diagonals
//phi_proposed = (float)(soln1 + soln2) / 2;
// geometric mean - seems identical to mean
//phi_proposed = (float)Math.Sqrt(soln1 * soln2);
// weighted mean - appears to offer the best compromise
var max = (float)Math.Max(soln1, soln2);
var min = (float)Math.Min(soln1, soln2);
float wMax = CCValues.S.maxWeight;
float wMin = CCValues.S.minWeight;
phi_proposed = (float)(max * wMax + min * wMin) / (wMax + wMin);
}
// we now have a phi_proposed
// we are re-writing the phi-array real time, so we simply compare to the current slot
if (phi_proposed < Phi[neighbor.x, neighbor.y])
{
// save the value of the lower phi
Phi[neighbor.x, neighbor.y] = phi_proposed;
if (considered.Contains(neighbor))
{
// re-write the old value in the queue
considered.UpdatePriority(neighbor, phi_proposed);
}
else
{
// -OR- add this value to the queue
considered.Enqueue(neighbor, Phi[neighbor.x, neighbor.y]);
}
}
}
}
}