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