public void Insert(double distance, double angle, double accuracy)
{
Point p = CurrentPosition(distance, angle);
root = insert(p, root, 0, accuracy);
count++;
}
private BDNode insert(Point coord, Object value, BDNode tree, int level, double accuracy, ref BDNode rn)
{
// Create a new node
if (tree == null)
{
BDNode n = new BDNode(coord, value);
tree = n;
rn = n;
}
// the node already exists
else if (coord.Distance(tree.Coord) <= accuracy)
{
rn = tree;
}
// insert on the right
else if ((level % 2 == 0 && coord.X > tree.Coord.X) || (level % 2 == 1 && coord.Y > tree.Coord.Y))
{
tree.Right = insert(coord, value, tree.Right, level + 1, accuracy, ref rn);
}
// insert on the left
else
{
tree.Left = insert(coord, value, tree.Left, level + 1, accuracy, ref rn);
}
return(tree);
}
private void canvas1_PreviewMouseLeftButtonDown(object sender, MouseButtonEventArgs e)
{
if (dirtPen)
{
Point p = e.GetPosition(grid1);
BDNode n = Environment.Instance.Dirt.Insert(p, new DirtElement(), 0);
DirtElement de = (DirtElement)n.Value;
if (de.count == 0)
{
Rectangle r = new Rectangle();
r.Stroke = Brushes.Green;
r.Fill = Brushes.Green;
r.Width = 3;
r.Height = 3;
r.HorizontalAlignment = HorizontalAlignment.Left;
r.VerticalAlignment = VerticalAlignment.Top;
r.Margin = new Thickness(p.X, p.Y, 0, 0);
de.element = r;
grid1.Children.Add(r);
}
de.count++;
}
else if (selectedTool != null)
{
selectedTool.Start = e.GetPosition(grid1);
}
grid1.CaptureMouse();
}
public BDNode Insert(Point coord, Object value, double accuracy)
{
BDNode n = null;
root = insert(coord, value, root, 0, accuracy, ref n);
count++;
return(n);
}
private void OnTimedEvent2(object source, ElapsedEventArgs e)
{
DispatcherOperation dispatcherOp = grid2.Dispatcher.BeginInvoke(DispatcherPriority.Normal,
new Action(delegate()
{
grid2.Children.Clear();
List <BDNode> nodes = agent.Nodes;
Line line;
label1.Content = String.Format("Amount of reference points {0}", nodes.Count);
line = new Line();
line.Stroke = Brushes.Red;
line.StrokeThickness = 5;
BDNode n = agent.Nearest;
if (n != null)
{
line.X1 = n.Coord.X + offset.X;
line.Y1 = n.Coord.Y + offset.Y;
line.X2 = n.Coord.X + 2 + offset.X;
line.Y2 = n.Coord.Y + 2 + offset.Y;
grid2.Children.Add(line);
}
line = new Line();
line.Stroke = Brushes.Green;
line.StrokeThickness = 5;
Point currentPoint = agent.CurrentPosition;
if (n != null)
{
line.X1 = currentPoint.X + offset.X;
line.Y1 = currentPoint.Y + offset.Y;
line.X2 = currentPoint.X + 2 + offset.X;
line.Y2 = currentPoint.Y + 2 + offset.Y;
grid2.Children.Add(line);
}
foreach (BDNode node in nodes)
{
if (node.Next != null)
{
line = new Line();
line.Stroke = Brushes.Black;
line.X1 = node.Coord.X + offset.X;
line.Y1 = node.Coord.Y + offset.Y;
line.X2 = node.Next.Coord.X + offset.X;
line.Y2 = node.Next.Coord.Y + offset.Y;
grid2.Children.Add(line);
}
}
}
));
}
private void inorder(BDNode node, List <BDNode> list)
{
if (node.Left != null)
{
inorder(node.Left, list);
}
list.Add(node);
if (node.Right != null)
{
inorder(node.Right, list);
}
}
public BDNode[] Nearest(Point p, int n)
{
BDNode[] nbrs = new BDNode[n];
NearestNeighborList nnl = new NearestNeighborList(n);
Rect rect = Rect.inifiniteRect();
double max_dist_sqd = Double.MaxValue;
Point keyp = new Point(p);
nearest(root, p, rect, max_dist_sqd, 0, nnl);
for (int i = 0; i < n; ++i)
{
BDNode kd = (BDNode)nnl.removeHighest();
nbrs[n - i - 1] = kd;
}
return(nbrs);
}
private void canvas1_PreviewMouseMove(object sender, MouseEventArgs e)
{
Point p = e.GetPosition(grid1);
if (dirtPen && Mouse.LeftButton == MouseButtonState.Pressed)
{
BDNode n = Environment.Instance.Dirt.Insert(p, new DirtElement(), 0);
DirtElement de = (DirtElement)n.Value;
if (de.count == 0)
{
Rectangle r = new Rectangle();
r.Stroke = Brushes.Green;
r.Fill = Brushes.Green;
r.Width = 3;
r.Height = 3;
r.HorizontalAlignment = HorizontalAlignment.Left;
r.VerticalAlignment = VerticalAlignment.Top;
r.Margin = new Thickness(p.X, p.Y, 0, 0);
de.element = r;
grid1.Children.Add(r);
}
de.count++;
}
else if (selectedTool != null && selectedTool.Start != null)
{
selectedTool.update(p, Keyboard.IsKeyDown(Key.LeftShift) || Keyboard.IsKeyDown(Key.RightShift),
Keyboard.IsKeyDown(Key.LeftCtrl) || Keyboard.IsKeyDown(Key.RightCtrl));
// Remove old snapshot
foreach (UIElement elem in snapshot)
{
grid1.Children.Remove(elem);
}
// Add new
foreach (Segment s in selectedTool.Segments)
{
snapshot.Add(drawSegment(s, Brushes.Black));
}
}
}
private static void rsearch(Point lowk, Point uppk, BDNode tree, int level, List <BDNode> v)
{
// if the tree is null stop the recursivity
if (tree == null)
{
return;
}
// if the current working dimension is smaller or equal to the current node value
// recursive range search on the left sub tree
if ((level % 2 == 0 && lowk.X <= tree.Coord.X) || (level % 2 == 1 && lowk.Y <= tree.Coord.Y))
{
rsearch(lowk, uppk, tree.Left, level + 1, v);
}
// if the keys does not lies in the node range
int j = 0;
if (lowk.X <= tree.Coord.X && uppk.X >= tree.Coord.X)
{
j++;
if (lowk.Y <= tree.Coord.Y && uppk.Y >= tree.Coord.Y)
{
j++;
}
}
// if the node lies in the boundary values
if (j == 2)
{
v.Add(tree);
}
// if the current working dimension node is less than the upper key boundary
// recursive range search on the right sub tree
if ((level % 2 == 0 && uppk.X > tree.Coord.X) || (level % 2 == 1 && uppk.Y > tree.Coord.Y))
{
rsearch(lowk, uppk, tree.Right, level + 1, v);
}
}
private BDNode insert(Point coord, BDNode tree, int level, double accuracy)
{
// Create a new node
if (tree == null)
{
BDNode n = new BDNode(coord);
tree = n;
if (previous != null)
{
previous.Next = n;
}
n.Previous = previous;
previous = n;
}
// the node already exists
else if (coord.Distance(tree.Coord) <= accuracy)
{
if (tree.Previous != null && previous.Coord.Distance(tree.Coord) > tree.Previous.Coord.Distance(tree.Coord))
{
tree.Previous.Next = null;
tree.Previous = previous;
previous.Next = tree;
}
previous = tree;
}
// insert on the right
else if ((level % 2 == 0 && coord.X > tree.Coord.X) || (level % 2 == 1 && coord.Y > tree.Coord.Y))
{
tree.Right = insert(coord, tree.Right, level + 1, accuracy);
}
// insert on the left
else
{
tree.Left = insert(coord, tree.Left, level + 1, accuracy);
}
return(tree);
}
public void Path(double distance, double angle, Point target)
{
BDNode near = Nearest(CurrentPosition(distance, angle), 1)[0];
}
public void run()
{
double prevReading = 0;
double newReading = 0;
// Wall following
while (!gohome)
{
// Go to the wall
while (!longRangeSensor.againstWall())
{
movement.straight(1);
distance += 1;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
// While the agent is touching a wall
while (longRangeSensor.againstWall())
{
movement.spinLeft(1, 1);
angle = (angle - 1) % 360;
System.Threading.Thread.Sleep(5);
}
// start following the wall adjusting the angle
prevReading = ShortRangeSensor.getDistance();
while (!longRangeSensor.againstWall())
{
movement.straight(1);
distance += 1;
newReading = shortRangeSensor.getDistance();
double alpha;
if (prevReading == -1)
{
alpha = 0;
}
else if (newReading == -1)
{
alpha = 90;
}
else
{
alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
if (Math.Abs(alpha) < 5)
{
alpha = 0;
}
}
if (alpha != 0)
{
//no more wall
nodes.Insert(distance, angle, 5);
distance = 0;
}
movement.spinRight(1, alpha);
angle = (angle + alpha) % 360;
prevReading = newReading;
System.Threading.Thread.Sleep(5);
}
}
nodes.Insert(distance, angle, 5);
distance = 0;
while (nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
{
// Go home
// point in direction to the start
Point A = new Point(0, 0);
Point C = nodes.CurrentPosition(distance, angle);
Point Btemp = C + new Point(0, 100);
Point B = new Segment(C, Btemp).Rotate(angle).End;
double a_sq = Math.Pow(100, 2);
double b_sq = A.SquareDistance(C);
double c_sq = A.SquareDistance(B);
double gamma;
gamma = Math.Acos((c_sq - a_sq - b_sq) / (-2 * Math.Sqrt(a_sq) * Math.Sqrt(b_sq))) * 180 / Math.PI;
if (b_sq > c_sq)
{
gamma = 90 - gamma;
}
if (angle < 0)
{
angle = (360 + angle);
}
if (angle < 180)
{
movement.spinRight(1, gamma);
angle = (angle + gamma) % 360;
}
else
{
movement.spinLeft(1, gamma);
angle = (angle - gamma) % 360;
}
// no wall let's move straight
while (longRangeSensor.getDistance() == -1)
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
// A wall is in range
Point pos = nodes.CurrentPosition(distance, angle);
Segment sensor = new Segment(pos, new Point(longRangeSensor.getDistance(), 0)).Rotate(angle);
BDNode near = nodes.Nearest(sensor.End, 1)[0];
if (near.Next != null && new Segment(near.Coord, near.Next.Coord).Intersects(sensor) != null)
{
;//MessageBox.Show("intersect");
}
else if (near.Previous != null && new Segment(near.Coord, near.Previous.Coord).Intersects(sensor) != null)
{
;//MessageBox.Show("intersect");
}
else
{
;// MessageBox.Show("unknown wall");
}
// move straight until hit the wall
while (!longRangeSensor.againstWall() && nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
// While the agent is touching a wall
while (longRangeSensor.inRange())
{
movement.spinLeft(1, 1);
angle = (angle - 1) % 360;
System.Threading.Thread.Sleep(5);
}
while (shortRangeSensor.inRange())
{
movement.straight(1);
distance++;
System.Threading.Thread.Sleep(5);
}
nodes.Insert(distance, angle, 5);
distance = 0;
}
}
public BDNode[] Nearest(Point p, int n)
{
BDNode[] nbrs = new BDNode[n];
NearestNeighborList nnl = new NearestNeighborList(n);
Rect rect = Rect.inifiniteRect();
double max_dist_sqd = Double.MaxValue;
Point keyp = new Point(p);
nearest(root, p, rect, max_dist_sqd, 0, nnl);
for (int i = 0; i < n; ++i)
{
BDNode kd = (BDNode)nnl.removeHighest();
nbrs[n - i - 1] = kd;
}
return nbrs;
}
private BDNode insert(Point coord, BDNode tree, int level, double accuracy)
{
// Create a new node
if (tree == null)
{
BDNode n = new BDNode(coord);
tree = n;
if (previous != null)
previous.Next = n;
n.Previous = previous;
previous = n;
}
// the node already exists
else if (coord.Distance(tree.Coord) <= accuracy)
{
if (tree.Previous != null && previous.Coord.Distance(tree.Coord) > tree.Previous.Coord.Distance(tree.Coord))
{
tree.Previous.Next = null;
tree.Previous = previous;
previous.Next = tree;
}
previous = tree;
}
// insert on the right
else if ((level % 2 == 0 && coord.X > tree.Coord.X) || (level % 2 == 1 && coord.Y > tree.Coord.Y))
{
tree.Right = insert(coord, tree.Right, level + 1, accuracy);
}
// insert on the left
else
{
tree.Left = insert(coord, tree.Left, level + 1, accuracy);
}
return tree;
}
private void inorder(BDNode node, List<BDNode> list)
{
if (node.Left != null)
inorder(node.Left, list);
list.Add(node);
if (node.Right != null)
inorder(node.Right, list);
}
private static void nearest(BDNode tree, Point target, Rect rect, double max_dist_sqd, int level, NearestNeighborList nnl)
{
if (tree == null)
return;
Point pivot = tree.Coord;
double pivot_to_target = pivot.SquareDistance(target);
Rect left_rect = rect;
Rect right_rect = new Rect(rect);
bool target_in_left;
if (level % 2 == 0)
{
left_rect.max.X = pivot.X;
right_rect.min.X = pivot.X;
target_in_left = target.X < pivot.X;
}
else
{
left_rect.max.Y = pivot.Y;
right_rect.min.Y = pivot.Y;
target_in_left = target.Y < pivot.Y;
}
BDNode nearer_node, further_node;
Rect nearer_rect, further_rect;
if (target_in_left)
{
nearer_node = tree.Left;
nearer_rect = left_rect;
further_node = tree.Right;
further_rect = right_rect;
}
else
{
nearer_node = tree.Right;
nearer_rect = right_rect;
further_node = tree.Left;
further_rect = left_rect;
}
nearest(nearer_node, target, nearer_rect, max_dist_sqd, level + 1, nnl);
BDNode nearest_node = (BDNode)nnl.getHighest();
double dist_sqd;
if (!nnl.isCapacityReached())
dist_sqd = Double.MaxValue;
else
dist_sqd = nnl.getMaxPriority();
max_dist_sqd = Math.Min(max_dist_sqd, dist_sqd);
Point closest = further_rect.Closest(target);
if (closest.Distance(target) < Math.Sqrt(max_dist_sqd))
{
if (pivot_to_target < dist_sqd)
{
nearest_node = tree;
dist_sqd = pivot_to_target;
nnl.insert(tree, dist_sqd);
if (nnl.isCapacityReached())
max_dist_sqd = nnl.getMaxPriority();
else
max_dist_sqd = Double.MaxValue;
}
nearest(further_node, target, further_rect, max_dist_sqd, level + 1, nnl);
BDNode temp_nearest = (BDNode)nnl.getHighest();
double temp_dist_sqd = nnl.getMaxPriority();
if (temp_dist_sqd < dist_sqd)
{
nearest_node = temp_nearest;
dist_sqd = temp_dist_sqd;
}
}
else if (pivot_to_target < max_dist_sqd)
{
nearest_node = tree;
dist_sqd = pivot_to_target;
}
}
private static void nearest(BDNode tree, Point target, Rect rect, double max_dist_sqd, int level, NearestNeighborList nnl)
{
if (tree == null)
{
return;
}
Point pivot = tree.Coord;
double pivot_to_target = pivot.SquareDistance(target);
Rect left_rect = rect;
Rect right_rect = new Rect(rect);
bool target_in_left;
if (level % 2 == 0)
{
left_rect.max.X = pivot.X;
right_rect.min.X = pivot.X;
target_in_left = target.X < pivot.X;
}
else
{
left_rect.max.Y = pivot.Y;
right_rect.min.Y = pivot.Y;
target_in_left = target.Y < pivot.Y;
}
BDNode nearer_node, further_node;
Rect nearer_rect, further_rect;
if (target_in_left)
{
nearer_node = tree.Left;
nearer_rect = left_rect;
further_node = tree.Right;
further_rect = right_rect;
}
else
{
nearer_node = tree.Right;
nearer_rect = right_rect;
further_node = tree.Left;
further_rect = left_rect;
}
nearest(nearer_node, target, nearer_rect, max_dist_sqd, level + 1, nnl);
BDNode nearest_node = (BDNode)nnl.getHighest();
double dist_sqd;
if (!nnl.isCapacityReached())
{
dist_sqd = Double.MaxValue;
}
else
{
dist_sqd = nnl.getMaxPriority();
}
max_dist_sqd = Math.Min(max_dist_sqd, dist_sqd);
Point closest = further_rect.Closest(target);
if (closest.Distance(target) < Math.Sqrt(max_dist_sqd))
{
if (pivot_to_target < dist_sqd)
{
nearest_node = tree;
dist_sqd = pivot_to_target;
nnl.insert(tree, dist_sqd);
if (nnl.isCapacityReached())
{
max_dist_sqd = nnl.getMaxPriority();
}
else
{
max_dist_sqd = Double.MaxValue;
}
}
nearest(further_node, target, further_rect, max_dist_sqd, level + 1, nnl);
BDNode temp_nearest = (BDNode)nnl.getHighest();
double temp_dist_sqd = nnl.getMaxPriority();
if (temp_dist_sqd < dist_sqd)
{
nearest_node = temp_nearest;
dist_sqd = temp_dist_sqd;
}
}
else if (pivot_to_target < max_dist_sqd)
{
nearest_node = tree;
dist_sqd = pivot_to_target;
}
}
public void Insert(double distance, double angle, double accuracy)
{
Point p = CurrentPosition(distance, angle);
root = insert(p, root, 0, accuracy);
count++;
}
