-
Notifications
You must be signed in to change notification settings - Fork 2
/
RangeFinder.cs
130 lines (106 loc) · 3.89 KB
/
RangeFinder.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Diagnostics;
namespace Engine
{
public class Radar : ISensor
{
public Radar(double sA, double eA,Robot o)
{
owner = o;
startAngle = sA;
endAngle = eA;
max_range = 100.0;
distance = (-1);
noise = 0.0;
}
public double get_value() {
return distance/max_range;
}
public double get_value_raw() {
return distance;
}
public void update(Environment env, List<Robot> robots)
{
offsetx = Math.Cos(owner.heading+delta_theta)*delta_r;
offsety = Math.Sin(owner.heading+delta_theta)*delta_r;
}
public void draw(Graphics g, CoordinateFrame frame)
{
Point a = frame.to_display((float)(owner.location.x+offsetx),(float)(owner.location.y+offsety));
Point b = frame.to_display((float)(a.X+Math.Cos(startAngle+owner.heading)*distance),
(float)(a.Y+Math.Sin(startAngle+owner.heading)*distance));
Point c = frame.to_display((float)(a.X+Math.Cos(endAngle+owner.heading)*distance),
(float)(a.Y+Math.Sin(endAngle+owner.heading)*distance));
g.DrawLine(EngineUtilities.GreendPen, a, b);
g.DrawLine(EngineUtilities.GreendPen, a, c);
g.DrawLine(EngineUtilities.GreendPen, b, c);
}
public Robot owner;
public double startAngle;
public double endAngle;
public double delta_r=0.0;
public double delta_theta=0.0;
public double offsetx=0.0;
public double offsety=0.0;
public bool hitTerrorist;
public bool hitRobot;
public double distance;
public double max_range;
public double noise; //for broken sensors?
}
public class RangeFinder : ISensor
{
public RangeFinder(double a,Robot o,double _max_range)
{
owner =o;
angle = a;
max_range = _max_range;///EPUCK 15.0; 30 works //epuck radius = 3.8
distance = (-1);
noise = 0.0;
}
public double get_value() {
return distance/max_range;
}
public double get_value_raw() {
return distance;
}
public void update(Environment env, List<Robot> robots)
{
bool hitRobot;
bool agentsVisible = false; //TODO
offsetx = Math.Cos(owner.heading+delta_theta)*delta_r;
offsety = Math.Sin(owner.heading+delta_theta)*delta_r;
if (agentsVisible) {
distance = EngineUtilities.raycast(angle,max_range, new Point2D(offsetx + owner.location.x, offsety + owner.location.y), owner.heading, out hitRobot,
env.walls, robots, owner);
}
else
{
distance = EngineUtilities.raycast(angle,max_range, new Point2D(offsetx + owner.circle.p.x, offsety + owner.circle.p.y), owner.heading,env.walls,owner);
}
Debug.Assert(!Double.IsNaN(distance), "NaN in inputs");
}
public void set_delta(double dr,double dt) { delta_r=dr; delta_theta=dt; }
public void draw(Graphics g, CoordinateFrame frame)
{
Point a = frame.to_display((float)(owner.location.x+offsetx),(float)(owner.location.y+offsety));
Point b = frame.to_display((float)(owner.location.x+offsetx+Math.Cos(angle+owner.heading)*distance),
(float)(owner.location.y+offsety+Math.Sin(angle+owner.heading)*distance));
g.DrawLine(EngineUtilities.GreendPen, a, b);
}
public Robot owner;
public double delta_r=0.0;
public double delta_theta=0.0;
public double offsetx=0.0;
public double offsety=0.0;
public bool hitTerrorist;
public bool hitRobot;
public double angle;
public double distance;
public double max_range;
public double noise; //for broken sensors?
}
}