private void ComputeSmoothedPath() { // Convert the raw path steps into a proper path step sequence // Always add the first step m_finalPath.Add(new PathStep(m_startNavRef, m_startPosition)); // Add a step at the midpoint between each neighboring nav-cell in the path for (uint rawStepIndex = 1; rawStepIndex < m_rawPath.Count; rawStepIndex++) { NavRef previousNavRef = m_rawPath[(int)rawStepIndex - 1]; NavRef currentNavRef = m_rawPath[(int)rawStepIndex]; Point3d portalLeft, portalRight; if (m_navMesh.ComputePortalPoints( previousNavRef, currentNavRef, out portalLeft, out portalRight)) { Point3d portalMidpoint = Point3d.Interpolate(portalLeft, portalRight, 0.5F); m_finalPath.Add(new PathStep(currentNavRef, portalMidpoint)); } } // Always add the lest step m_finalPath.Add(new PathStep(m_endNavRef, m_endPosition)); // TODO: Remove the extraneous path steps using funnel algorithm + ray casting m_state = eState.complete; }
private List <Point3d> InterpolatePoints(Line line, int n) { List <Point3d> tempP = new List <Point3d>(n + 1); double[] t = LinSpace(0, 1, n + 1); for (int i = 0; i < t.Length; i++) { var tPm = new Point3d(); tPm.Interpolate(line.From, line.To, t[i]); tPm = new Point3d(Math.Round(tPm.X, 4), Math.Round(tPm.Y, 4), Math.Round(tPm.Z, 4)); tempP.Add(tPm); } return(tempP); }