private void ComputeEndpoints() { m_startNavRef = m_navMesh.ComputeNavRefAtPoint(m_startPosition); m_endNavRef = m_navMesh.ComputeNavRefAtPoint(m_endPosition); if (m_startNavRef.IsValid) { if (m_endNavRef.IsValid) { if (m_navMesh.AreNavRefsConnected(m_startNavRef, m_endNavRef)) { if (m_startNavRef.Equals(m_endNavRef)) { // We can head directly to the destination m_finalPath.Add(new PathStep(m_startNavRef, m_startPosition)); m_finalPath.Add(new PathStep(m_endNavRef, m_endPosition)); m_state = eState.complete; } else { // We have to do the expensive query m_state = eState.setup_raw_path; } } else { m_state = eState.complete; m_resultCode = eResult.failed_start_off_nav_mesh; } } else { m_state = eState.complete; m_resultCode = eResult.failed_end_off_nav_mesh; } } else { m_state = eState.complete; m_resultCode = eResult.failed_start_off_nav_mesh; } }
public bool Raycast( Point3d start, NavRef startNavRef, Point3d end, NavRef endNavRef, out float t) { bool hit = false; if (!startNavRef.IsValid || !endNavRef.IsValid) { hit = true; t = 0.0f; } else if (startNavRef.Equals(endNavRef)) { hit = false; t = 1.0f; } else { uint maxRaycastIteration = m_rowCount + m_colomnCount; uint iterationCount = 0; NavRef currentNavRef = startNavRef; AABB2d currentNavCellBounds = ComputeNavCellBounds2d((uint)currentNavRef.NavCellIndex); Point2d start2d = start.ToPoint2d(); Vector2d rayDirection = (end - start).ToVector2d(); float tEpsilon = MathConstants.POSITIONAL_EPSILON / rayDirection.Magnitude(); t = 0.0f; while (t < 1.0f && !hit) { float clipMinT = 0.0f; float clipMaxT = 0.0f; // Compute where the ray exists the bounding box of the cell if (!currentNavCellBounds.ClipRay( start2d, rayDirection, out clipMinT, out clipMaxT)) { // If we failed to clip against the bounds that we were suppose to be inside of, // just use the current t and rely on the positional epsilon advancement to get // us back on track Debug.Assert(false, "Raycast didn't clip against box it was suppose to intersect"); clipMinT = t; clipMaxT = t; } if (clipMaxT < 1.0f) { // If we haven't gotten to the end of the ray yet, try to advance to the next nav cell Point2d newTestPoint = start2d + rayDirection * (clipMaxT + tEpsilon); // Find the adjacent neighboring cell, if any currentNavRef = ComputeNavRefAtPoint(newTestPoint); if (currentNavRef.IsValid) { currentNavCellBounds = ComputeNavCellBounds2d((uint)currentNavRef.NavCellIndex); if (currentNavRef.Equals(endNavRef)) { t = 1.0f; hit = false; } else { t = Math.Min(clipMaxT + tEpsilon, 1.0f); hit = false; } } else { t = clipMaxT; hit = true; } } else { // Made it all the way to the end without hitting anything t = 1.0f; hit = false; } // Safety iteration max to prevent infinite loops iterationCount++; if (iterationCount > maxRaycastIteration) { // Something funky happened. Just assume we can see to the end Debug.Assert(false, "Raycast hit iteration limit"); t = 1.0f; hit = false; break; } } } return(hit); }
public bool Raycast( Point3d start, NavRef startNavRef, Point3d end, NavRef endNavRef, out float t) { bool hit = false; if (!startNavRef.IsValid || !endNavRef.IsValid) { hit = true; t = 0.0f; } else if (startNavRef.Equals(endNavRef)) { hit = false; t = 1.0f; } else { uint maxRaycastIteration = m_rowCount + m_colomnCount; uint iterationCount = 0; NavRef currentNavRef = startNavRef; AABB2d currentNavCellBounds = ComputeNavCellBounds2d((uint)currentNavRef.NavCellIndex); Point2d start2d = start.ToPoint2d(); Vector2d rayDirection = (end - start).ToVector2d(); float tEpsilon = MathConstants.POSITIONAL_EPSILON / rayDirection.Magnitude(); t = 0.0f; while (t < 1.0f && !hit) { float clipMinT = 0.0f; float clipMaxT = 0.0f; // Compute where the ray exists the bounding box of the cell if (!currentNavCellBounds.ClipRay( start2d, rayDirection, out clipMinT, out clipMaxT)) { // If we failed to clip against the bounds that we were suppose to be inside of, // just use the current t and rely on the positional epsilon advancement to get // us back on track Debug.Assert(false, "Raycast didn't clip against box it was suppose to intersect"); clipMinT = t; clipMaxT = t; } if (clipMaxT < 1.0f) { // If we haven't gotten to the end of the ray yet, try to advance to the next nav cell Point2d newTestPoint = start2d + rayDirection * (clipMaxT + tEpsilon); // Find the adjacent neighboring cell, if any currentNavRef = ComputeNavRefAtPoint(newTestPoint); if (currentNavRef.IsValid) { currentNavCellBounds = ComputeNavCellBounds2d((uint)currentNavRef.NavCellIndex); if (currentNavRef.Equals(endNavRef)) { t = 1.0f; hit = false; } else { t = Math.Min(clipMaxT + tEpsilon, 1.0f); hit = false; } } else { t = clipMaxT; hit = true; } } else { // Made it all the way to the end without hitting anything t = 1.0f; hit = false; } // Safety iteration max to prevent infinite loops iterationCount++; if (iterationCount > maxRaycastIteration) { // Something funky happened. Just assume we can see to the end Debug.Assert(false, "Raycast hit iteration limit"); t = 1.0f; hit = false; break; } } } return hit; }