public List <IAgent> QueryRadius(Rectangle queryArea, float radius)
{
List <IAgent> results = new List <IAgent>();
//Fame.Fame.Singleton.UnityPrint(String.Format("{0} {1}", thisIndex,contents.Count));
//Fame.Fame.Singleton.UnityPrint(String.Format("Checking:{0} ", thisIndex));
//List<IAgent> cloneContents;
//lock (thisLock)
//{
// cloneContents = new List<IAgent>(contents);
//}
FVec2 rectCenter = queryArea.Center;
float radiusSq = radius * radius;
foreach (IAgent item in this.contents)
{
FVec2 agentPos = new FVec2(rectCenter.x - item.Position.x, rectCenter.z - item.Position.z);
if (agentPos.SquaredLength < radiusSq)
{
results.Add(item);
}
}
foreach (QuadTreeNode node in subNodes)
{
//if(node.IsEmpty){
// Fame.Fame.Singleton.UnityPrint(String.Format("skipping:{0} ", node.thisIndex));
// continue;
//}
//case 1: search area completely contained by sub-quad
//if a node completely contains the query area, go down that branch
//and skip the remaining node (break this loop)
if (node.Bounds.Contains(queryArea))
{
results.AddRange(node.QueryRadius(queryArea, radius));
break;
}
//case 2: sub-quad completely contained by search area
//if the query area completely contains a sub-quad,
//just add all the contents of that quad and its children
//to the result set. You need to continue the loop to test
//the other quads
if (queryArea.Contains(node.Bounds))
{
results.AddRange(node.SubTreeContents);
continue;
}
//case 3: search area intersects with sub-quad
//traverse into this quad, continue the loop to
//search other quads
if (node.Bounds.IntersectsWith(queryArea))
{
results.AddRange(node.QueryRadius(queryArea, radius));
}
}
return(results);
}
public float GetDistanceSqTo(FVec2 pt)
{
float dx = pt.x - mX;
float dz = pt.z - mZ;
return(dx * dx + dz * dz);
}
private double GetDistance(int v, int w)
{
FVec2 vv = GetPosition(v);
FVec2 vw = GetPosition(w);
return(vv.GetDistanceTo(vw));
}
public void MoveAbs(FVec2 center)
{
x1 = center.x - Width / 2;
x2 = center.x + Width / 2;
y1 = center.z - Height / 2;
y2 = center.z + Height / 2;
}
public float GetDistanceTo(FVec2 pt)
{
float dx = pt.x - mX;
float dz = pt.z - mZ;
return((float)System.Math.Sqrt(dx * dx + dz * dz));
}
public static FVec2 operator +(FVec2 lhs, FVec2 rhs)
{
FVec2 result = lhs.Clone();
result.x += rhs.x;
result.z += rhs.z;
return(result);
}
public static FVec2 operator *(float scale, FVec2 rhs)
{
FVec2 result = rhs.Clone();
result.x *= scale;
result.z *= scale;
return(result);
}
public static FVec2 operator *(FVec2 lhs, float scale)
{
FVec2 result = lhs.Clone();
result.x *= scale;
result.z *= scale;
return(result);
}
public static FVec2 operator -(FVec2 lhs)
{
FVec2 result = lhs.Clone();
result.x *= -1;
result.z *= -1;
return(result);
}
public FVec2 Assign(FVec2 rhs)
{
if (this == rhs)
{
return(this);
}
x = rhs.x;
z = rhs.z;
return(this);
}
public FVec2 Rotate(float radian)
{
double cosine = System.Math.Cos(radian);
double sine = System.Math.Sin(radian);
FVec2 result = new FVec2();
result.x = (float)(x * cosine - z * sine);
result.z = (float)(x * sine + z * cosine);
return(result);
}
public static int SidenessTest(FVec2 A, FVec2 B, FVec2 C)
{
float result = (B.x - A.x) * (C.z - A.z) - (B.z - A.z) * (C.x - A.x);
if (result == 0)
{
return(0);
}
else if (result > 0)
{
return(1);
}
else
{
return(-1);
}
}
static void Main(string[] args)
{
Random random = new Random();
QuadTree space = new QuadTree(new Rectangle(0, 0, 2000, 2000));
GridWorld pathFinder = new GridWorld(space);
FVec2 target = new FVec2(1000, 1000);
List <IAgent> population = new List <IAgent>();
for (int i = 0; i < 10; ++i)
{
IAgent agent = new SimpleAgent(space);
float y = 0; // vertical position
float x = random.Next(2000);
float z = random.Next(2000);
agent.Position = new FVec3(x, y, z);
agent.AgentID = i + 1;
population.Add(agent);
}
for (int steps = 0; steps < 20; ++steps)
{
Dijstra paths = pathFinder.dijstra(target);
foreach (IAgent agent in population)
{
float y = 0; // vertical position
float x = random.Next(2000);
float z = random.Next(2000);
agent.Position = new FVec3(x, y, z);
List <FVec3> path = paths.GetPath(agent);
Console.WriteLine("Path for agent #{0} in step {1}: {2} navigation points",
agent.AgentID, steps + 1, path.Count);
}
target = new FVec2(random.Next(2000), random.Next(2000));
}
}
public int GetVertex(FVec2 position)
{
int x = (int)(position.x / xInterval);
int z = (int)(position.z / zInterval);
if (x < 0)
{
x = 0;
}
if (x >= colCount)
{
x = colCount - 1;
}
if (z < 0)
{
z = 0;
}
if (z >= RowCount)
{
z = rowCount - 1;
}
return(z * colCount + x);
}
public Dijstra dijstra(FVec2 target)
{
int s = GetVertex(target);
return(new Dijstra(this, s));
}
public void Assign(FVec2 vect2)
{
this.x = vect2.x;
this.z = vect2.z;
}
public float CrossLength(FVec2 v)
{
return(Math.Abs(x * v.z - z * v.x));
}
public static float dotProduct(FVec2 lhs, FVec2 rhs)
{
return(lhs.x * rhs.x + lhs.z * rhs.z);
}
public float dotProduct(FVec2 rhs)
{
return(mX * rhs.x + mZ * rhs.z);
}
