|
public static IntersectionFactor ( |
||
| start1 | ||
| end1 | ||
| start2 | ||
| end2 | ||
| factor1 | float | |
| factor2 | float | |
| Результат | bool | |
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
Int3 @int = (Int3)tmp_end;
Int3 int2 = (Int3)tmp_origin;
hit = default(GraphHitInfo);
if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
{
throw new ArgumentException("origin is NaN");
}
if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
{
throw new ArgumentException("end is NaN");
}
TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;
if (triangleMeshNode == null)
{
triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
if (triangleMeshNode == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return(true);
}
}
if (int2 == @int)
{
hit.node = triangleMeshNode;
return(false);
}
int2 = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2);
hit.origin = (Vector3)int2;
if (!triangleMeshNode.Walkable)
{
hit.point = (Vector3)int2;
hit.tangentOrigin = (Vector3)int2;
return(true);
}
List <Vector3> list = ListPool <Vector3> .Claim();
List <Vector3> list2 = ListPool <Vector3> .Claim();
int num = 0;
while (true)
{
num++;
if (num > 2000)
{
break;
}
TriangleMeshNode triangleMeshNode2 = null;
if (trace != null)
{
trace.Add(triangleMeshNode);
}
if (triangleMeshNode.ContainsPoint(@int))
{
goto Block_9;
}
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
{
list.Clear();
list2.Clear();
if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
{
Vector3 vector = list[0];
Vector3 vector2 = list2[0];
if (Polygon.LeftNotColinear(vector, vector2, hit.origin) || !Polygon.LeftNotColinear(vector, vector2, tmp_end))
{
float num2;
float num3;
if (Polygon.IntersectionFactor(vector, vector2, hit.origin, tmp_end, out num2, out num3))
{
if (num3 >= 0f)
{
if (num2 >= 0f && num2 <= 1f)
{
triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
break;
}
}
}
}
}
}
}
if (triangleMeshNode2 == null)
{
goto Block_18;
}
triangleMeshNode = triangleMeshNode2;
}
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
Block_9:
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
Block_18:
int vertexCount = triangleMeshNode.GetVertexCount();
for (int j = 0; j < vertexCount; j++)
{
Vector3 vector3 = (Vector3)triangleMeshNode.GetVertex(j);
Vector3 vector4 = (Vector3)triangleMeshNode.GetVertex((j + 1) % vertexCount);
if (Polygon.LeftNotColinear(vector3, vector4, hit.origin) || !Polygon.LeftNotColinear(vector3, vector4, tmp_end))
{
float num4;
float num5;
if (Polygon.IntersectionFactor(vector3, vector4, hit.origin, tmp_end, out num4, out num5))
{
if (num5 >= 0f)
{
if (num4 >= 0f && num4 <= 1f)
{
Vector3 point = vector3 + (vector4 - vector3) * num4;
hit.point = point;
hit.node = triangleMeshNode;
hit.tangent = vector4 - vector3;
hit.tangentOrigin = vector3;
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
}
}
}
}
}
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
var end = (Int3)tmp_end;
var origin = (Int3)tmp_origin;
hit = new GraphHitInfo();
if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
{
throw new System.ArgumentException("origin is NaN");
}
if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
{
throw new System.ArgumentException("end is NaN");
}
var node = hint as TriangleMeshNode;
if (node == null)
{
node = (graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode;
if (node == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return(true);
}
}
if (origin == end)
{
hit.node = node;
return(false);
}
origin = (Int3)node.ClosestPointOnNode((Vector3)origin);
hit.origin = (Vector3)origin;
if (!node.Walkable)
{
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return(true);
}
List <Vector3> left = Pathfinding.Util.ListPool <Vector3> .Claim(); //new List<Vector3>(1);
List <Vector3> right = Pathfinding.Util.ListPool <Vector3> .Claim(); //new List<Vector3>(1);
int counter = 0;
while (true)
{
counter++;
if (counter > 2000)
{
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(true);
}
TriangleMeshNode newNode = null;
if (trace != null)
{
trace.Add(node);
}
if (node.ContainsPoint(end))
{
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(false);
}
for (int i = 0; i < node.connections.Length; i++)
{
//Nodes on other graphs should not be considered
//They might even be of other types (not MeshNode)
if (node.connections[i].GraphIndex != node.GraphIndex)
{
continue;
}
left.Clear();
right.Clear();
if (!node.GetPortal(node.connections[i], left, right, false))
{
continue;
}
Vector3 a = left[0];
Vector3 b = right[0];
//i.e Right or colinear
if (!Polygon.LeftNotColinear(a, b, hit.origin))
{
if (Polygon.LeftNotColinear(a, b, tmp_end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor(a, b, hit.origin, tmp_end, out factor1, out factor2))
{
//Intersection behind the start
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
newNode = node.connections[i] as TriangleMeshNode;
break;
}
}
}
if (newNode == null)
{
//Possible edge hit
int vs = node.GetVertexCount();
for (int i = 0; i < vs; i++)
{
var a = (Vector3)node.GetVertex(i);
var b = (Vector3)node.GetVertex((i + 1) % vs);
//i.e right or colinear
if (!Polygon.LeftNotColinear(a, b, hit.origin))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
if (Polygon.LeftNotColinear(a, b, tmp_end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor(a, b, hit.origin, tmp_end, out factor1, out factor2))
{
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
Vector3 intersectionPoint = a + (b - a) * factor1;
hit.point = intersectionPoint;
hit.node = node;
hit.tangent = b - a;
hit.tangentOrigin = a;
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(true);
}
}
}
//Ok, this is wrong...
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(false);
}
node = newNode;
}
}
public Vector3 ClampMovement(Vector3 direction)
{
Vector3 targetDir = direction * delta;
Vector3 targetPoint = transform.position + direction;
Vector3 bestPoint = targetPoint;
float bestPointDist = 0;
//float lowestVO = float.PositiveInfinity;
int lowestVOCounter = 0;
//List<VO> vos = new List<VO> ();
vos.Clear();
float velocityMagn = velocity.magnitude;
foreach (LocalAvoidance agent in agents)
{
if (agent == this || agent == null)
{
continue;
}
Vector3 rCenter = agent.transform.position - transform.position;
float dist = rCenter.magnitude;
float doubleRad = (radius + agent.radius);
if (dist > targetDir.magnitude * delta + doubleRad + velocityMagn + agent.GetVelocity().magnitude)
{
continue;
}
if (lowestVOCounter > maxVOCounter)
{
continue;
}
else
{
lowestVOCounter++;
}
VO vo = new VO();
vo.origin = transform.position + Vector3.Lerp(velocity * delta, agent.GetVelocity() * delta, responability);
vo.direction = rCenter.normalized;
if (doubleRad > rCenter.magnitude)
{
vo.angle = Mathf.PI * 0.5F;
/*vo.limit = dist-doubleRad;
* if (vo.limit < 0) {
* vo.limit = 0.001F;
* }
*
* vos.Add (vo);
* continue;*/
//doubleRad = rCenter.magnitude*0.99F;
//continue;
}
else
{
vo.angle = (float)Math.Asin(doubleRad / dist);
}
//float n = (float)Math.Tan (Math.Asin ((doubleRad)/rCenter.magnitude))*(rCenter.magnitude-doubleRad);
//Vector3 m = (rCenter.magnitude - doubleRad)*rCenter.normalized;//+(agent.velocity+velocity)/2F;
/*if (drawGizmos) {
* Debug.DrawLine (transform.position,m,Color.red);
* Debug.Log (n);
* }*/
vo.limit = dist - doubleRad; //m.magnitude;
if (vo.limit < 0)
{
vo.origin += vo.direction * (vo.limit);
vo.limit = 0.000F;
}
float ax = Mathf.Atan2(vo.direction.z, vo.direction.x);
vo.pRight = new Vector3(Mathf.Cos(ax + vo.angle), 0, Mathf.Sin(ax + vo.angle));
vo.pLeft = new Vector3(Mathf.Cos(ax - vo.angle), 0, Mathf.Sin(ax - vo.angle));
vo.nLeft = new Vector3(Mathf.Cos(ax + vo.angle - Mathf.PI * 0.5F), 0, Mathf.Sin(ax + vo.angle - Mathf.PI * 0.5F));
vo.nRight = new Vector3(Mathf.Cos(ax - vo.angle + Mathf.PI * 0.5F), 0, Mathf.Sin(ax - vo.angle + Mathf.PI * 0.5F));
//Debug.DrawRay (vo.origin,vo.nLeft,Color.red);
//Debug.DrawRay (vo.origin,vo.nRight,Color.magenta);
//vo.limit = Mathf.Min (rCenter.magnitude - doubleRad, rCenter.magnitude/2F);
//doubleRad /= delta;
//doubleRad /= 2;
//vo.limit = doubleRad/Mathf.Tan (vo.angle);
vos.Add(vo);
}
if (resType == ResolutionType.Geometric)
{
//Is there any need to run more calculations
for (int i = 0; i < vos.Count; i++)
{
if (vos[i].Contains(bestPoint))
{
bestPointDist = float.PositiveInfinity;
if (drawGizmos)
{
Debug.DrawRay(bestPoint, Vector3.down, Color.red);
}
bestPoint = transform.position;
break;
}
}
if (drawGizmos)
{
for (int i = 0; i < vos.Count; i++)
{
vos[i].Draw(Color.black);
/*if (vos[i].Contains (testPoint)) {
* Debug.DrawLine (vos[i].origin,testPoint,Color.green);
* } else {
* Debug.DrawLine (vos[i].origin,testPoint,Color.red);
* }*/
}
}
if (bestPointDist == 0)
{
return(targetDir);
}
List <VOLine> lines = new List <VOLine> ();
//List<VOIntersection> ints = new List<VOIntersection>();
for (int i = 0; i < vos.Count; i++)
{
VO vo = vos[i];
float a = (float)Math.Atan2(vo.direction.z, vo.direction.x);
//Vector3 cross = Vector3.Cross (vo.direction,Vector3.up);
//Vector3 p1 = cross*(float)Math.Tan (vo.angle)*vo.limit;
Vector3 pLeft = vo.origin + new Vector3((float)Math.Cos(a + vo.angle), 0, (float)Math.Sin(a + vo.angle)) * vo.limit;
Vector3 pRight = vo.origin + new Vector3((float)Math.Cos(a - vo.angle), 0, (float)Math.Sin(a - vo.angle)) * vo.limit;
//Vector3 pLeft = vo.origin+vo.direction*vo.limit+p1;
//Vector3 pRight = vo.origin+vo.direction*vo.limit-p1;
Vector3 pLeft2 = pLeft + new Vector3((float)Math.Cos(a + vo.angle), 0, (float)Math.Sin(a + vo.angle)) * 100;
Vector3 pRight2 = pRight + new Vector3((float)Math.Cos(a - vo.angle), 0, (float)Math.Sin(a - vo.angle)) * 100;
int IgnoreDirection = Polygon.Left(vo.origin, vo.origin + vo.direction, transform.position + velocity) ? 1 : 2; //Vector3.Dot (transform.position+velocity-vo.origin,cross) > 0 ? 2 : 1;//(pRight-(transform.position+velocity)).sqrMagnitude < (pLeft-(transform.position+velocity)).sqrMagnitude ? 2 : 1;//Vector3.Dot (pRight-pLeft,transform.position+velocity-pLeft) > 0 ? 1 : 2;
lines.Add(new VOLine(vo, pLeft, pLeft2, true, 1, IgnoreDirection == 1));
lines.Add(new VOLine(vo, pRight, pRight2, true, 2, IgnoreDirection == 2));
lines.Add(new VOLine(vo, pLeft, pRight, false, 3, false));
bool pLeftInside = false; // || IgnoreDirection == 1;
bool pRightInside = false; // || IgnoreDirection == 2;
if (!pLeftInside)
{
for (int q = 0; q < vos.Count; q++)
{
if (q != i)
{
if (vos[q].Contains(pLeft))
{
pLeftInside = true;
break;
}
}
}
}
if (!pRightInside)
{
for (int q = 0; q < vos.Count; q++)
{
if (q != i)
{
if (vos[q].Contains(pRight))
{
pRightInside = true;
break;
}
}
}
}
vo.AddInt(0, pLeftInside, 1); //pLeftInside || IgnoreDirection == 1,1);
vo.AddInt(0, pRightInside, 2); //pRightInside || IgnoreDirection == 2,2);
vo.AddInt(0, pLeftInside, 3); //pLeftInside ,3);//@ Ignore shouldn't really be there, but it works better with it of some reason
vo.AddInt(1, pRightInside, 3); //pRightInside ,3);//@ Ignore shouldn't really be there, but it works better with it of some reason
}
for (int i = 0; i < lines.Count; i++)
{
for (int j = i + 1; j < lines.Count; j++)
{
//if (i==j) continue;
VOLine line = lines[i];
VOLine line2 = lines[j];
if (line.vo == line2.vo)
{
continue;
}
float factor1;
float factor2;
if (Polygon.IntersectionFactor(line.start, line.end, line2.start, line2.end, out factor1, out factor2))
{
if (factor1 < 0 || factor2 < 0 || (!line.inf && factor1 > 1) || (!line2.inf && factor2 > 1))
{
continue;
}
Vector3 p = line.start + (line.end - line.start) * factor1;
bool inside = line.wrongSide || line2.wrongSide;
if (!inside)
{
for (int q = 0; q < vos.Count; q++)
{
if (vos[q] != line.vo && vos[q] != line2.vo)
{
if (vos[q].Contains(p))
{
inside = true;
break;
}
}
}
}
line.vo.AddInt(factor1, inside, line.id);
line2.vo.AddInt(factor2, inside, line2.id);
//ints.Add (new VOIntersection (line.vo,line2.vo, factor1, factor2,inside));
if (drawGizmos)
{
Debug.DrawRay(line.start + (line.end - line.start) * factor1, Vector3.up, inside ? Color.magenta : Color.green);
}
}
}
}
for (int i = 0; i < vos.Count; i++)
{
Vector3 segmClosest;
if (vos[i].FinalInts(targetPoint, transform.position + velocity, drawGizmos, out segmClosest))
{
float dist = (segmClosest - targetPoint).sqrMagnitude;
if (dist < bestPointDist)
{
bestPoint = segmClosest;
bestPointDist = dist;
if (drawGizmos)
{
Debug.DrawLine(targetPoint + Vector3.up, bestPoint + Vector3.up, Color.red);
}
}
}
}
//}
if (drawGizmos)
{
Debug.DrawLine(targetPoint + Vector3.up, bestPoint + Vector3.up, Color.red);
}
return(Vector3.ClampMagnitude(bestPoint - transform.position, targetDir.magnitude * maxSpeedScale));
}
else if (resType == ResolutionType.Sampled)
{
/*Sampling Solution
* ================= */
Vector3 forward = targetDir;
Vector3 normForward = forward.normalized;
Vector3 right = Vector3.Cross(normForward, Vector3.up);
int circleSamples = 10;
for (int i = 0; i < 10; i++, circleSamples += 2)
{
float radPerSamples = (float)(Math.PI * circlePoint / circleSamples);
float offset = (float)(Math.PI - circlePoint * Math.PI) * 0.5F;
for (int j = 0; j < circleSamples; j++)
{
float a = radPerSamples * j;
Vector3 sample = transform.position + targetDir - (forward * (float)Math.Sin(a + offset) * i * circleScale + right * (float)Math.Cos(a + offset) * i * circleScale);
if (CheckSample(sample, vos))
{
return(sample - transform.position);
}
}
}
for (int i = 0; i < samples.Length; i++)
{
Vector3 sample = transform.position + samples[i].x * right + samples[i].z * normForward + samples[i].y * forward;
if (CheckSample(sample, vos))
{
return(sample - transform.position);
}
}
return(Vector3.zero);
/* End of Sampling Solution
* ========================
*/
}
return(Vector3.zero);
//if (drawGizmos) {
/*HalfPlane[] hps = new HalfPlane[vos.Count];
* for (int i=0;i<vos.Count;i++) {
* HalfPlane hp = (HalfPlane)vos[i];
* hps[i] = hp;
* }
*
* /*HalfPlane hp_top = new HalfPlane ();
* hp_top.point = transform.position+Vector3.forward*speed;
* hp_top.normal = -new Vector3 (0.001F,0,1F).normalized;
* hps[vos.Count+0] = hp_top;
*
* HalfPlane hp_bot = new HalfPlane ();
* hp_bot.point = transform.position-Vector3.forward*speed;
* hp_bot.normal = new Vector3 (0.001F,0,1F).normalized;
* hps[vos.Count+1] = hp_bot;
*
* HalfPlane hp_left = new HalfPlane ();
* hp_left.point = transform.position-Vector3.right*speed;
* hp_left.normal = new Vector3 (1F,0,0.001F).normalized;
* hps[vos.Count+2] = hp_left;
*
* HalfPlane hp_right = new HalfPlane ();
* hp_right.point = transform.position+Vector3.right*speed;
* hp_right.normal = -new Vector3 (1F,0,0.001F).normalized;
* hps[vos.Count+3] = hp_right;*
*
* for (int i=0;i<hps.Length;i++) {
* if (drawGizmos) { hps[i].Draw (); }
* }
*
* for (int i=0;i<hps.Length;i++) {
* HalfPlane hp = hps[i];
* if (hp.Contains (bestPoint)) {
* if (drawGizmos) {
* Debug.DrawLine (bestPoint,hp.point,Color.green);
* }
* continue;
* }
*
*
* Vector3 tangent = Vector3.Cross (hp.normal,Vector3.up);
*
* float leftBound = -100;
* float rightBound = 100;
*
* if (tangent.x < 0) {
* tangent = -tangent;
* }
*
* for (int j=0;j<i;j++) {
* HalfPlane hp2 = hps[j];
* Vector3 intersection = hp.Intersection (hp2);
*
* float dot = Vector3.Dot (tangent,hp2.normal);
* if (dot > 0) {
* leftBound = Mathf.Max (leftBound,intersection.x);
* } else if (dot < 0) {
* rightBound = Mathf.Min (rightBound,intersection.x);
* } else {
* //Parallel
* if (Vector3.Dot (hp.normal,hp2.normal) < 0) {
* if (drawGizmos) {
* Debug.DrawLine (bestPoint,hp.point,Color.red);
* }
* return Vector3.zero;
* }
* }
* /*if (hp.normal.x > hp2.normal.x) {
* if (hp2.normal.x < 0) {
* rightBound = Mathf.Min (intersection.x, rightBound);
* } else {
* leftBound = Mathf.Max (leftBound,intersection.x);
* }
* } else if (hp.normal.x < hp2.normal.x) {
* if (hp2.normal.x < 0) {
* leftBound = Mathf.Max (leftBound,intersection.x);
* } else {
* rightBound = Mathf.Min (rightBound,intersection.x);
* }
* }*
* }
*
* if (drawGizmos) {
* hp.DrawBounds (leftBound,rightBound);
* }
* if (leftBound > rightBound) {
* if (drawGizmos) {
* Debug.DrawRay (bestPoint,Vector3.up,Color.red);
* Debug.DrawLine (bestPoint,hp.point,Color.red);
* }
* return Vector3.zero;
* }
*
* Vector3 closest = hp.ClosestPoint (targetPoint,leftBound,rightBound);
* if (drawGizmos) {
* Debug.DrawLine (bestPoint,closest,Color.red);
* }
* bestPoint = closest;
* }
*
* return bestPoint-transform.position;*/
}
public static bool Linecast(INavmesh graph, VInt3 tmp_origin, VInt3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace)
{
VInt3 vInt = tmp_end;
VInt3 vInt2 = tmp_origin;
hit = default(GraphHitInfo);
if (float.IsNaN((float)(tmp_origin.x + tmp_origin.y + tmp_origin.z)))
{
throw new ArgumentException("origin is NaN");
}
if (float.IsNaN((float)(tmp_end.x + tmp_end.y + tmp_end.z)))
{
throw new ArgumentException("end is NaN");
}
TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;
if (triangleMeshNode == null)
{
triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
if (triangleMeshNode == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return true;
}
}
if (vInt2 == vInt)
{
hit.node = triangleMeshNode;
return false;
}
vInt2 = (VInt3)triangleMeshNode.ClosestPointOnNode((Vector3)vInt2);
hit.origin = vInt2;
if (!triangleMeshNode.Walkable)
{
hit.point = vInt2;
hit.tangentOrigin = vInt2;
return true;
}
List<VInt3> list = ListPool<VInt3>.Claim();
List<VInt3> list2 = ListPool<VInt3>.Claim();
int num = 0;
while (true)
{
num++;
if (num > 2000)
{
break;
}
TriangleMeshNode triangleMeshNode2 = null;
if (trace != null)
{
trace.Add(triangleMeshNode);
}
if (triangleMeshNode.ContainsPoint(vInt))
{
goto Block_9;
}
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
{
list.Clear();
list2.Clear();
if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
{
VInt3 vInt3 = list.get_Item(0);
VInt3 vInt4 = list2.get_Item(0);
if (Polygon.LeftNotColinear(vInt3, vInt4, hit.origin) || !Polygon.LeftNotColinear(vInt3, vInt4, tmp_end))
{
float num2;
float num3;
if (Polygon.IntersectionFactor(vInt3, vInt4, hit.origin, tmp_end, out num2, out num3))
{
if (num3 >= 0f)
{
if (num2 >= 0f && num2 <= 1f)
{
triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
break;
}
}
}
}
}
}
}
if (triangleMeshNode2 == null)
{
goto Block_18;
}
triangleMeshNode = triangleMeshNode2;
}
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return true;
Block_9:
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return false;
Block_18:
int vertexCount = triangleMeshNode.GetVertexCount();
for (int j = 0; j < vertexCount; j++)
{
VInt3 vertex = triangleMeshNode.GetVertex(j);
VInt3 vertex2 = triangleMeshNode.GetVertex((j + 1) % vertexCount);
if (Polygon.LeftNotColinear(vertex, vertex2, hit.origin) || !Polygon.LeftNotColinear(vertex, vertex2, tmp_end))
{
VFactor vFactor;
VFactor vFactor2;
if (Polygon.IntersectionFactor(vertex, vertex2, hit.origin, tmp_end, out vFactor, out vFactor2))
{
if (!vFactor2.IsNegative)
{
if (!vFactor.IsNegative && vFactor.nom / vFactor.den <= 1L)
{
VInt3 vInt5 = (vertex2 - vertex) * (float)vFactor.nom;
vInt5 = IntMath.Divide(vInt5, vFactor.den);
vInt5 += vertex;
hit.point = vInt5;
hit.node = triangleMeshNode;
hit.tangent = vertex2 - vertex;
hit.tangentOrigin = vertex;
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return true;
}
}
}
}
}
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return false;
}
public Vector3 ClampMovement(Vector3 direction)
{
Vector3 vector = direction * this.delta;
Vector3 vector2 = base.transform.position + direction;
Vector3 vector3 = vector2;
float num = 0f;
int num2 = 0;
this.vos.Clear();
float magnitude = this.velocity.magnitude;
LocalAvoidance[] array = this.agents;
for (int i = 0; i < array.Length; i++)
{
LocalAvoidance localAvoidance = array[i];
if (!(localAvoidance == this) && !(localAvoidance == null))
{
Vector3 vector4 = localAvoidance.transform.position - base.transform.position;
float magnitude2 = vector4.magnitude;
float num3 = this.radius + localAvoidance.radius;
if (magnitude2 <= vector.magnitude * this.delta + num3 + magnitude + localAvoidance.GetVelocity().magnitude)
{
if (num2 <= 50)
{
num2++;
LocalAvoidance.VO vO = new LocalAvoidance.VO();
vO.origin = base.transform.position + Vector3.Lerp(this.velocity * this.delta, localAvoidance.GetVelocity() * this.delta, this.responability);
vO.direction = vector4.normalized;
if (num3 > vector4.magnitude)
{
vO.angle = 1.57079637f;
}
else
{
vO.angle = (float)Math.Asin((double)(num3 / magnitude2));
}
vO.limit = magnitude2 - num3;
if (vO.limit < 0f)
{
vO.origin += vO.direction * vO.limit;
vO.limit = 0f;
}
float num4 = Mathf.Atan2(vO.direction.z, vO.direction.x);
vO.pRight = new Vector3(Mathf.Cos(num4 + vO.angle), 0f, Mathf.Sin(num4 + vO.angle));
vO.pLeft = new Vector3(Mathf.Cos(num4 - vO.angle), 0f, Mathf.Sin(num4 - vO.angle));
vO.nLeft = new Vector3(Mathf.Cos(num4 + vO.angle - 1.57079637f), 0f, Mathf.Sin(num4 + vO.angle - 1.57079637f));
vO.nRight = new Vector3(Mathf.Cos(num4 - vO.angle + 1.57079637f), 0f, Mathf.Sin(num4 - vO.angle + 1.57079637f));
this.vos.Add(vO);
}
}
}
}
if (this.resType == LocalAvoidance.ResolutionType.Geometric)
{
for (int j = 0; j < this.vos.Count; j++)
{
if (this.vos[j].Contains(vector3))
{
num = float.PositiveInfinity;
if (this.drawGizmos)
{
Debug.DrawRay(vector3, Vector3.down, Color.red);
}
vector3 = base.transform.position;
break;
}
}
if (this.drawGizmos)
{
for (int k = 0; k < this.vos.Count; k++)
{
this.vos[k].Draw(Color.black);
}
}
if (num == 0f)
{
return(vector);
}
List <LocalAvoidance.VOLine> list = new List <LocalAvoidance.VOLine>();
for (int l = 0; l < this.vos.Count; l++)
{
LocalAvoidance.VO vO2 = this.vos[l];
float num5 = (float)Math.Atan2((double)vO2.direction.z, (double)vO2.direction.x);
Vector3 vector5 = vO2.origin + new Vector3((float)Math.Cos((double)(num5 + vO2.angle)), 0f, (float)Math.Sin((double)(num5 + vO2.angle))) * vO2.limit;
Vector3 vector6 = vO2.origin + new Vector3((float)Math.Cos((double)(num5 - vO2.angle)), 0f, (float)Math.Sin((double)(num5 - vO2.angle))) * vO2.limit;
Vector3 end = vector5 + new Vector3((float)Math.Cos((double)(num5 + vO2.angle)), 0f, (float)Math.Sin((double)(num5 + vO2.angle))) * 100f;
Vector3 end2 = vector6 + new Vector3((float)Math.Cos((double)(num5 - vO2.angle)), 0f, (float)Math.Sin((double)(num5 - vO2.angle))) * 100f;
int num6 = (!Polygon.Left(vO2.origin, vO2.origin + vO2.direction, base.transform.position + this.velocity)) ? 2 : 1;
list.Add(new LocalAvoidance.VOLine(vO2, vector5, end, true, 1, num6 == 1));
list.Add(new LocalAvoidance.VOLine(vO2, vector6, end2, true, 2, num6 == 2));
list.Add(new LocalAvoidance.VOLine(vO2, vector5, vector6, false, 3, false));
bool flag = false;
bool flag2 = false;
if (!flag)
{
for (int m = 0; m < this.vos.Count; m++)
{
if (m != l && this.vos[m].Contains(vector5))
{
flag = true;
break;
}
}
}
if (!flag2)
{
for (int n = 0; n < this.vos.Count; n++)
{
if (n != l && this.vos[n].Contains(vector6))
{
flag2 = true;
break;
}
}
}
vO2.AddInt(0f, flag, 1);
vO2.AddInt(0f, flag2, 2);
vO2.AddInt(0f, flag, 3);
vO2.AddInt(1f, flag2, 3);
}
for (int num7 = 0; num7 < list.Count; num7++)
{
for (int num8 = num7 + 1; num8 < list.Count; num8++)
{
LocalAvoidance.VOLine vOLine = list[num7];
LocalAvoidance.VOLine vOLine2 = list[num8];
if (vOLine.vo != vOLine2.vo)
{
float num9;
float num10;
if (Polygon.IntersectionFactor(vOLine.start, vOLine.end, vOLine2.start, vOLine2.end, out num9, out num10))
{
if (num9 >= 0f && num10 >= 0f && (vOLine.inf || num9 <= 1f) && (vOLine2.inf || num10 <= 1f))
{
Vector3 p = vOLine.start + (vOLine.end - vOLine.start) * num9;
bool flag3 = vOLine.wrongSide || vOLine2.wrongSide;
if (!flag3)
{
for (int num11 = 0; num11 < this.vos.Count; num11++)
{
if (this.vos[num11] != vOLine.vo && this.vos[num11] != vOLine2.vo && this.vos[num11].Contains(p))
{
flag3 = true;
break;
}
}
}
vOLine.vo.AddInt(num9, flag3, vOLine.id);
vOLine2.vo.AddInt(num10, flag3, vOLine2.id);
if (this.drawGizmos)
{
Debug.DrawRay(vOLine.start + (vOLine.end - vOLine.start) * num9, Vector3.up, (!flag3) ? Color.green : Color.magenta);
}
}
}
}
}
}
for (int num12 = 0; num12 < this.vos.Count; num12++)
{
Vector3 vector7;
if (this.vos[num12].FinalInts(vector2, base.transform.position + this.velocity, this.drawGizmos, out vector7))
{
float sqrMagnitude = (vector7 - vector2).sqrMagnitude;
if (sqrMagnitude < num)
{
vector3 = vector7;
num = sqrMagnitude;
if (this.drawGizmos)
{
Debug.DrawLine(vector2 + Vector3.up, vector3 + Vector3.up, Color.red);
}
}
}
}
if (this.drawGizmos)
{
Debug.DrawLine(vector2 + Vector3.up, vector3 + Vector3.up, Color.red);
}
return(Vector3.ClampMagnitude(vector3 - base.transform.position, vector.magnitude * this.maxSpeedScale));
}
else
{
if (this.resType == LocalAvoidance.ResolutionType.Sampled)
{
Vector3 a = vector;
Vector3 normalized = a.normalized;
Vector3 a2 = Vector3.Cross(normalized, Vector3.up);
int num13 = 10;
int num14 = 0;
while (num14 < 10)
{
float num15 = (float)(3.1415926535897931 * (double)this.circlePoint / (double)num13);
float num16 = (float)(3.1415926535897931 - (double)this.circlePoint * 3.1415926535897931) * 0.5f;
for (int num17 = 0; num17 < num13; num17++)
{
float num18 = num15 * (float)num17;
Vector3 vector8 = base.transform.position + vector - (a * (float)Math.Sin((double)(num18 + num16)) * (float)num14 * this.circleScale + a2 * (float)Math.Cos((double)(num18 + num16)) * (float)num14 * this.circleScale);
if (this.CheckSample(vector8, this.vos))
{
return(vector8 - base.transform.position);
}
}
num14++;
num13 += 2;
}
for (int num19 = 0; num19 < this.samples.Length; num19++)
{
Vector3 vector9 = base.transform.position + this.samples[num19].x * a2 + this.samples[num19].z * normalized + this.samples[num19].y * a;
if (this.CheckSample(vector9, this.vos))
{
return(vector9 - base.transform.position);
}
}
return(Vector3.zero);
}
return(Vector3.zero);
}
}