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);
}
}
}
}
// Start is called before the first frame update
private void sampleNodes(int start, int end, Vector3 center)
{
Utils.Rectangle tmpRec = new Utils.Rectangle();
Utils.Circle tmpCircle = new Utils.Circle();
tmpCircle.radius = agentRadius;
for (int i = start; i < end; i++)
{
bool collideObstacle = true;
while (collideObstacle)
{
Vector3 tmpPos = new Vector3(Random.Range(-ConstValues.kCrowdRange, ConstValues.kCrowdRange), 0,
Random.Range(-ConstValues.kCrowdRange, ConstValues.kCrowdRange)) - center;
for (int j = 0; j < i; j++)
{
tmpRec.origin = crowd[i].position;
tmpRec.sideLength = new Vector2(colliderX, colliderZ);
tmpCircle.origin = tmpPos;
if (Utils.CollisionCheck.circleAABB2D(tmpRec, tmpCircle))
{
break;
}
else if (j == i - 1)
{
crowd[i].position = tmpPos;
crowd[i].center = center;
collideObstacle = false;
}
}
if (i == 0)
{
crowd[i].position = tmpPos;
crowd[i].center = center;
collideObstacle = false;
}
}
}
}
private void sampleNodes(int numNodeToUpdate)
{
// sample nodes from 0 to numNodeToUpdate
Utils.Rectangle tmpRec = new Utils.Rectangle();
Utils.Circle tmpCircle = new Utils.Circle();
float colliderHalfX = templateAgent.GetComponent <BoxCollider>().size.x / 2;
float colliderHalfZ = templateAgent.GetComponent <BoxCollider>().size.z / 2;
float agentRadius = Mathf.Sqrt(colliderHalfX * colliderHalfX + colliderHalfZ * colliderHalfZ);
tmpCircle.radius = agentRadius;
for (int i = 0; i < numNodeToUpdate; i++)
{
bool collideObstacle = true;
while (collideObstacle)
{
Vector3 tmpPos = new Vector3(Random.Range(-ConstValues.kPRMSize * 5, ConstValues.kPRMSize * 5), 0,
Random.Range(-ConstValues.kPRMSize * 5, ConstValues.kPRMSize * 5));
for (int j = 0; j < obstacles.transform.childCount; j++)
{
var curObstacle = obstacles.transform.GetChild(j);
tmpRec.origin = curObstacle.position;
tmpRec.sideLength = new Vector2(curObstacle.localScale.x, curObstacle.localScale.z);
tmpCircle.origin = tmpPos;
if (Utils.CollisionCheck.circleAABB2D(tmpRec, tmpCircle))
{
break;
}
else if (j == obstacles.transform.childCount - 1)
{
myNodes[i].position = tmpPos;
collideObstacle = false;
}
}
}
}
}