protected override void ActorMoveUpdate(GameObject actor)
{
base.ActorMoveUpdate(actor);
PointBasedFoundPath path = (PointBasedFoundPath)pathTable[actor];
if (!path.hasStarted)
{
Services.gameEvents.SetManTargetPosition(actor, path.pathEdges[0].EndPos(), path.tol);
path.hasStarted = true;
}
//if (GoToNextPoint(actor, path.pathEdges[0].EndPos(), path.speed))
if (Vector3.Distance(actor.transform.position, path.pathEdges[0].EndPos()) <= path.tol)
{
if (path.pathEdges.Count <= 1)
{
// arrived at the final position
if (path.endEvent != null)
{
Services.eventManager.QueueEvent(path.endEvent);
}
pathTable.Remove(actor);
Services.gameEvents.StopMan(actor);
}
else
{
// walking out of path 0
//GameObject middlePoint = path.pathEdges[0].AcrossPoint();
//if (middlePoint.GetComponent<PathPoint>().followObject != null && middlePoint.GetComponent<PathPoint>().isBorder)
//{
// Services.gameController.ManAcrossBorder(actor, middlePoint.GetComponent<PathPoint>().followObject);
//}
Services.gameEvents.SetManTargetPosition(actor, path.pathEdges[1].EndPos(), path.tol);
}
path.pathEdges.RemoveAt(0);
}
}
private bool FindPath(Vector3 startPos, Vector3 endPos, float clampTol)
{
// re-number all the pathpoints
Dictionary <GameObject, int> IDs = new Dictionary <GameObject, int>();
int N = 0;
foreach (GameObject pathPoint in pathPoints)
{
IDs[pathPoint] = N;
++N;
}
// init path matrix
float[,] path = new float[pathPoints.Count, pathPoints.Count];
for (int i = 0; i < N; ++i)
{
for (int j = 0; j < N; ++j)
{
path[i, j] = maxDistance;
}
}
foreach (GameObject pathEdge in pathEdges)
{
GameObject p0 = pathEdge.GetComponent <PathEdge>().P0();
GameObject p1 = pathEdge.GetComponent <PathEdge>().P1();
int id0 = IDs[p0];
int id1 = IDs[p1];
path[id0, id1] = Distance(p0, p1);
path[id1, id0] = path[id0, id1];
}
//int s = IDs[FindNearestPathPoint(startPos)];
//int t = IDs[FindNearestPathPoint(endPos)];
GameObject es = FindNearestPathEdge(startPos);
GameObject et = FindNearestPathEdge(endPos);
Debug.Log(es + " " + et);
if (es == null || et == null)
{
return(false);
}
// move in the same "zone"
if (es == et)
{
//Debug.Log("same");
recentPath = new FoundPath();
Vector3 validEndPos = Clamp(et.GetComponent <PathEdge>(), endPos);
if (Vector3.Distance(validEndPos, endPos) > clampTol)
{
return(false);
}
recentPath.Insert(new DirectedPathEdgeOnPositions(startPos, validEndPos));
}
else
{
float[] d = new float[N];
for (int i = 0; i < N; ++i)
{
d[i] = maxDistance;
}
int esp0 = IDs[es.GetComponent <PathEdge>().P0()];
int esp1 = IDs[es.GetComponent <PathEdge>().P1()];
int etp0 = IDs[et.GetComponent <PathEdge>().P0()];
int etp1 = IDs[et.GetComponent <PathEdge>().P1()];
d[esp0] = Vector3.Distance(es.GetComponent <PathEdge>().P0().transform.position, startPos);
d[esp1] = Vector3.Distance(es.GetComponent <PathEdge>().P1().transform.position, startPos);
//d[s] = 0;
prePathPoint = new int[N];
List <int> queue = new List <int>();
bool[] inQueue = new bool[N];
for (int i = 0; i < N; ++i)
{
inQueue[i] = false;
prePathPoint[i] = -1;
}
// SPFA starts from two points
int head = 0;
int tail = 0;
queue.Add(esp0);
++tail;
inQueue[esp0] = true;
queue.Add(esp1);
++tail;
inQueue[esp1] = true;
//queue.Add(s);
//++tail;
//inQueue[s] = true;
while (head < tail)
{
int o = queue[head];
inQueue[o] = false;
for (int i = 0; i < N; ++i)
{
if (d[i] > d[o] + path[o, i])
{
d[i] = d[o] + path[o, i];
if (!inQueue[i])
{
queue.Add(i);
inQueue[i] = true;
++tail;
prePathPoint[i] = o;
}
}
}
++head;
}
// two end points - choose the nearer one
float endDis0 = d[etp0] + Vector3.Distance(et.GetComponent <PathEdge>().P0().transform.position, endPos);
float endDis1 = d[etp1] + Vector3.Distance(et.GetComponent <PathEdge>().P1().transform.position, endPos);
//Debug.Log(endDis0 + " " + endDis1);
if (endDis0 >= maxDistance && endDis1 >= maxDistance)
{
return(false);
}
// construct result path
recentPath = new PointBasedFoundPath();
int curPoint = etp0;
if (endDis0 > endDis1)
{
curPoint = etp1;
}
Vector3 validEndPos = Clamp(et.GetComponent <PathEdge>(), endPos);
if (Vector3.Distance(validEndPos, endPos) > clampTol)
{
return(false);
}
recentPath.Insert(new DirectedPathEdgeOnObjectAndPosition(pathPoints[curPoint], validEndPos));
while (true)
{
int prePoint = prePathPoint[curPoint];
if (prePoint != -1)
{
recentPath.Insert(new DirectedPathEdgeOnTwoObjects(pathPoints[prePoint], pathPoints[curPoint]));
}
else
{
break;
}
curPoint = prePathPoint[curPoint];
}
recentPath.Insert(new DirectedPathEdgeOnPositionAndObject(startPos, pathPoints[curPoint]));
}
return(true);
}
