private void connectNeighbors()
{
Utils.Rectangle tmpRec = new Utils.Rectangle();
Utils.Ray2D ray = new Utils.Ray2D();
//Though box collider here but still consider it as circle, hard to check collisions when rectange rotates
float colliderHalfX = templateAgent.GetComponent <BoxCollider>().size.x / 2;
float colliderHalfZ = templateAgent.GetComponent <BoxCollider>().size.z / 2;
float agentRadius = Mathf.Sqrt(colliderHalfX * colliderHalfX + colliderHalfZ * colliderHalfZ);
//connect map nodes to agent
for (int i = 0; i < numMapNode; i++)
{
for (int j = i + 1; j < totalNode; j++)
{
ray.updateRay(myNodes[i].position, myNodes[j].position);
for (int k = 0; k < obstacles.transform.childCount; k++)
{
var curObstacle = obstacles.transform.GetChild(k);
tmpRec.origin = curObstacle.position;
tmpRec.sideLength = new Vector2(curObstacle.localScale.x, curObstacle.localScale.z);
if (Utils.CollisionCheck.rayAABB2D(tmpRec, ray, agentRadius))
{
break;
}
else if (k == obstacles.transform.childCount - 1)
{
myNodes[i].neighbors.Add(j);
myNodes[j].neighbors.Add(i);
}
}
}
}
//connect agent to its goal if possible
for (int i = startIdxOfAgent; i < startIdxOfAgent + numAgentNode; i++)
{
int j = i + numAgentNode;
ray.updateRay(myNodes[i].position, myNodes[j].position);
for (int k = 0; k < obstacles.transform.childCount; k++)
{
var curObstacle = obstacles.transform.GetChild(k);
tmpRec.origin = curObstacle.position;
tmpRec.sideLength = new Vector2(curObstacle.localScale.x, curObstacle.localScale.z);
if (Utils.CollisionCheck.rayAABB2D(tmpRec, ray, agentRadius))
{
break;
}
else if (k == obstacles.transform.childCount - 1)
{
myNodes[i].neighbors.Add(j);
myNodes[j].neighbors.Add(i);
}
}
}
}
public static bool IsValid(Ray2D ray)
{
return(ray.direction != Vector2.zero);
}