protected VInt3 CalculateTargetPoint(VInt3 p, VInt3 a, VInt3 b) { if (a.x == b.x && a.z == b.z) { return(a); } VFactor f = AstarMath.NearestPointFactorXZ(ref a, ref b, ref p); long a2 = VInt3.Lerp(a, b, f).XZSqrMagnitude(ref p); int num = IntMath.Sqrt(a2); long a3 = a.XZSqrMagnitude(ref b); int num2 = IntMath.Sqrt(a3); if (num2 == 0) { return(b); } int num3 = Mathf.Clamp(this.forwardLook.i - num, 0, this.forwardLook.i); VFactor vFactor = new VFactor((long)num3 * f.den + f.nom * (long)num2, (long)num2 * f.den); if (vFactor > VFactor.one) { vFactor = VFactor.one; } else if (vFactor < VFactor.zero) { vFactor = VFactor.zero; } vFactor.strip(); return(VInt3.Lerp(a, b, vFactor)); }
public static void ShrinkPortals(FunnelPortals portals, float shrink) { if (shrink <= 0.00001f) { return; } for (int i = 0; i < portals.left.Count; i++) { var left = portals.left[i]; var right = portals.right[i]; var length = (left - right).magnitude; if (length > 0) { float s = Mathf.Min(shrink / length, 0.4f); //Good Game /*portals.left[i] = Vector3.Lerp(left, right, s); * portals.right[i] = Vector3.Lerp(left, right, 1 - s);*/ portals.left[i] = VInt3.Lerp(left, right, s); portals.right[i] = VInt3.Lerp(left, right, 1 - s); } } }
protected VInt3 CalculateTargetPoint(VInt3 p, VInt3 a, VInt3 b) { if ((a.x == b.x) && (a.z == b.z)) { return(a); } VFactor f = AstarMath.NearestPointFactorXZ(ref a, ref b, ref p); int num3 = IntMath.Sqrt(VInt3.Lerp(a, b, f).XZSqrMagnitude(ref p)); int num5 = IntMath.Sqrt(a.XZSqrMagnitude(ref b)); if (num5 == 0) { return(b); } int num6 = Mathf.Clamp(this.forwardLook.i - num3, 0, this.forwardLook.i); VFactor one = new VFactor((num6 * f.den) + (f.nom * num5), num5 * f.den); if (one > VFactor.one) { one = VFactor.one; } else if (one < VFactor.zero) { one = VFactor.zero; } one.strip(); return(VInt3.Lerp(a, b, one)); }
protected VInt3 CalculateTargetPoint(VInt3 p, VInt3 a, VInt3 b) { if (a.x == b.x && a.z == b.z) { return(a); } VFactor vFactor = AstarMath.NearestPointFactorXZ(ref a, ref b, ref p); long num = VInt3.Lerp(a, b, vFactor).XZSqrMagnitude(ref p); int num2 = IntMath.Sqrt(num); long num3 = a.XZSqrMagnitude(ref b); int num4 = IntMath.Sqrt(num3); if (num4 == 0) { return(b); } int num5 = Mathf.Clamp(this.forwardLook.i - num2, 0, this.forwardLook.i); VFactor vFactor2 = new VFactor((long)num5 * vFactor.den + vFactor.nom * (long)num4, (long)num4 * vFactor.den); if (vFactor2 > VFactor.one) { vFactor2 = VFactor.one; } else if (vFactor2 < VFactor.zero) { vFactor2 = VFactor.zero; } vFactor2.strip(); return(VInt3.Lerp(a, b, vFactor2)); }
/** Move as close as possible to the specified point */ //Good Game //public void MoveToClosestPoint (Vector3 point) { public void MoveToClosestPoint(VInt3 point) { if (path == null) { return; } float bestDist = float.PositiveInfinity; float bestFactor = 0f; int bestIndex = 0; for (int i = 0; i < path.Count - 1; i++) { //Good Game /*float factor = VectorMath.ClosestPointOnLineFactor(path[i], path[i+1], point); * Vector3 closest = Vector3.Lerp(path[i], path[i+1], factor);*/ long factor = IntMath.ClosestPointOnLineFactor(path[i], path[i + 1], point); VInt3 closest = VInt3.Lerp(path[i], path[i + 1], factor); float dist = (point - closest).sqrMagnitude; if (dist < bestDist) { bestDist = dist; bestFactor = factor; bestIndex = i; } } MoveToSegment(bestIndex, bestFactor); }
//Good Game //public void MoveToLocallyClosestPoint (Vector3 point, bool allowForwards = true, bool allowBackwards = true) { public void MoveToLocallyClosestPoint(VInt3 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) { //Good Game /*factor1 = VectorMath.ClosestPointOnLineFactor(path[segmentIndex-1], path[segmentIndex], point); * d1 = (Vector3.Lerp(path[segmentIndex-1], path[segmentIndex], factor1) - point).sqrMagnitude;*/ factor1 = IntMath.ClosestPointOnLineFactor(path[segmentIndex - 1], path[segmentIndex], point); d1 = (VInt3.Lerp(path[segmentIndex - 1], path[segmentIndex], factor1) - point).sqrMagnitude; } if (segmentIndex < path.Count - 1) { //Good Game /*factor2 = VectorMath.ClosestPointOnLineFactor(path[segmentIndex], path[segmentIndex+1], point); * d2 = (Vector3.Lerp(path[segmentIndex], path[segmentIndex+1], factor2) - point).sqrMagnitude;*/ factor2 = IntMath.ClosestPointOnLineFactor(path[segmentIndex], path[segmentIndex + 1], point); d2 = (VInt3.Lerp(path[segmentIndex], path[segmentIndex + 1], factor2) - point).sqrMagnitude; } if (d1 < d2) { MoveToSegment(segmentIndex - 1, factor1); } else { MoveToSegment(segmentIndex, factor2); } }
/** Calculate the shortest path through the funnel. * \param funnel The portals of the funnel. The first and last vertices portals must be single points (so for example left[0] == right[0]). * \param unwrap Determines if twists and bends should be straightened out before running the funnel algorithm. * \param splitAtEveryPortal If true, then a vertex will be inserted every time the path crosses a portal * instead of only at the corners of the path. The result will have exactly one vertex per portal if this is enabled. * This may introduce vertices with the same position in the output (esp. in corners where many portals meet). * * If the unwrap option is disabled the funnel will simply be projected onto the XZ plane. * If the unwrap option is enabled then the funnel may be oriented arbitrarily and may have twists and bends. * This makes it possible to support the funnel algorithm in XY space as well as in more complicated cases, such * as on curved worlds. * \shadowimage{funnel_unwrap_illustration.png} * * \shadowimage{funnel_split_at_every_portal.png} * * \see Unwrap */ //Good Game //public static List<Vector3> Calculate (FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { public static List <VInt3> Calculate(FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { if (funnel.left.Count != funnel.right.Count) { throw new System.ArgumentException("funnel.left.Count != funnel.right.Count"); } // Get arrays at least as large as the number of portals //Good Game /*var leftArr = ArrayPool<Vector2>.Claim(funnel.left.Count); * var rightArr = ArrayPool<Vector2>.Claim(funnel.left.Count);*/ var leftArr = ArrayPool <VInt2> .Claim(funnel.left.Count); var rightArr = ArrayPool <VInt2> .Claim(funnel.left.Count); if (unwrap) { Unwrap(funnel, leftArr, rightArr); } else { // Copy to arrays for (int i = 0; i < funnel.left.Count; i++) { leftArr[i] = ToXZ(funnel.left[i]); rightArr[i] = ToXZ(funnel.right[i]); } } int startIndex = FixFunnel(leftArr, rightArr, funnel.left.Count); var intermediateResult = ListPool <int> .Claim(); if (startIndex == -1) { // If funnel algorithm failed, fall back to a simple line intermediateResult.Add(0); intermediateResult.Add(funnel.left.Count - 1); } else { bool lastCorner; Calculate(leftArr, rightArr, funnel.left.Count, startIndex, intermediateResult, int.MaxValue, out lastCorner); } // Get list for the final result //Good Game //var result = ListPool<Vector3>.Claim(intermediateResult.Count); var result = ListPool <VInt3> .Claim(intermediateResult.Count); //Good Game //Vector2 prev2D = leftArr[0]; VInt2 prev2D = leftArr[0]; var prevIdx = 0; for (int i = 0; i < intermediateResult.Count; i++) { var idx = intermediateResult[i]; if (splitAtEveryPortal) { // Check intersections with every portal segment var next2D = idx >= 0 ? leftArr[idx] : rightArr[-idx]; for (int j = prevIdx + 1; j < System.Math.Abs(idx); j++) { //Good Game //var factor = VectorMath.LineIntersectionFactorXZ(FromXZ(leftArr[j]), FromXZ(rightArr[j]), FromXZ(prev2D), FromXZ(next2D)); var factor = IntMath.LineIntersectionFactorXZ(FromXZ(leftArr[j]), FromXZ(rightArr[j]), FromXZ(prev2D), FromXZ(next2D)); //result.Add(Vector3.Lerp(funnel.left[j], funnel.right[j], factor)); //Good Game result.Add(VInt3.Lerp(funnel.left[j], funnel.right[j], factor)); } prevIdx = Mathf.Abs(idx); prev2D = next2D; } if (idx >= 0) { result.Add(funnel.left[idx]); } else { result.Add(funnel.right[-idx]); } } // Release lists back to the pool ListPool <int> .Release(ref intermediateResult); //Good Game /*ArrayPool<Vector2>.Release(ref leftArr); * ArrayPool<Vector2>.Release(ref rightArr);*/ ArrayPool <VInt2> .Release(ref leftArr); ArrayPool <VInt2> .Release(ref rightArr); return(result); }
private static void MoveAlongEdge(TriangleMeshNode node, int edge, VInt3 srcLoc, VInt3 destLoc, MoveDirectionState state, out VInt3 result, bool checkAnotherEdge = true) { bool flag; DebugHelper.Assert((edge >= 0) && (edge <= 2)); VInt3 vertex = node.GetVertex(edge); VInt3 num2 = node.GetVertex((edge + 1) % 3); VInt3 a = destLoc - srcLoc; a.y = 0; VInt3 lhs = num2 - vertex; lhs.y = 0; lhs.NormalizeTo(0x3e8); int num5 = 0; if (state != null) { num5 = a.magnitude2D * 0x3e8; VInt3 num6 = !state.enabled ? a : state.firstAdjDir; if (VInt3.Dot(ref lhs, ref num6) < 0) { num5 = -num5; num6 = -lhs; } else { num6 = lhs; } if (!state.enabled) { state.enabled = true; state.firstAdjDir = VInt3.Lerp(a, num6, 1, 3); state.firstDir = state.curDir; state.adjDir = num6; } else if (VInt3.Dot(ref state.adjDir, ref num6) >= 0) { state.adjDir = num6; } else { num5 = 0; } state.applied = true; } else { num5 = (lhs.x * a.x) + (lhs.z * a.z); } VInt3 rhs = Polygon.IntersectionPoint(ref vertex, ref num2, ref srcLoc, ref destLoc, out flag); if (!flag) { if (!Polygon.IsColinear(vertex, num2, srcLoc) || !Polygon.IsColinear(vertex, num2, destLoc)) { result = srcLoc; return; } if (num5 >= 0) { int num8 = (lhs.x * (num2.x - vertex.x)) + (lhs.z * (num2.z - vertex.z)); int num9 = (lhs.x * (destLoc.x - vertex.x)) + (lhs.z * (destLoc.z - vertex.z)); rhs = (num8 <= num9) ? num2 : destLoc; DebugHelper.Assert((num8 >= 0) && (num9 >= 0)); } else { int num10 = (-lhs.x * (vertex.x - num2.x)) - (lhs.z * (vertex.z - num2.z)); int num11 = (-lhs.x * (destLoc.x - num2.x)) - (lhs.z * (destLoc.z - num2.z)); rhs = (Mathf.Abs(num10) <= Mathf.Abs(num11)) ? vertex : destLoc; DebugHelper.Assert((num10 >= 0) && (num11 >= 0)); } } int num12 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 0xf4240L); int num13 = IntMath.Sqrt(num2.XZSqrMagnitude(rhs) * 0xf4240L); if ((num5 >= num12) && (num5 <= num13)) { result = IntMath.Divide(lhs, (long)num5, 0xf4240L) + rhs; if (!node.ContainsPoint(result)) { int num16; int num17; int num18; int num19; Vector3 vector = (Vector3)(num2 - vertex); vector.y = 0f; vector.Normalize(); VInt3 num14 = num2 - vertex; num14.y = 0; num14 *= 0x2710; long magnitude = num14.magnitude; VFactor factor = new VFactor { nom = num5, den = magnitude * 0x3e8L }; getMinMax(out num16, out num18, (long)num14.x, ref factor); getMinMax(out num17, out num19, (long)num14.z, ref factor); if (!MakePointInTriangle(ref result, node, num16, num18, num17, num19, srcLoc) && !MakePointInTriangle(ref result, node, num16 - 4, num18 + 4, num17 - 4, num19 + 4, srcLoc)) { result = srcLoc; } } if (MoveAxisY) { CalculateY(ref result, node); } } else { int num20; int num21; VInt3 num22; int num24; if (num5 < num12) { num20 = num5 - num12; num21 = (edge + 2) % 3; num22 = vertex; } else { num20 = num5 - num13; num21 = (edge + 1) % 3; num22 = num2; } VInt3 num23 = (VInt3)((lhs * num20) / 1000000f); TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(num21, out num24); if (neighborByEdge != null) { checkedNodes.Add(node); MoveFromNode(neighborByEdge, num24, num22, num23 + num22, state, out result); } else { if (checkAnotherEdge) { VInt3 num27 = node.GetVertex((edge + 2) % 3) - num22; if (VInt3.Dot(num27.NormalizeTo(0x3e8), num23) > 0) { checkedNodes.Add(node); MoveAlongEdge(node, num21, num22, num23 + num22, state, out result, false); return; } } result = num22; } } }
private static void MoveAlongEdge(TriangleMeshNode node, int edge, VInt3 srcLoc, VInt3 destLoc, MoveDirectionState state, out VInt3 result, bool checkAnotherEdge = true) { DebugHelper.Assert(edge >= 0 && edge <= 2); VInt3 vertex = node.GetVertex(edge); VInt3 vertex2 = node.GetVertex((edge + 1) % 3); VInt3 vInt = destLoc - srcLoc; vInt.y = 0; VInt3 vInt2 = vertex2 - vertex; vInt2.y = 0; vInt2.NormalizeTo(1000); int num; if (state != null) { num = vInt.magnitude2D * 1000; VInt3 vInt3 = state.enabled ? state.firstAdjDir : vInt; if (VInt3.Dot(ref vInt2, ref vInt3) < 0) { num = -num; vInt3 = -vInt2; } else { vInt3 = vInt2; } if (!state.enabled) { state.enabled = true; state.firstAdjDir = VInt3.Lerp(vInt, vInt3, 1, 3); state.firstDir = state.curDir; state.adjDir = vInt3; } else if (VInt3.Dot(ref state.adjDir, ref vInt3) >= 0) { state.adjDir = vInt3; } else { num = 0; } state.applied = true; } else { num = vInt2.x * vInt.x + vInt2.z * vInt.z; } bool flag; VInt3 rhs = Polygon.IntersectionPoint(ref vertex, ref vertex2, ref srcLoc, ref destLoc, out flag); if (!flag) { if (!Polygon.IsColinear(vertex, vertex2, srcLoc) || !Polygon.IsColinear(vertex, vertex2, destLoc)) { result = srcLoc; return; } if (num >= 0) { int num2 = vInt2.x * (vertex2.x - vertex.x) + vInt2.z * (vertex2.z - vertex.z); int num3 = vInt2.x * (destLoc.x - vertex.x) + vInt2.z * (destLoc.z - vertex.z); rhs = ((num2 > num3) ? destLoc : vertex2); DebugHelper.Assert(num2 >= 0 && num3 >= 0); } else { int num4 = -vInt2.x * (vertex.x - vertex2.x) - vInt2.z * (vertex.z - vertex2.z); int num5 = -vInt2.x * (destLoc.x - vertex2.x) - vInt2.z * (destLoc.z - vertex2.z); rhs = ((Mathf.Abs(num4) > Mathf.Abs(num5)) ? destLoc : vertex); DebugHelper.Assert(num4 >= 0 && num5 >= 0); } } int num6 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 1000000L); int num7 = IntMath.Sqrt(vertex2.XZSqrMagnitude(rhs) * 1000000L); if (num >= num6 && num <= num7) { result = IntMath.Divide(vInt2, (long)num, 1000000L) + rhs; if (!node.ContainsPoint(result)) { Vector3 vector = (Vector3)(vertex2 - vertex); vector.y = 0f; vector.Normalize(); VInt3 lhs = vertex2 - vertex; lhs.y = 0; lhs *= 10000; long num8 = (long)lhs.magnitude; VFactor vFactor = default(VFactor); vFactor.nom = (long)num; vFactor.den = num8 * 1000L; int num9; int num10; PathfindingUtility.getMinMax(out num9, out num10, (long)lhs.x, ref vFactor); int num11; int num12; PathfindingUtility.getMinMax(out num11, out num12, (long)lhs.z, ref vFactor); if (!PathfindingUtility.MakePointInTriangle(ref result, node, num9, num10, num11, num12, srcLoc) && !PathfindingUtility.MakePointInTriangle(ref result, node, num9 - 4, num10 + 4, num11 - 4, num12 + 4, srcLoc)) { result = srcLoc; } } if (PathfindingUtility.MoveAxisY) { PathfindingUtility.CalculateY(ref result, node); } } else { int rhs2; int edge2; VInt3 vInt4; if (num < num6) { rhs2 = num - num6; edge2 = (edge + 2) % 3; vInt4 = vertex; } else { rhs2 = num - num7; edge2 = (edge + 1) % 3; vInt4 = vertex2; } VInt3 vInt5 = vInt2 * rhs2 / 1000000f; int startEdge; TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(edge2, out startEdge); if (neighborByEdge != null) { PathfindingUtility.checkedNodes.Add(node); PathfindingUtility.MoveFromNode(neighborByEdge, startEdge, vInt4, vInt5 + vInt4, state, out result); } else { if (checkAnotherEdge) { VInt3 vertex3 = node.GetVertex((edge + 2) % 3); VInt3 lhs2 = (vertex3 - vInt4).NormalizeTo(1000); if (VInt3.Dot(lhs2, vInt5) > 0) { PathfindingUtility.checkedNodes.Add(node); PathfindingUtility.MoveAlongEdge(node, edge2, vInt4, vInt5 + vInt4, state, out result, false); return; } } result = vInt4; } } }
//Good Game //public List<Vector3> SmoothSimple (List<Vector3> path) { public List <VInt3> SmoothSimple(List <VInt3> path) { if (path.Count < 2) { return(path); } //Good Game //List<Vector3> subdivided; List <VInt3> subdivided; if (uniformLength) { // Clamp to a small value to avoid the path being divided into a huge number of segments maxSegmentLength = Mathf.Max(maxSegmentLength, 0.005f); float pathLength = 0; for (int i = 0; i < path.Count - 1; i++) { //Good Game //pathLength += Vector3.Distance(path[i], path[i+1]); pathLength += VInt3.Distance(path[i], path[i + 1]); } int estimatedNumberOfSegments = Mathf.FloorToInt(pathLength / maxSegmentLength); // Get a list with an initial capacity high enough so that we can add all points //Good Game //subdivided = ListPool<Vector3>.Claim(estimatedNumberOfSegments+2); subdivided = ListPool <VInt3> .Claim(estimatedNumberOfSegments + 2); //Good Game float distanceAlong = 0; // Sample points every [maxSegmentLength] world units along the path for (int i = 0; i < path.Count - 1; i++) { var start = path[i]; var end = path[i + 1]; //Good Game //float length = Vector3.Distance(start, end); long length = VInt3.Distance(start, end); while (distanceAlong < length) { //Good Game //subdivided.Add(Vector3.Lerp(start, end, distanceAlong / length)); subdivided.Add(VInt3.Lerp(start, end, distanceAlong / length)); distanceAlong += maxSegmentLength; } distanceAlong -= length; } // Make sure we get the exact position of the last point subdivided.Add(path[path.Count - 1]); } else { subdivisions = Mathf.Max(subdivisions, 0); if (subdivisions > 10) { Debug.LogWarning("Very large number of subdivisions. Cowardly refusing to subdivide every segment into more than " + (1 << subdivisions) + " subsegments"); subdivisions = 10; } int steps = 1 << subdivisions; //Good Game //subdivided = ListPool<Vector3>.Claim((path.Count-1)*steps + 1); subdivided = ListPool <VInt3> .Claim((path.Count - 1) *steps + 1); Polygon.Subdivide(path, subdivided, steps); } if (strength > 0) { for (int it = 0; it < iterations; it++) { //Good Game //Vector3 prev = subdivided[0]; VInt3 prev = subdivided[0]; for (int i = 1; i < subdivided.Count - 1; i++) { //Good Game //Vector3 tmp = subdivided[i]; VInt3 tmp = subdivided[i]; // prev is at this point set to the value that subdivided[i-1] had before this loop started // Move the point closer to the average of the adjacent points //Good Game //subdivided[i] = Vector3.Lerp(tmp, (prev+subdivided[i+1])/2F, strength); subdivided[i] = VInt3.Lerp(tmp, (prev + subdivided[i + 1]) / 2F, strength); prev = tmp; } } } return(subdivided); }
public void Update() { if (Time.time >= nextRepath && canSearchAgain) { RecalculatePath(); } Vector3 pos = transform.position; if (vectorPath != null && vectorPath.Count != 0) { //Good Game //while ((controller.To2D(pos - vectorPath[wp]).sqrMagnitude < moveNextDist*moveNextDist && wp != vectorPath.Count-1) || wp == 0) //while ((controller.To2D((VInt3)pos - vectorPath[wp]).sqrMagnitude < moveNextDist*moveNextDist && wp != vectorPath.Count-1) || wp == 0) //Debug.Log($"--waypoint--{gameObject.name}--{(controller.To2D((VInt3)pos - vectorPath[wp])).sqrMagnitude / 1000000f}--{moveNextDist * moveNextDist}"); while ((((Vector2)controller.To2D((VInt3)pos - vectorPath[wp])).sqrMagnitude < moveNextDist * moveNextDist && wp != vectorPath.Count - 1) || wp == 0) { wp++; //Debug.Log($"--agent{transform.GetSiblingIndex()}--wp--{wp}"); } // Current path segment goes from vectorPath[wp-1] to vectorPath[wp] // We want to find the point on that segment that is 'moveNextDist' from our current position. // This can be visualized as finding the intersection of a circle with radius 'moveNextDist' // centered at our current position with that segment. var p1 = vectorPath[wp - 1]; var p2 = vectorPath[wp]; // Calculate the intersection with the circle. This involves some math. //Good Game //var t = VectorMath.LineCircleIntersectionFactor(controller.To2D(transform.position), controller.To2D(p1), controller.To2D(p2), moveNextDist); var t = VectorMath.LineCircleIntersectionFactor((Vector2)controller.To2D((VInt3)transform.position), (Vector2)controller.To2D(p1), (Vector2)controller.To2D(p2), moveNextDist); // Clamp to a point on the segment t = Mathf.Clamp01(t); //Good Game //Vector3 waypoint = Vector3.Lerp(p1, p2, t); VInt3 waypoint = VInt3.Lerp(p1, p2, t); // Calculate distance to the end of the path //Good Game /*float remainingDistance = controller.To2D(waypoint - pos).magnitude + controller.To2D(waypoint - p2).magnitude; * for (int i = wp; i < vectorPath.Count - 1; i++) * remainingDistance += controller.To2D(vectorPath[i+1] - vectorPath[i]).magnitude;*/ float remainingDistance = controller.To2D(waypoint - (VInt3)pos).magnitude + controller.To2D(waypoint - p2).magnitude; for (int i = wp; i < vectorPath.Count - 1; i++) { remainingDistance += controller.To2D(vectorPath[i + 1] - vectorPath[i]).magnitude; } // Set the target to a point in the direction of the current waypoint at a distance // equal to the remaining distance along the path. Since the rvo agent assumes that // it should stop when it reaches the target point, this will produce good avoidance // behavior near the end of the path. When not close to the end point it will act just // as being commanded to move in a particular direction, not toward a particular point //GG remainingDistance /= 1000000; //Debug.Log("--remian--" + remainingDistance.ToString("f2")); //var rvoTarget = (waypoint - pos).normalized * remainingDistance + pos; var rvoTarget = ((Vector3)waypoint - pos).normalized * remainingDistance + pos; // When within [slowdownDistance] units from the target, use a progressively lower speed var desiredSpeed = Mathf.Clamp01(remainingDistance / slowdownDistance) * maxSpeed; Debug.DrawLine(transform.position, waypoint, Color.red); //GG //controller.SetTarget(rvoTarget, desiredSpeed, maxSpeed); //Debug.Log($"--target--{rvoTarget}--de speed--{desiredSpeed}--maxSpeed--{maxSpeed}"); controller.SetTarget((VInt3)rvoTarget, (int)(desiredSpeed * 1000), (int)(maxSpeed * 1000)); //controller.SetTarget((VInt3)rvoTarget, 50, (int)(maxSpeed* 1000)); } else { // Stand still //GG //controller.SetTarget(pos, maxSpeed, maxSpeed); controller.SetTarget((VInt3)pos, (int)(maxSpeed * 1000), (int)(maxSpeed * 1000)); } // Get a processed movement delta from the rvo controller and move the character. // This is based on information from earlier frames. //GG //var movementDelta = controller.CalculateMovementDelta(Time.deltaTime); //var movementDelta = controller.CalculateMovementDelta((int)(Time.deltaTime * 1000)); var movementDelta = controller.CalculateMovementDelta(50); //GG //pos += movementDelta; pos += (Vector3)movementDelta; //Debug.Log("--" + movementDelta.ToString()); //Rotate the character if the velocity is not extremely small //GG //if (Time.deltaTime > 0 && movementDelta.magnitude / Time.deltaTime > 0.01f) if (Time.deltaTime > 0 && ((Vector3)movementDelta).magnitude / Time.deltaTime > 0.01f) { var rot = transform.rotation; //GG //var targetRot = Quaternion.LookRotation((Vector3)movementDelta, (Vector3)controller.To3D(Vector2.zero, 1)); var targetRot = Quaternion.LookRotation((Vector3)movementDelta, (Vector3)controller.To3D(VInt2.zero, 1000)); const float RotationSpeed = 5; if (controller.movementPlane == MovementPlane.XY) { targetRot = targetRot * Quaternion.Euler(-90, 180, 0); } transform.rotation = Quaternion.Slerp(rot, targetRot, Time.deltaTime * RotationSpeed); } if (controller.movementPlane == MovementPlane.XZ) { RaycastHit hit; if (Physics.Raycast(pos + Vector3.up, Vector3.down, out hit, 2, groundMask)) { pos.y = hit.point.y; } } transform.position = pos; //Good Game /*if(pos.x > 200 || pos.y > 200 || pos.z > 200 || pos.x > -200 || pos.z < -200) * Debug.LogError("--" + gameObject.name + "--" + pos);*/ }
/** Will calculate a number of points around \a center which are on the graph and are separated by \a clearance from each other. * The maximum distance from \a center to any point will be \a radius. * Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected. * This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position * this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using * some kind of local avoidance. * * \param center The point to generate points around * \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph. * Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best. * \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a center. * The new points will overwrite the existing points in the list. The result will be in world space, not relative to \a center. * \param radius The final points will be at most this distance from \a center. * \param clearanceRadius The points will if possible be at least this distance from each other. * * \todo Write unit tests */ //Good Game //public static void GetPointsAroundPoint (Vector3 center, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) { public static void GetPointsAroundPoint(VInt3 center, IRaycastableGraph g, List <VInt3> previousPoints, float radius, float clearanceRadius) { if (g == null) { throw new System.ArgumentNullException("g"); } var graph = g as NavGraph; if (graph == null) { throw new System.ArgumentException("g is not a NavGraph"); } NNInfoInternal nn = graph.GetNearestForce(center, NNConstraint.Default); center = nn.clampedPosition; if (nn.node == null) { // No valid point to start from return; } // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5 radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2)); clearanceRadius *= clearanceRadius; for (int i = 0; i < previousPoints.Count; i++) { //Good Game //Vector3 dir = previousPoints[i]; VInt3 dir = previousPoints[i]; float magn = dir.magnitude; if (magn > 0) { dir /= magn; } float newMagn = radius; //magn > radius ? radius : magn; dir *= newMagn; GraphHitInfo hit; int tests = 0; while (true) { //Good Game //Vector3 pt = center + dir; VInt3 pt = center + dir; if (g.Linecast(center, pt, nn.node, out hit)) { //Good Game //if (hit.point == Vector3.zero) if (hit.point == VInt3.zero) { // Oops, linecast actually failed completely // try again unless we have tried lots of times // then we just continue anyway tests++; if (tests > 8) { previousPoints[i] = pt; break; } } else { pt = hit.point; } } bool worked = false; for (float q = 0.1f; q <= 1.0f; q += 0.05f) { //Good Game //Vector3 qt = Vector3.Lerp(center, pt, q); VInt3 qt = VInt3.Lerp(center, pt, q); worked = true; for (int j = 0; j < i; j++) { if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) { worked = false; break; } } // Abort after 8 tests or when we have found a valid point if (worked || tests > 8) { worked = true; previousPoints[i] = qt; break; } } // Break out of nested loop if (worked) { break; } // If we could not find a valid point, reduce the clearance radius slightly to improve // the chances next time clearanceRadius *= 0.9f; // This will pick points in 2D closer to the edge of the circle with a higher probability //Good Game //dir = (Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5)); dir = (VInt3)(Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5f)); dir.y = 0; tests++; } } }