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