public static float Angle(Int3 lhs, Int3 rhs)
{
double num = (double)Int3.Dot(lhs, rhs) / ((double)lhs.magnitude * (double)rhs.magnitude);
num = ((num >= -1.0) ? ((num <= 1.0) ? num : 1.0) : -1.0);
return((float)Math.Acos(num));
}
public static IntFactor AngleInt(Int3 lhs, Int3 rhs)
{
long den = (long)lhs.magnitude * (long)rhs.magnitude;
return(IntMath.acos((long)Int3.Dot(ref lhs, ref rhs), den));
}
/// <summary>
///
/// </summary>
/// <param name="node">所在node</param>
/// <param name="edge">所沿着的边</param>
/// <param name="srcLoc">from</param>
/// <param name="destLoc">to</param>
/// <param name="result">实际可到达的点</param>
private static void MoveAlongEdge(NavMeshNode node, int edge, Int3 srcLoc, Int3 destLoc, out Int3 result, bool checkAnotherEdge = true)
{
bool flag;
//DebugHelper.Assert((edge >= 0) && (edge <= 2));
Int3 vertex = node.GetVertex(edge);
Int3 num2 = node.GetVertex((edge + 1) % 3);
Int3 a = destLoc - srcLoc;
a.y = 0;
Int3 lhs = num2 - vertex;
lhs.y = 0;
lhs.NormalizeTo(1000);
int num5 = 0;
// 点乘移动向量和边向量
num5 = (lhs.x * a.x) + (lhs.z * a.z);
// 移动向量和边向量的交点
Int3 rhs = Polygon.IntersectionPoint(ref vertex, ref num2, ref srcLoc, ref destLoc, out flag);
// 不相交？
if (!flag)
{
// 边和from to存在不共线，则不能移动
if (!VectorMath.IsColinearXZ(vertex, num2, srcLoc) || !VectorMath.IsColinearXZ(vertex, num2, destLoc))
{
result = srcLoc;
return;
}
// 计算本三角形内可移动到的点 rhs
// 移动向量和边向量 同向
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));
}
}
// 计算交点到边的顶点的距离1000倍
int num12 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 0xf4240L); // 一百万
int num13 = IntMath.Sqrt(num2.XZSqrMagnitude(rhs) * 0xf4240L);
// num5是在边上的投影
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();
Int3 num14 = num2 - vertex;
num14.y = 0;
num14 *= 10000;
long magnitude = num14.magnitude;
VFactor factor = new VFactor();
factor.nom = num5;
factor.den = magnitude * 1000L;
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;
Int3 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;
}
Int3 num23 = (Int3)((lhs * num20) / 1000000f);
NavMeshNode neighborByEdge = node.GetNeighborByEdge(num21, out num24);
if (neighborByEdge != null)
{
checkedNodes.Add(node);
MoveFromNode(neighborByEdge, num24, num22, num23 + num22, out result);
}
else
{
if (checkAnotherEdge)
{
Int3 num27 = node.GetVertex((edge + 2) % 3) - num22;
if (Int3.Dot(num27.NormalizeTo(1000), num23) > 0)
{
checkedNodes.Add(node);
MoveAlongEdge(node, num21, num22, num23 + num22, out result, false);
return;
}
}
result = num22;
}
}
}
private static void MoveAlongEdge(TriangleMeshNode node, int edge, Int3 srcLoc, Int3 destLoc, out Int3 result, bool checkAnotherEdge = true)
{
Int3 vertex = node.GetVertex(edge);
Int3 vertex2 = node.GetVertex((edge + 1) % 3);
Int3 vInt = destLoc - srcLoc;
vInt.y = 0;
Int3 vInt2 = vertex2 - vertex;
vInt2.y = 0;
vInt2.NormalizeTo(1000);
int num;
num = vInt2.x * vInt.x + vInt2.z * vInt.z;
bool flag;
Int3 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) ? vertex2 : destLoc);
}
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)) ? vertex : destLoc);
}
}
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();
Int3 lhs = vertex2 - vertex;
lhs.y = 0;
lhs *= 10000;
long num8 = (long)lhs.magnitude;
IntFactor vFactor = default(IntFactor);
vFactor.numerator = (long)num;
vFactor.denominator = 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;
Int3 vInt4;
if (num < num6)
{
rhs2 = num - num6;
edge2 = (edge + 2) % 3;
vInt4 = vertex;
}
else
{
rhs2 = num - num7;
edge2 = (edge + 1) % 3;
vInt4 = vertex2;
}
Int3 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, out result);
}
else
{
if (checkAnotherEdge)
{
Int3 vertex3 = node.GetVertex((edge + 2) % 3);
Int3 lhs2 = (vertex3 - vInt4).NormalizeTo(1000);
if (Int3.Dot(lhs2, vInt5) > 0)
{
PathfindingUtility.checkedNodes.Add(node);
PathfindingUtility.MoveAlongEdge(node, edge2, vInt4, vInt5 + vInt4, out result, false);
return;
}
}
result = vInt4;
}
}
}
