List <PF.Vector3> ApplyDP(Path p, List <PF.Vector3> points)
{
if (DPCosts.Length < points.Count)
{
DPCosts = new float[points.Count];
DPParents = new int[points.Count];
}
for (int i = 0; i < DPParents.Length; i++)
{
DPCosts[i] = DPParents[i] = -1;
}
bool canBeOriginalNodes = points.Count == p.path.Count;
for (int i = 0; i < points.Count; i++)
{
float d = DPCosts[i];
PF.Vector3 start = points[i];
var startIsOriginalNode = canBeOriginalNodes && start == (PF.Vector3)p.path[i].position;
for (int j = i + 1; j < points.Count; j++)
{
// Total distance from the start to this point using the best simplified path
// The small additive constant is to make sure that the number of points is kept as small as possible
// even when the total distance is the same (which can happen with e.g multiple colinear points).
float d2 = d + (points[j] - start).magnitude + 0.0001f;
if (DPParents[j] == -1 || d2 < DPCosts[j])
{
var endIsOriginalNode = canBeOriginalNodes && points[j] == (PF.Vector3)p.path[j].position;
if (j == i + 1 || ValidateLine(startIsOriginalNode ? p.path[i] : null, endIsOriginalNode ? p.path[j] : null, start, points[j]))
{
DPCosts[j] = d2;
DPParents[j] = i;
}
else
{
break;
}
}
}
}
int c = points.Count - 1;
while (c != -1)
{
buffer.Add(points[c]);
c = DPParents[c];
}
buffer.Reverse();
Memory.Swap(ref buffer, ref points);
buffer.ClearFast();
return(points);
}
public void MoveToLocallyClosestPoint(PF.Vector3 point, bool allowForwards = true, bool allowBackwards = true)
{
if (path == null)
{
return;
}
while (allowForwards && segmentIndex < path.Count - 2 && (path[segmentIndex + 1] - point).sqrMagnitude <= (path[segmentIndex] - point).sqrMagnitude)
{
NextSegment();
}
while (allowBackwards && segmentIndex > 0 && (path[segmentIndex - 1] - point).sqrMagnitude <= (path[segmentIndex] - point).sqrMagnitude)
{
PrevSegment();
}
// Check the distances to the two segments extending from the vertex path[segmentIndex]
// and pick the position on those segments that is closest to the #point parameter.
float factor1 = 0, factor2 = 0, d1 = float.PositiveInfinity, d2 = float.PositiveInfinity;
if (segmentIndex > 0)
{
factor1 = VectorMath.ClosestPointOnLineFactor(path[segmentIndex - 1], path[segmentIndex], point);
d1 = (PF.Vector3.Lerp(path[segmentIndex - 1], path[segmentIndex], factor1) - point).sqrMagnitude;
}
if (segmentIndex < path.Count - 1)
{
factor2 = VectorMath.ClosestPointOnLineFactor(path[segmentIndex], path[segmentIndex + 1], point);
d2 = (PF.Vector3.Lerp(path[segmentIndex], path[segmentIndex + 1], factor2) - point).sqrMagnitude;
}
if (d1 < d2)
{
MoveToSegment(segmentIndex - 1, factor1);
}
else
{
MoveToSegment(segmentIndex, factor2);
}
}
/** Updates graphs with a created GUO.
* Creates a Pathfinding.GraphUpdateObject with a Pathfinding.GraphUpdateShape
* representing the polygon of this object and update all graphs using AstarPath.UpdateGraphs.
* This will not update graphs immediately. See AstarPath.UpdateGraph for more info.
*/
public void Apply()
{
if (AstarPath.active == null)
{
Debug.LogError("There is no AstarPath object in the scene", this);
return;
}
GraphUpdateObject guo;
if (points == null || points.Length == 0)
{
var polygonCollider = GetComponent <PolygonCollider2D>();
if (polygonCollider != null)
{
var points2D = polygonCollider.points;
Vector3[] pts = new Vector3[points2D.Length];
for (int i = 0; i < pts.Length; i++)
{
var p = points2D[i] + polygonCollider.offset;
pts[i] = new Vector3(p.x, 0, p.y);
}
var mat = transform.localToWorldMatrix * Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(-90, 0, 0), Vector3.one);
var shape = new GraphUpdateShape(points, convex, mat, minBoundsHeight);
guo = new GraphUpdateObject(GetBounds());
guo.shape = shape;
}
else
{
var bounds = GetBounds();
if (bounds.center == Vector3.zero && bounds.size == Vector3.zero)
{
Debug.LogError("Cannot apply GraphUpdateScene, no points defined and no renderer or collider attached", this);
return;
}
guo = new GraphUpdateObject(bounds);
}
}
else
{
GraphUpdateShape shape;
if (legacyMode && !legacyUseWorldSpace)
{
// Used for compatibility with older versions
var worldPoints = new PF.Vector3[points.Length];
for (int i = 0; i < points.Length; i++)
{
worldPoints[i] = transform.TransformPoint(points[i]);
}
shape = new GraphUpdateShape(worldPoints, convex, Matrix4x4.identity, minBoundsHeight);
}
else
{
shape = new GraphUpdateShape(points, convex, legacyMode && legacyUseWorldSpace ? Matrix4x4.identity : transform.localToWorldMatrix, minBoundsHeight);
}
var bounds = shape.GetBounds();
guo = new GraphUpdateObject(bounds);
guo.shape = shape;
}
firstApplied = true;
guo.modifyWalkability = modifyWalkability;
guo.setWalkability = setWalkability;
guo.addPenalty = penaltyDelta;
guo.updatePhysics = updatePhysics;
guo.updateErosion = updateErosion;
guo.resetPenaltyOnPhysics = resetPenaltyOnPhysics;
guo.modifyTag = modifyTag;
guo.setTag = setTag;
}
/** Gradient and value of the cost function of this VO.
* The VO has a cost function which is 0 outside the VO
* and increases inside it as the point moves further into
* the VO.
*
* This is the negative gradient of that function as well as its
* value (the weight). The negative gradient points in the direction
* where the function decreases the fastest.
*
* The value of the function is the distance to the closest edge
* of the VO and the gradient is normalized.
*/
public Vector2 Gradient(Vector2 p, out float weight)
{
if (colliding)
{
// Calculate double signed area of the triangle consisting of the points
// {line1, line1+dir1, p}
float l1 = SignedDistanceFromLine(line1, dir1, p);
// Serves as a check for which side of the line the point p is
if (l1 >= 0)
{
weight = l1;
return(new Vector2(-dir1.y, dir1.x));
}
else
{
weight = 0;
return(new Vector2(0, 0));
}
}
float det3 = SignedDistanceFromLine(cutoffLine, cutoffDir, p);
if (det3 <= 0)
{
weight = 0;
return(Vector2.zero);
}
else
{
// Signed distances to the two edges along the sides of the VO
float det1 = SignedDistanceFromLine(line1, dir1, p);
float det2 = SignedDistanceFromLine(line2, dir2, p);
if (det1 >= 0 && det2 >= 0)
{
// We are inside both of the half planes
// (all three if we count the cutoff line)
// and thus inside the forbidden region in velocity space
// Actually the negative gradient because we want the
// direction where it slopes the most downwards, not upwards
Vector2 gradient;
// Check if we are in the semicircle region near the cap of the VO
if (Vector2.Dot(p - line1, dir1) > 0 && Vector2.Dot(p - line2, dir2) < 0)
{
if (segment)
{
// This part will only be reached for line obstacles (i.e not other agents)
if (det3 < radius)
{
PF.Vector3 closestPointOnLine = VectorMath.ClosestPointOnSegment(segmentStart.ToPFV2(), segmentEnd.ToPFV2(), p.ToPFV2());
var dirFromCenter = p.ToPFV2() - closestPointOnLine.ToV2();
float distToCenter;
gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
// The weight is the distance to the edge
weight = radius - distToCenter;
return(gradient);
}
}
else
{
var dirFromCenter = p - circleCenter;
float distToCenter;
gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
// The weight is the distance to the edge
weight = radius - distToCenter;
return(gradient);
}
}
if (segment && det3 < det1 && det3 < det2)
{
weight = det3;
gradient = new Vector2(-cutoffDir.y, cutoffDir.x);
return(gradient);
}
// Just move towards the closest edge
// The weight is the distance to the edge
if (det1 < det2)
{
weight = det1;
gradient = new Vector2(-dir1.y, dir1.x);
}
else
{
weight = det2;
gradient = new Vector2(-dir2.y, dir2.x);
}
return(gradient);
}
weight = 0;
return(Vector2.zero);
}
}