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