public override Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
float d;
float minDistance = float.MaxValue;
Vector3 onPath = Vector3.zero;
pathRelative.segmentIndex = -1;
// loop over all segments, find the one nearest to the given point
for (int i = 1; i < _points.Count; i++)
{
float segmentLength = _lengths[i];
Vector3 segmentNormal = _normals[i];
Vector3 chosenPoint = Vector3.zero;
d = OpenSteerUtility.PointToSegmentDistance(point, _points[i - 1], _points[i],
segmentNormal, segmentLength,
ref chosenPoint);
if (d < minDistance)
{
minDistance = d;
onPath = chosenPoint;
pathRelative.tangent = segmentNormal;
pathRelative.segmentIndex = i;
}
}
// measure how far original point is outside the Pathway's "tube"
pathRelative.outside = (onPath - point).magnitude - Radius;
// return point on path
return(onPath);
}
/// <summary>
/// Determines whether the received point is inside the path.
/// </summary>
/// <param name="point">Point to evaluate.</param>
/// <returns><c>true</c> if the point is inside the path; otherwise, <c>false</c>.</returns>
public bool IsInsidePath(Vector3 point)
{
var tStruct = new PathRelativePosition();
MapPointToPath(point, ref tStruct);
return(tStruct.Outside < 0);
}
/// <summary>
/// Calculates how far Outside the path is the reference point.
/// </summary>
/// <param name="point">Reference point.</param>
/// <returns>How far Outside the path is the reference point.</returns>
public float HowFarOutsidePath(Vector3 point)
{
var tStruct = new PathRelativePosition();
MapPointToPath(point, ref tStruct);
return(tStruct.Outside);
}
// ----------------------------------------------------------------------------
// Path Following behaviors
public Vector3 steerToStayOnPath(float predictionTime, Pathway path)
{
// predict our future position
Vector3 futurePosition = predictFuturePosition(predictionTime);
// find the point on the path nearest the predicted future position
PathRelativePosition tStruct = new PathRelativePosition();
Vector3 onPath = path.MapPointToPath(futurePosition, ref tStruct);
if (tStruct.outside < 0)
{
// our predicted future position was in the path,
// return zero steering.
return(Vector3.zero);
}
else
{
// our predicted future position was outside the path, need to
// steer towards it. Use onPath projection of futurePosition
// as seek target
#if ANNOTATE_PATH
annotatePathFollowing(futurePosition, onPath, onPath, tStruct.outside);
#endif
return(steerForSeek(onPath));
}
}
/// <summary>
/// Given an arbitrary point ("A"), returns the nearest point ("P") on
/// this path. Also returns, via output arguments, the path Tangent at
/// P and a measure of how far A is Outside the Pathway's "tube". Note
/// that a negative distance indicates A is inside the Pathway.
/// </summary>
/// <param name="point">Reference point.</param>
/// <param name="pathRelative">Structure indicating the relative path position.</param>
/// <returns>The closest point to the received reference point.</returns>
public virtual Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
var minDistance = float.MaxValue;
var onPath = Vector3.zero;
pathRelative.SegmentIndex = -1;
// loop over all segments, find the one nearest to the given point
for (var i = 1; i < Path.Count; i++)
{
var segmentLength = Lengths[i];
var segmentNormal = Normals[i];
var chosenPoint = Vector3.zero;
var d = OpenSteerUtility.PointToSegmentDistance(point, Path[i - 1], Path[i],
segmentNormal, segmentLength,
ref chosenPoint);
if (!(d < minDistance))
{
continue;
}
minDistance = d;
onPath = chosenPoint;
pathRelative.Tangent = segmentNormal;
pathRelative.SegmentIndex = i;
}
// measure how far original point is Outside the Pathway's "tube"
pathRelative.Outside = (onPath - point).magnitude - Radius;
// return point on path
return(onPath);
}
/// <summary>
/// Given an arbitrary point ("A"), returns the nearest point ("P") on
/// this path. Also returns, via output arguments, the path Tangent at
/// P and a measure of how far A is Outside the Pathway's "tube". Note
/// that a negative distance indicates A is inside the Pathway.
/// </summary>
/// <param name="point">Reference point.</param>
/// <param name="pathRelative">Structure indicating the relative path position.</param>
/// <returns>The closest point to the received reference point.</returns>
public override Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
// Approximate the closest path point on a linear path
var onPath = base.MapPointToPath(point, ref pathRelative);
var distance = MapPointToPathDistance(onPath) / TotalPathLength;
var splinePoint = CalculateCatmullRomPoint(1, distance);
// return point on path
return(splinePoint);
}
/// <summary>
/// Force to apply to the vehicle
/// </summary>
/// <returns>
/// A <see cref="Vector3"/>
/// </returns>
protected override Vector3 CalculateForce()
{
Vector3 forceNode = Vector3.zero;
if (_path != null)
{
var tStruct = new PathRelativePosition();
var futurePosition = Vehicle.PredictFuturePosition(_predictionTime);
var futurePathPoint = _path.MapPointToPath (futurePosition, ref tStruct);
#if TRACE_PATH
Debug.DrawLine(Vehicle.Position, futurePosition, Color.red);
Debug.DrawLine(Vehicle.Position, futurePathPoint, Color.green);
#endif
forceNode = Vehicle.GetSeekVector(futurePathPoint, false);
}
return forceNode;
}
// how far outside path tube is the given point? (negative is inside)
public float HowFarOutsidePath(Vector3 point)
{
var tStruct = new PathRelativePosition();
MapPointToPath(point, ref tStruct);
return tStruct.outside;
}
// ----------------------------------------------------------------------------
// Path Following behaviors
public Vector3 steerToStayOnPath(float predictionTime, IPathway path)
{
// predict our future position
Vector3 futurePosition = predictFuturePosition (predictionTime);
// find the point on the path nearest the predicted future position
PathRelativePosition tStruct = new PathRelativePosition();
Vector3 onPath = path.MapPointToPath(futurePosition, ref tStruct);
if (tStruct.outside < 0)
{
// our predicted future position was in the path,
// return zero steering.
return Vector3.zero;
}
else
{
// our predicted future position was outside the path, need to
// steer towards it. Use onPath projection of futurePosition
// as seek target
#if ANNOTATE_PATH
annotatePathFollowing (futurePosition, onPath, onPath,tStruct.outside);
#endif
return steerForSeek (onPath);
}
}
// Given an arbitrary point ("A"), returns the nearest point ("P") on
// this path. Also returns, via output arguments, the path tangent at
// P and a measure of how far A is outside the Pathway's "tube". Note
// that a negative distance indicates A is inside the Pathway.
public abstract Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition tStruct);
public override Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
float d;
float minDistance = float.MaxValue;
Vector3 onPath = Vector3.zero;
pathRelative.segmentIndex = -1;
// loop over all segments, find the one nearest to the given point
for (int i = 1; i < _points.Count; i++)
{
float segmentLength = _lengths[i];
Vector3 segmentNormal = _normals[i];
Vector3 chosenPoint = Vector3.zero;
d = OpenSteerUtility.PointToSegmentDistance(point, _points[i-1], _points[i],
segmentNormal, segmentLength,
ref chosenPoint);
if (d < minDistance)
{
minDistance = d;
onPath = chosenPoint;
pathRelative.tangent = segmentNormal;
pathRelative.segmentIndex = i;
}
}
// measure how far original point is outside the Pathway's "tube"
pathRelative.outside = (onPath - point).magnitude - _radius;
// return point on path
return onPath;
}
/// <summary>
/// Should the force be calculated?
/// </summary>
/// <returns>
/// A <see cref="Vector3"/>
/// </returns>
protected override Vector3 CalculateForce()
{
if (_path == null || _path.SegmentCount < 2)
return Vector3.zero;
// our goal will be offset from our path distance by this amount
float pathDistanceOffset = (int)_direction * _predictionTime * Vehicle.Speed;
// predict our future position
Vector3 futurePosition = Vehicle.PredictFuturePosition(_predictionTime);
// measure distance along path of our current and predicted positions
float nowPathDistance = _path.MapPointToPathDistance (Vehicle.Position);
float futurePathDistance = _path.MapPointToPathDistance (futurePosition);
// are we facing in the correction direction?
bool rightway = ((pathDistanceOffset > 0) ? (nowPathDistance < futurePathDistance) : (nowPathDistance > futurePathDistance));
// find the point on the path nearest the predicted future position
var tStruct = new PathRelativePosition ();
_path.MapPointToPath (futurePosition, ref tStruct);
// no steering is required if (a) our future position is inside
// the path tube and (b) we are facing in the correct direction
if ((tStruct.outside < 0) && rightway)
{
// TODO Evaluate. This assumes the vehicle has inertia, and would stop if inside the path tube
return Vector3.zero;
} else
{
/*
* Otherwise we need to steer towards a target point obtained
* by adding pathDistanceOffset to our current path position.
*
* Notice that this method does not steer for the point in the
* path that is closest to our future position, which is why
* the return value of MapPointToPath is ignored above. Instead,
* it estimates how far the vehicle will move in units, and then
* aim for the point in the path that is that many units away
* from our current path position _in path length_. This means
* that it adds up the segment lengths and aims for the point
* that is N units along the length of the path, which can imply
* bends and turns and is not a straight vector projected away
* from our position.
*
* This also means that having too high a prediction time will
* have the effect of the agent seemingly approaching the path
* in an elliptical manner.
*/
float targetPathDistance = nowPathDistance + pathDistanceOffset;
var target = _path.MapPathDistanceToPoint (targetPathDistance);
// return steering to seek target on path
var seek = Vehicle.GetSeekVector(target);
return seek;
}
}
/// <summary>
/// Given an arbitrary point ("A"), returns the nearest point ("P") on
/// this path. Also returns, via output arguments, the path Tangent at
/// P and a measure of how far A is Outside the Pathway's "tube". Note
/// that a negative distance indicates A is inside the Pathway.
/// </summary>
/// <param name="point">Reference point.</param>
/// <param name="pathRelative">Structure indicating the relative path position.</param>
/// <returns>The closest point to the received reference point.</returns>
public override Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
// Approximate the closest path point on a linear path
var onPath = base.MapPointToPath(point, ref pathRelative);
var distance = MapPointToPathDistance(onPath) / TotalPathLength;
var splinePoint = CalculateCatmullRomPoint(1, distance);
// return point on path
return splinePoint;
}
// Given an arbitrary point ("A"), returns the nearest point ("P") on
// this path. Also returns, via output arguments, the path tangent at
// P and a measure of how far A is outside the Pathway's "tube". Note
// that a negative distance indicates A is inside the Pathway.
public virtual Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition tStruct)
{
return(Vector3.zero);
}
// is the given point inside the path tube?
public bool IsInsidePath(Vector3 point)
{
var tStruct = new PathRelativePosition();
MapPointToPath(point, ref tStruct);
return tStruct.outside < 0;
}
/// <summary>
/// Given an arbitrary point ("A"), returns the nearest point ("P") on
/// this path. Also returns, via output arguments, the path Tangent at
/// P and a measure of how far A is Outside the Pathway's "tube". Note
/// that a negative distance indicates A is inside the Pathway.
/// </summary>
/// <param name="point">Reference point.</param>
/// <param name="pathRelative">Structure indicating the relative path position.</param>
/// <returns>The closest point to the received reference point.</returns>
public virtual Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition pathRelative)
{
var minDistance = float.MaxValue;
var onPath = Vector3.zero;
pathRelative.SegmentIndex = -1;
// loop over all segments, find the one nearest to the given point
for (var i = 1; i < Path.Count; i++)
{
var segmentLength = Lengths[i];
var segmentNormal = Normals[i];
var chosenPoint = Vector3.zero;
var d = OpenSteerUtility.PointToSegmentDistance(point, Path[i - 1], Path[i],
segmentNormal, segmentLength,
ref chosenPoint);
if (!(d < minDistance)) continue;
minDistance = d;
onPath = chosenPoint;
pathRelative.Tangent = segmentNormal;
pathRelative.SegmentIndex = i;
}
// measure how far original point is Outside the Pathway's "tube"
pathRelative.Outside = (onPath - point).magnitude - Radius;
// return point on path
return onPath;
}
// Given an arbitrary point ("A"), returns the nearest point ("P") on
// this path. Also returns, via output arguments, the path tangent at
// P and a measure of how far A is outside the Pathway's "tube". Note
// that a negative distance indicates A is inside the Pathway.
public virtual Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition tStruct)
{
return Vector3.zero;
}
// Given an arbitrary point ("A"), returns the nearest point ("P") on
// this path. Also returns, via output arguments, the path tangent at
// P and a measure of how far A is outside the Pathway's "tube". Note
// that a negative distance indicates A is inside the Pathway.
public abstract Vector3 MapPointToPath(Vector3 point, ref PathRelativePosition tStruct);
