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