/// <summary> /// returns the nearest point in the path to the givenPoint /// @@TODO: Dynamic path stuff /// </summary> public Vector3 NearestPoint(Vector3 givenPoint) { Vector3 pos; if (transform != null) { pos = transform.position; } else { pos = new Vector3(0, 0, 0); } if ((!DynamicPath && IsValidPath()) || (SetupPointData() && IsValidPath())) { // offset, rotate and scale point to get it in the same world space as path givenPoint = FFMatrix3X3.ScaleBy(Quaternion.Inverse(transform.rotation) * (givenPoint - pos), new Vector3(1.0f / transform.lossyScale.x, 1.0f / transform.lossyScale.y, 1.0f / transform.lossyScale.z)); Vector3 nearestPoint = FFVector3.VecMaxValue; float nearestDist = float.MaxValue; foreach (Vector3 point in points) { float distToPoint = (point - givenPoint).sqrMagnitude; if (distToPoint < nearestDist) { nearestDist = distToPoint; nearestPoint = point; } } return(pos + (transform.rotation * FFMatrix3X3.ScaleBy(nearestPoint, transform.lossyScale))); } else { Debug.LogError("NearestPoint failed to setup"); return(new Vector3(0, 0, 0)); } }
/// <summary> /// Returns the previous point on the path relative to the /// given distance along the path /// </summary> public Vector3 PrevPoint(float distanceAlongPath) { float distmod = distanceAlongPath % PathLength; float distNegEqualZero = (distmod + PathLength) % PathLength; float distPos = distmod > 0 ? distPos = distmod : distPos = PathLength; if (distanceAlongPath <= 0) // given negative/zero distance { distanceAlongPath = distNegEqualZero; } else // given positive distance { distanceAlongPath = distPos; } Vector3 pos; // TODO: not sure if this is an edge case or not if (transform != null) { pos = transform.position; } else { pos = new Vector3(0, 0, 0); } if ((!DynamicPath && IsValidPath()) || (SetupPointData() && IsValidPath())) { int i = 0; int first = 1; int middle = PointCount / 2; int last = PointCount - 1; // do not mod unless >, so that we can move to the end of the path if (distanceAlongPath > PathLength) { distanceAlongPath = distanceAlongPath % PathLength; } while (first <= last) { if (distanceAlongPath > (linearDistanceAlongPath[middle])) // greater than { first = middle + 1; } else if (distanceAlongPath >= (linearDistanceAlongPath[middle - 1]) && // equal to distanceAlongPath <= (linearDistanceAlongPath[middle])) { i = middle; break; } else // less than (dist < linearDistanceAlongPath[middle - 1]) { last = middle - 1; } middle = (first + last) / 2; } return(pos + (transform.rotation * FFMatrix3X3.ScaleBy(points[i - 1], transform.lossyScale))); } Debug.LogError("Error, Path failed to setup"); return(new FFVector3(0, 0, 0)); }
/// <summary> /// returns the nearest point to a distance along the path /// TODO: Mod Needed in end? /// </summary> public Vector3 NearestPoint(float distanceAlongPath) { float distmod = distanceAlongPath % PathLength; float distNegEqualZero = (distmod + PathLength) % PathLength; float distPos = distmod > 0 ? distPos = distmod : distPos = PathLength; if (distanceAlongPath <= 0) // given negative/zero distance { distanceAlongPath = distNegEqualZero; } else // given positive distance { distanceAlongPath = distPos; } Vector3 pos; // TODO: not sure if this is an edge case or not if (transform != null) { pos = transform.position; } else { pos = new Vector3(0, 0, 0); } if ((!DynamicPath && IsValidPath()) || (SetupPointData() && IsValidPath())) { int i = 0; int first = 1; int middle = PointCount / 2; int last = PointCount - 1; while (first <= last) { if (distanceAlongPath > (linearDistanceAlongPath[middle])) // greater than { first = middle + 1; } else if (distanceAlongPath >= (linearDistanceAlongPath[middle - 1]) && // equal to distanceAlongPath <= (linearDistanceAlongPath[middle])) { i = middle; break; } else // less than (dist < linearDistanceAlongPath[middle - 1]) { last = middle - 1; } middle = (first + last) / 2; } distanceAlongPath -= linearDistanceAlongPath[i - 1]; float halfLengthBetweenPoints = (linearDistanceAlongPath[i] - linearDistanceAlongPath[i - 1]) / 2; if (distanceAlongPath > halfLengthBetweenPoints) { return(pos + (transform.rotation * FFMatrix3X3.ScaleBy(points[i % points.Length], transform.lossyScale))); // if we are more than halfway through line } else { return(pos + (transform.rotation * FFMatrix3X3.ScaleBy(points[i - 1], transform.lossyScale))); // if we less than or equal to than halfway through line } } Debug.LogError("Error, Path failed to setup"); return(new FFVector3(0, 0, 0)); }
/// <summary> /// returns the nearest point along path to the givenPoint and the distance along /// the path at which this point is. There are inaccuracies for curved paths. /// </summary> public Vector3 NearestPointAlongPath(Vector3 givenPoint, out float distAlongPathToNearestPoint) { distAlongPathToNearestPoint = -1.0f; Vector3 pos; if (transform != null) { pos = transform.position; } else { pos = new Vector3(0, 0, 0); } if ((!DynamicPath && IsValidPath()) || (SetupPointData() && IsValidPath())) { // Rotate relative point to match path's rotation, and / the scale givenPoint = FFMatrix3X3.ScaleBy(Quaternion.Inverse(transform.rotation) * (givenPoint - pos), new Vector3(1.0f / transform.lossyScale.x, 1.0f / transform.lossyScale.y, 1.0f / transform.lossyScale.z)); float closestDist = float.MaxValue; //check if first point is closest Vector3 nearestPoint = givenPoint - points[0]; if (nearestPoint.sqrMagnitude < closestDist) { closestDist = nearestPoint.sqrMagnitude; distAlongPathToNearestPoint = linearDistanceAlongPath[0]; } var pointCount = PointCount; for (int i = 1; i < pointCount; ++i) { Vector3 nextPoint = points[i % points.Length]; Vector3 prevPoint = points[i - 1]; Vector3 vecToGivenPoint = givenPoint - prevPoint; Vector3 vecToNextPoint = nextPoint - prevPoint; Vector3 vecToClosestPointOnLine = Vector3.Project(vecToGivenPoint, vecToNextPoint); Vector3 vecToPointOnLine = (vecToClosestPointOnLine + prevPoint) - givenPoint; nearestPoint = givenPoint - nextPoint; if (nearestPoint.sqrMagnitude < closestDist) { closestDist = nearestPoint.sqrMagnitude; distAlongPathToNearestPoint = linearDistanceAlongPath[i] * linearDistanceAlongPath[i]; } if (vecToClosestPointOnLine.sqrMagnitude > vecToNextPoint.sqrMagnitude || Vector3.Dot(vecToGivenPoint, vecToNextPoint) < 0) { continue; } if (vecToPointOnLine.sqrMagnitude < closestDist) { closestDist = vecToPointOnLine.sqrMagnitude; distAlongPathToNearestPoint = vecToClosestPointOnLine.magnitude + linearDistanceAlongPath[i - 1]; distAlongPathToNearestPoint = distAlongPathToNearestPoint * distAlongPathToNearestPoint; // Debug draw // vecToGivenPoint //Debug.DrawLine(points[i - 1], vecToGivenPoint + points[i - 1], Color.yellow); // vecToNextPoint //Debug.DrawLine(points[i - 1], vecToNextPoint + points[i - 1], Color.red); // Projection of vecToGivenPoint onto vecToNextPoint //Debug.DrawLine(points[i - 1], vecToClosestPointOnLine + points[i - 1], Color.magenta); } } // Since we already ran the SetupPointData fuction above // we shouldn't need to do it again in PointAlongPath, also // turning off smoothBetweenPoints increases accuracy bool tempDynamicPath = DynamicPath; bool tempSmoothBetweenPoints = SmoothBetweenPoints; DynamicPath = false; // Optimization SmoothBetweenPoints = false; // Accuracy distAlongPathToNearestPoint = Mathf.Sqrt(distAlongPathToNearestPoint); // sqrt to get size right Vector3 nearestPointAlongPath = PointAlongPath(distAlongPathToNearestPoint); DynamicPath = tempDynamicPath; SmoothBetweenPoints = tempSmoothBetweenPoints; return(nearestPointAlongPath); } else { Debug.LogError("NearestPointAlongPath failed to setup"); return(new Vector3(0, 0, 0)); } }