static void Main(string[] args)
{
KDTree tree = new KDTree(2);
Random r=new Random();
for(int i=0;i<2000000;i++)
{
double x=r.NextDouble();
double y=r.NextDouble();
double[] d = {x,y};
tree.insert(d,x.ToString()+","+y.ToString());
}
double[] target={0.2,0.3};
int k=4;
Object[] result = tree.nearest(target,k);
foreach (string s in result)
{
Console.WriteLine(s);
}
}
public List<Node> Compute(int startX, int startY, int endX, int endY, int attemps, float stepSize, float playerMaxHp, float playerSpeed, float playerDps, Cell[][][] matrix, bool smooth = false)
{
// Initialization
tree = new KDTree (3);
deathPaths = new List<List<Node>> ();
nodeMatrix = matrix;
//Start and ending node
Node start = GetNode (0, startX, startY);
start.visited = true;
start.parent = null;
start.playerhp = playerMaxHp;
foreach (Enemy e in enemies) {
start.enemyhp.Add (e, e.maxHealth);
}
// Prepare start and end node
Node end = GetNode (0, endX, endY);
tree.insert (start.GetArray (), start);
// Prepare the variables
Node nodeVisiting = null;
Node nodeTheClosestTo = null;
float tan = playerSpeed / 1;
angle = 90f - Mathf.Atan (tan) * Mathf.Rad2Deg;
/*Distribution algorithm
* List<Distribution.Pair> pairs = new List<Distribution.Pair> ();
for (int x = 0; x < matrix[0].Length; x++)
for (int y = 0; y < matrix[0].Length; y++)
if (((Cell)matrix [0] [x] [y]).waypoint)
pairs.Add (new Distribution.Pair (x, y));
pairs.Add (new Distribution.Pair (end.x, end.y));
Distribution rd = new Distribution(matrix[0].Length, pairs.ToArray());*/
DDA dda = new DDA (tileSizeX, tileSizeZ, nodeMatrix [0].Length, nodeMatrix [0] [0].Length);
//RRT algo
for (int i = 0; i <= attemps; i++) {
//Get random point
int rt = Random.Range (1, nodeMatrix.Length);
int rx = Random.Range (0, nodeMatrix [rt].Length);
int ry = Random.Range (0, nodeMatrix [rt] [rx].Length);
//Distribution.Pair p = rd.NextRandom();
//int rx = p.x, ry = p.y;
nodeVisiting = GetNode (rt, rx, ry);
if (nodeVisiting.visited || nodeVisiting.cell.blocked) {
i--;
continue;
}
nodeTheClosestTo = (Node)tree.nearest (new double[] {rx, rt, ry});
// Skip downwards movement
if (nodeTheClosestTo.t > nodeVisiting.t)
continue;
// Skip if player is dead
if (nodeTheClosestTo.playerhp <= 0)
continue;
// Only add if we are going in ANGLE degrees or higher
Vector3 p1 = nodeVisiting.GetVector3 ();
Vector3 p2 = nodeTheClosestTo.GetVector3 ();
Vector3 pd = p1 - p2;
if (Vector3.Angle (pd, new Vector3 (pd.x, 0f, pd.z)) < angle) {
continue;
}
// And we have line of sight
if (nodeVisiting.cell.blocked) {
continue;
}
// Check for all alive enemies
List<Cell[][][]> seenList = new List<Cell[][][]> ();
foreach (Enemy e in enemies) {
if (nodeTheClosestTo.enemyhp [e] > 0)
seenList.Add (e.seenCells);
}
Node hit = dda.Los3D (nodeMatrix, nodeTheClosestTo, nodeVisiting, seenList.ToArray ());
if (hit != null) {
if (hit.cell.blocked || (!simulateCombat && hit.cell.seen && !hit.cell.safe)) // Collision with obstacle, ignore. If we don't simulate combat, ignore collision with enemy
continue;
else {
// Which enemy has seen me?
Enemy toFight = null;
foreach (Enemy e in enemies) {
if (e.seenCells [hit.t] [hit.x] [hit.y] != null && nodeTheClosestTo.enemyhp [e] > 0)
toFight = e;
}
// Solve the time
float timef = nodeTheClosestTo.enemyhp [toFight] / (playerDps * stepSize);
int timeT = Mathf.CeilToInt (timef);
// Search for more enemies
List<object> more = new List<object> ();
foreach (Enemy e2 in enemies) {
if (toFight != e2)
for (int t = hit.t; t < hit.t + timeT; t++)
if (e2.seenCells [t] [hit.x] [hit.y] != null && nodeTheClosestTo.enemyhp [e2] > 0) {
Tuple<Enemy, int> whenSeen = new Tuple<Enemy, int> (e2, t);
more.Add (whenSeen);
break; // Skip this enemy
}
}
// Did another enemy saw the player while he was fighting?
if (more.Count > 0) {
// Who dies when
List<object> dyingAt = new List<object> ();
// First, save when the first fight starts
Node firstFight = NewNode (hit.t, hit.x, hit.y);
firstFight.parent = nodeTheClosestTo;
// Then when the first guy dies
Tuple<Enemy, int> death = new Tuple<Enemy, int> (toFight, firstFight.t + timeT);
dyingAt.Add (death);
// And proccess the other stuff
copy (nodeTheClosestTo, firstFight);
firstFight.fighting.Add (toFight);
// Navigation node
Node lastNode = firstFight;
// Solve for all enemies joining the fight
foreach (object o in more) {
Tuple<Enemy, int> joined = (Tuple<Enemy, int>)o;
// Calculate dying time
timef = timef + lastNode.enemyhp [joined.First] / (playerDps * stepSize);
timeT = Mathf.CeilToInt (timef);
death = new Tuple<Enemy, int> (joined.First, timeT + hit.t);
dyingAt.Add (death);
// Create the node structure
Node startingFight = NewNode (joined.Second, hit.x, hit.y);
// Add to fighting list
copy (lastNode, startingFight);
startingFight.fighting.Add (joined.First);
// Correct parenting
startingFight.parent = lastNode;
lastNode = startingFight;
}
// Solve for all deaths
foreach (object o in dyingAt) {
Tuple<Enemy, int> dead = (Tuple<Enemy, int>)o;
Node travel = lastNode;
bool didDie = false;
while (!didDie && travel.parent != null) {
// Does this guy dies between two nodes?
if (dead.Second > travel.parent.t && dead.Second < travel.t) {
// Add the node
Node adding = NewNode (dead.Second + hit.t, hit.x, hit.y);
adding.fighting = new List<Enemy> ();
adding.fighting.AddRange (travel.parent.fighting);
// And remove the dead people
adding.fighting.Remove (dead.First);
adding.died = dead.First;
Node remove = lastNode;
// Including from nodes deeper in the tree
while (remove != travel.parent) {
remove.fighting.Remove (dead.First);
remove = remove.parent;
}
// Reparent the nodes
adding.parent = travel.parent;
travel.parent = adding;
didDie = true;
}
travel = travel.parent;
}
if (!didDie) {
// The guy didn't die between, that means he's farthest away than lastNode
Node adding = NewNode (dead.Second, hit.x, hit.y);
copy (lastNode, adding);
adding.fighting.Remove (dead.First);
adding.enemyhp [dead.First] = 0;
adding.died = dead.First;
adding.parent = lastNode;
// This is the new lastNode
lastNode = adding;
}
}
// Grab the first node with fighting
Node first = lastNode;
while (first.parent != nodeTheClosestTo)
first = first.parent;
while (first != lastNode) {
Node navigate = lastNode;
// And grab the node just after the first
while (navigate.parent != first)
navigate = navigate.parent;
// Copy the damage already applied
navigate.playerhp = first.playerhp;
// And deal more damage
foreach (Enemy dmgDealer in first.fighting)
navigate.playerhp -= (navigate.t - first.t) * dmgDealer.dps * stepSize;
// Goto next node
first = navigate;
}
// Make the tree structure
nodeVisiting = lastNode;
} else {
// Only one enemy has seen me
Node toAdd = NewNode (hit.t, hit.x, hit.y);
nodeVisiting = NewNode (hit.t + timeT, hit.x, hit.y);
nodeVisiting.parent = toAdd;
toAdd.parent = nodeTheClosestTo;
copy (nodeTheClosestTo, toAdd);
toAdd.fighting.Add (toFight);
copy (nodeTheClosestTo, nodeVisiting);
nodeVisiting.playerhp = toAdd.playerhp - timef * toFight.dps * stepSize;
nodeVisiting.enemyhp [toFight] = 0;
nodeVisiting.died = toFight;
}
}
} else {
// Nobody has seen me
nodeVisiting.parent = nodeTheClosestTo;
copy (nodeTheClosestTo, nodeVisiting);
}
try {
tree.insert (nodeVisiting.GetArray (), nodeVisiting);
} catch (KeyDuplicateException) {
}
nodeVisiting.visited = true;
// Add the path to the death paths list
if (nodeVisiting.playerhp <= 0) {
Node playerDeath = nodeVisiting;
while (playerDeath.parent.playerhp <= 0)
playerDeath = playerDeath.parent;
deathPaths.Add (ReturnPath (playerDeath, smooth));
}
// Attemp to connect to the end node
if (nodeVisiting.playerhp > 0) {
// Compute minimum time to reach the end node
p1 = nodeVisiting.GetVector3 ();
p2 = end.GetVector3 ();
p2.y = p1.y;
float dist = Vector3.Distance (p1, p2);
float t = dist * Mathf.Tan (angle);
pd = p2;
pd.y += t;
if (pd.y <= nodeMatrix.GetLength (0)) {
Node endNode = GetNode ((int)pd.y, (int)pd.x, (int)pd.z);
// Try connecting
seenList = new List<Cell[][][]> ();
foreach (Enemy e in enemies) {
if (nodeTheClosestTo.enemyhp [e] > 0)
seenList.Add (e.seenCells);
}
hit = dda.Los3D (nodeMatrix, nodeVisiting, endNode, seenList.ToArray ());
// To simplify things, only connect if player isn't seen or collides with an obstacle
if (hit == null) {
endNode.parent = nodeVisiting;
copy (endNode.parent, endNode);
List<Node> done = ReturnPath (endNode, smooth);
//UpdateNodeMatrix (done);
return done;
}
}
if (nodeVisiting.playerhp < playerMaxHp)
// Health pack solving
foreach (HealthPack pack in packs) {
if (!nodeVisiting.picked.Contains(pack)) {
// Compute minimum time to reach the pack
p1 = nodeVisiting.GetVector3 ();
p2 = new Vector3(pack.posX, p1.y, pack.posZ);
dist = Vector3.Distance (p1, p2);
t = dist * Mathf.Tan (angle);
pd = p2;
pd.y += t;
if (pd.y <= nodeMatrix.GetLength (0)) {
// TODO If the node is already on the Tree, things may break!
// but we need to add it to the tree and retrieve from it to make it a possible path!
Node packNode = GetNode ((int)pd.y, (int)pd.x, (int)pd.z);
// Try connecting
seenList = new List<Cell[][][]> ();
foreach (Enemy e in enemies) {
if (nodeVisiting.enemyhp [e] > 0)
seenList.Add (e.seenCells);
}
hit = dda.Los3D (nodeMatrix, nodeVisiting, packNode, seenList.ToArray ());
// To simplify things, only connect if player isn't seen or collides with an obstacle
if (hit == null) {
packNode.parent = nodeVisiting;
copy (packNode.parent, packNode);
packNode.picked.Add(pack);
packNode.playerhp = playerMaxHp;
try {
tree.insert(packNode.GetArray(), packNode);
} catch (KeyDuplicateException) {
}
}
}
}
}
}
//Might be adding the neighboor as a the goal
if (nodeVisiting.x == end.x & nodeVisiting.y == end.y) {
//Debug.Log ("Done2");
List<Node> done = ReturnPath (nodeVisiting, smooth);
//UpdateNodeMatrix (done);
return done;
}
}
return new List<Node> ();
}
public List<Node> Compute(int startX, int startY, int endX, int endY, int attemps, float speed, Cell[][][] matrix, bool smooth = false)
{
// Initialization
tree = new KDTree (3);
explored = new List<Node> ();
nodeMatrix = matrix;
//Start and ending node
Node start = GetNode (0, startX, startY);
start.visited = true;
start.parent = null;
// Prepare start and end node
Node end = GetNode (0, endX, endY);
tree.insert (start.GetArray (), start);
explored.Add (start);
// Prepare the variables
Node nodeVisiting = null;
Node nodeTheClosestTo = null;
float tan = speed / 1;
angle = 90f - Mathf.Atan (tan) * Mathf.Rad2Deg;
List<Distribution.Pair> pairs = new List<Distribution.Pair> ();
for (int x = 0; x < matrix[0].Length; x++)
for (int y = 0; y < matrix[0].Length; y++)
if (((Cell)matrix [0] [x] [y]).waypoint)
pairs.Add (new Distribution.Pair (x, y));
pairs.Add (new Distribution.Pair (end.x, end.y));
//Distribution rd = new Distribution(matrix[0].Length, pairs.ToArray());
//RRT algo
for (int i = 0; i <= attemps; i++) {
//Get random point
int rt = Random.Range (1, nodeMatrix.Length);
//Distribution.Pair p = rd.NextRandom();
int rx = Random.Range (0, nodeMatrix [rt].Length);
int ry = Random.Range (0, nodeMatrix [rt] [rx].Length);
//int rx = p.x, ry = p.y;
nodeVisiting = GetNode (rt, rx, ry);
if (nodeVisiting.visited || nodeVisiting.cell.blocked) {
i--;
continue;
}
explored.Add (nodeVisiting);
nodeTheClosestTo = (Node)tree.nearest (new double[] {rx, rt, ry});
// Skip downwards movement
if (nodeTheClosestTo.t > nodeVisiting.t)
continue;
// Only add if we are going in ANGLE degrees or higher
Vector3 p1 = nodeVisiting.GetVector3 ();
Vector3 p2 = nodeTheClosestTo.GetVector3 ();
Vector3 pd = p1 - p2;
if (Vector3.Angle (pd, new Vector3 (pd.x, 0f, pd.z)) < angle) {
continue;
}
// And we have line of sight
if ((nodeVisiting.cell.seen && !nodeVisiting.cell.safe) || Extra.Collision.CheckCollision (nodeVisiting, nodeTheClosestTo, this, SpaceState.Editor, true))
continue;
try {
tree.insert (nodeVisiting.GetArray (), nodeVisiting);
} catch (KeyDuplicateException) {
}
nodeVisiting.parent = nodeTheClosestTo;
nodeVisiting.visited = true;
// Attemp to connect to the end node
if (Random.Range (0, 1000) > 0) {
p1 = nodeVisiting.GetVector3 ();
p2 = end.GetVector3 ();
p2.y = p1.y;
float dist = Vector3.Distance (p1, p2);
float t = dist * Mathf.Tan (angle);
pd = p2;
pd.y += t;
if (pd.y <= nodeMatrix.GetLength (0)) {
Node endNode = GetNode ((int)pd.y, (int)pd.x, (int)pd.z);
if (!Extra.Collision.CheckCollision (nodeVisiting, endNode, this, SpaceState.Editor, true)) {
//Debug.Log ("Done3");
endNode.parent = nodeVisiting;
return ReturnPath (endNode, smooth);
}
}
}
//Might be adding the neighboor as a the goal
if (nodeVisiting.x == end.x & nodeVisiting.y == end.y) {
//Debug.Log ("Done2");
return ReturnPath (nodeVisiting, smooth);
}
}
return new List<Node> ();
}
public bool FindPath(ref RRTNode originalRoot, Vector2 goalPoint, List<Polygon> obstacles, out RRTNode goal)
{
RRTNode root = new RRTNode(originalRoot.State);
double distance = goalPoint.DistanceTo(root.State.Pose.ToVector2());
randomSampleRadius = distance + 15.0;
timeStep = rand.NextDouble();
//vSigma = rand.NextDouble() * 5.0;
numSlice = (int)Math.Round(timeStep / 0.2);
//if (distance < 2)
// timeStep = 0.3;
//else if (distance > 4)
// timeStep = 1.0;
//else
// timeStep = 0.5;
//numSlice = (int)Math.Round(timeStep / 0.1);
Stopwatch randomGenerationTimer = new Stopwatch();
Stopwatch extendingTimer = new Stopwatch();
Stopwatch closestSearchTimer = new Stopwatch();
List<Double> randomTime = new List<double>();
List<Double> extendingTime = new List<double>();
List<Double> closestSearchTime = new List<double>();
bool foundPath = false;
goal = null; //not found yet!
//RRT is divided into the following steps:
//0) assume the root node is the first node
//1) randomly select a sample point in space centered around our robot within some fixed distance. Every 20th can be the goal.
//2) select the closest node to the sampled point in the existing tree based on xy distance
//3) generate a control input that drives towards the sample point also biased with our initial control inputs
//3a) -Biasing Details:
// Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity
// Select Turn Rate:
// Apply the following heuristic: mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT)
// sigma = SigmaTurnRate
//4) Divide the total RRT timestep into smaller sub-steps
//4a) calculate the trajectory at one substep given the control inputs and closest node initial conditions
//4b) check at each the linear path between the initial and simulation end does not intersect a polygon
//4c) if intersects, terminate and go to 1.
//4d) if not intersects
//4da) if last substep, add the results of this simulation to the closest node as a child
//4db) else simulate the next subtime step by going to 4a
//5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1.
//----------------------------------------------------------------------------------------------------------------------------------//
// Declare variables
int sampleCount = 0; // counter for sample to be biased every 20th time
int iterationCount = 0; // counter for termination
kdTree = new KDTree(2);
kdTree.insert(RRTNode.ToKey(root.State.Pose.x, root.State.Pose.y), root);
// 0) assume root note is the first node
while (!foundPath)
//for (int i = 0; i < 1000; i++)
{
//--- Termination ---//
iterationCount++;
if (iterationCount > terminationCount)
{
Console.WriteLine("//-----------------------------------------------------------------------//");
Console.WriteLine("Random generation average time: " + (randomTime.Sum() / randomTime.Count) + " ms | total time: " + randomTime.Sum() + " | iteration: " + randomTime.Count);
Console.WriteLine("Searching closest node average time: " + (closestSearchTime.Sum() / closestSearchTime.Count) + " ms | total time: " + closestSearchTime.Sum() + " | iteration: " + closestSearchTime.Count);
Console.WriteLine("Extending average time: " + (extendingTime.Sum() / extendingTime.Count) + " ms | total time: " + extendingTime.Sum() + " | iteration: " + extendingTime.Count);
// Benchmark output
Console.WriteLine("|--Simulation average time: " + (simulationTime.Sum() / simulationTime.Count) + " ms | total time: " + simulationTime.Sum() + " | iteration: " + simulationTime.Count);
Console.WriteLine("|--Obstacle checking average time: " + (obstacleTime.Sum() / obstacleTime.Count) + " ms | total time: " + obstacleTime.Sum() + " | iteration: " + obstacleTime.Count);
Console.WriteLine("//-----------------------------------------------------------------------//");
simulationTime.Clear();
obstacleTime.Clear();
return false;
}
//-------------------//
// 1) randomly select a sample point in space centered around our robot within some fixed distance.
int actualNumNodesToExtend = numNodesToExtend;
Vector2 samplePoint;
if (sampleCount < goalPointSamplingRate)
{
if (rand.NextDouble() > chanceToSampleRoot)
{
randomGenerationTimer.Start();
//double randomX = root.State.Pose.x + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
//double randomY = root.State.Pose.y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
double randomX = goalPoint.X + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
double randomY = goalPoint.Y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
samplePoint = new Vector2(randomX, randomY);
randomTime.Add(randomGenerationTimer.ElapsedMilliseconds);
randomGenerationTimer.Reset();
}
else
{
samplePoint = root.Point;
actualNumNodesToExtend = 1;
}
sampleCount++;
}
else
{
samplePoint = goalPoint;
sampleCount = 0;
}
closestSearchTimer.Start();
// 2) select the closest node to the sampled point in the existing tree based on xy distance
//List<RRTNode> closestNodes = root.FindNClosestNodeInTree(samplePoint, actualNumNodesToExtend);
List<RRTNode> closestNodes = root.FindNClosestNodeInTreeKDTREE(3, samplePoint, kdTree);
closestSearchTime.Add(closestSearchTimer.ElapsedMilliseconds);
closestSearchTimer.Reset();
extendingTimer.Start();
foreach (RRTNode closeNode in closestNodes)
{
RRTNode newNode = ExtendNode(ref goalPoint, obstacles, ref goal, ref foundPath, ref samplePoint, closeNode, rand);
if (newNode != null) kdTree.insert(RRTNode.ToKey(newNode.State.Pose.x, newNode.State.Pose.y), newNode);
}
extendingTime.Add(extendingTimer.ElapsedMilliseconds);
extendingTimer.Reset();
}
Console.WriteLine("//-----------------------------------------------------------------------//");
Console.WriteLine("Random generation average time: " + (randomTime.Sum() / randomTime.Count) + " ms | total time: " + randomTime.Sum() + " | iteration: " + randomTime.Count);
Console.WriteLine("Searching closest node average time: " + (closestSearchTime.Sum() / closestSearchTime.Count) + " ms | total time: " + closestSearchTime.Sum() + " | iteration: " + closestSearchTime.Count);
Console.WriteLine("Extending average time: " + (extendingTime.Sum() / extendingTime.Count) + " ms | total time: " + extendingTime.Sum() + " | iteration: " + extendingTime.Count);
// Benchmark output
Console.WriteLine("|--Simulation average time: " + (simulationTime.Sum() / simulationTime.Count) + " ms | total time: " + simulationTime.Sum() + " | iteration: " + simulationTime.Count);
Console.WriteLine("|--Obstacle checking average time: " + (obstacleTime.Sum() / obstacleTime.Count) + " ms | total time: " + obstacleTime.Sum() + " | iteration: " + obstacleTime.Count);
Console.WriteLine("//-----------------------------------------------------------------------//");
simulationTime.Clear();
obstacleTime.Clear();
return true;
}
// Rebuild a KDTree by taking every item and inserting it into a new
// tree. Takes a reference so it can try to garbage collect the old
// tree before the new one is built.
static void rebuildKDTree(ref KDTree kd)
{
Object[] objs = allInKDTree(ref kd);
objs.Shuffle();
Console.Out.WriteLine("Rebuilding KD tree with " + objs.Length + " items");
kd = new KDTree();
GC.Collect();
for (int i = 0; i < objs.Length; ++i) {
kd.insert(((ColorLocation)objs[i]).Location, objs[i]);
}
}
// Build a KDTree of all the possible colors, indexed by location in the
// chosen color space. When fewer than 8 bits per pixel are used, the
// low order bits are skipped within each channel.
static KDTree buildKDTreeOfColors(int bitsPerChannel, ColorSpace cs)
{
int colorsPerChannel = 1 << bitsPerChannel;
int shift = 8 - bitsPerChannel;
ColorLocation[] colors = new ColorLocation[colorsPerChannel * colorsPerChannel * colorsPerChannel];
int colorIndex = 0;
for (int r = 0; r < colorsPerChannel; ++r) {
for (int g = 0; g < colorsPerChannel; ++g) {
for (int b = 0; b < colorsPerChannel; ++b) {
ColorLocation c = new ColorLocation((byte)(r << shift),
(byte)(g << shift),
(byte)(b << shift),
cs);
colors[colorIndex] = c;
colorIndex++;
}
}
}
colors.Shuffle();
KDTree kd = new KDTree();
for (int i = 0; i < colors.Length; ++i) {
ColorLocation c = colors[i];
kd.insert(c.Location, c);
}
colors = null;
return kd;
}