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