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