public bool FindPath(Point3d start, Point3d end, LinkedList <Point3d> path)
{
bool ret = false;
path.Clear();
RTree.Point point = new RTree.Point((float)end.X, (float)end.Y, (float)end.Z);
List <MeshElement> list = m_rtree.Nearest(point, 10.0f);
MeshElement nearest = null;
float minDis = float.MaxValue;
foreach (MeshElement e in list)
{
float dis = (float)e.triangle.Centroid.DistanceTo(end);
if (dis < minDis)
{
nearest = e;
minDis = dis;
}
}
if (nearest == null)
{
return(false);
}
ret = FindPath(nearest, start, end, path);
ClearState();
return(ret);
}
public void LoadNavMesh(DNavM mesh)
{
foreach (Common.DTngl tr in mesh.list)
{
MeshElement me = new MeshElement(tr.p);
AddElement(me);
}
foreach (KeyValuePair <int, MeshElement> kv in this.m_meshDict)
{
List <MeshElement> meshList = m_rtree.Intersects(kv.Value.bounding);
foreach (MeshElement element in meshList)
{
if (element.meshId != kv.Value.meshId)
{
kv.Value.SetNeighbor(element);
}
}
int count = 0;
foreach (MeshElement n in kv.Value.neighbor)
{
if (n != null)
{
count++;
}
}
if (count == 0)
{
m_rtree.Delete(kv.Value.bounding, kv.Value);
continue;
}
}
}
public void SetNeighbor(MeshElement meshTriangle)
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
float d1 = (float)m_point[i].DistanceTo(meshTriangle.m_point[(j + 1) % 3]);
float d2 = (float)m_point[(i + 1) % 3].DistanceTo(meshTriangle.m_point[j]);
if (d1 < 0.1f && d2 < 0.1f)
{
int edge = i + (i + 1) % 3 - 1;
float dev = MathF.Sqrt(d1 * d1 + d2 * d2);
if (deviation[edge] > dev)
{
neighbor[edge] = meshTriangle;
deviation[edge] = dev;
}
}
}
}
}
void AddElement(MeshElement element)
{
m_meshDict.Add(element.meshId, element);
m_rtree.Add(element.bounding, element);
}
public Step(MeshElement e)
{
element = e;
}
bool FindPath(MeshElement start, Point3d startPos, Point3d endPos, LinkedList <Point3d> path)
{
Point3d currentPos = start.triangle.Centroid.Copy();
float distance = Distance(startPos, endPos);
Step step = new Step(start);
step.costS2C = 0;
step.costS2E = distance;
m_open.Add(start.meshId, step);
m_openSorted.Add(step);
while (m_openSorted.Count != 0)
{
IEnumerator <Step> e = m_openSorted.GetEnumerator();
if (!e.MoveNext())
{
return(false);
}
Step currentStep = e.Current;
int id = currentStep.element.meshId;
m_openSorted.Remove(currentStep);
m_open.Remove(id);
MeshElement element = currentStep.element;
if (TriangleContainPoint(element.triangle, endPos))
{
// Success ...
ConstructPath(currentStep, endPos, path);
return(true);
}
m_close.Add(id);
foreach (MeshElement neighbor in element.neighbor)
{
if (neighbor == null)
{
continue;
}
if (m_close.Contains(neighbor.meshId))
{
continue;
}
// start position to this neighbor piece cost
float costS2C = currentStep.costS2C + Distance(neighbor.triangle.Centroid, endPos);
if (m_open.ContainsKey(neighbor.meshId))
{
Step s = m_open.GetValueOrDefault(neighbor.meshId);
if (s != null && costS2C < s.costS2C)
{
m_openSorted.Remove(s);
s.costS2C = costS2C;
s.prev = currentStep;
m_openSorted.Add(s);
}
}
else
{
Step nextStep = new Step(neighbor);
nextStep.costS2C = costS2C;
nextStep.costS2E = costS2C + CostEstimate(startPos, neighbor.triangle.Centroid, endPos);
m_open.Add(neighbor.meshId, nextStep);
m_openSorted.Add(nextStep);
nextStep.prev = currentStep;
}
}
}
return(false);
}
