1 package cz.cuni.amis.pogamut.ut2004.agent.module.sensor;
2
3 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
4 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
5 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
6 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
7 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
8 import cz.cuni.amis.utils.HashCode;
9 import cz.cuni.amis.utils.NullCheck;
10 import cz.cuni.amis.utils.SafeEquals;
11
12
13
14
15
16
17
18
19
20 public class NavLinkPair {
21
22 private NavPointNeighbourLink link1;
23 private NavPointNeighbourLink link2;
24
25 private NavPoint nav1;
26 private NavPoint nav2;
27
28 private Location x1, x2;
29
30 private int hashCode;
31
32 public NavLinkPair(NavPointNeighbourLink first) {
33 link1 = first;
34
35 NullCheck.check(link1, "first");
36
37 HashCode hc = new HashCode();
38 hc.add(link1.hashCode());
39 hashCode = hc.getHash();
40
41 nav1 = link1.getFromNavPoint();
42 nav2 = link1.getToNavPoint();
43 x1 = link1.getFromNavPoint().getLocation();
44 x2 = link1.getToNavPoint().getLocation();
45
46
47
48
49
50
51
52 }
53
54
55
56
57
58
59
60
61
62
63
64 public NavLinkPair(NavPointNeighbourLink first, NavPointNeighbourLink second) {
65 if (first == null) {
66 first = second;
67 second = null;
68 } else
69 if (second != null) {
70 if (first.hashCode() > second.hashCode()) {
71 NavPointNeighbourLink temp = second;
72 second = first;
73 first = temp;
74 }
75 }
76
77 link1 = first;
78 link2 = second;
79
80 NullCheck.check(link1, "'first' and 'second'");
81
82 HashCode hc = new HashCode();
83 hc.add(link1.hashCode());
84 if (link2 != null) {
85 hc.add(link2.hashCode());
86 }
87 hashCode = hc.getHash();
88
89 nav1 = link1.getFromNavPoint();
90 nav2 = link1.getToNavPoint();
91 x1 = nav1.getLocation();
92 x2 = nav2.getLocation();
93
94
95
96
97
98
99
100 }
101
102 @Override
103 public int hashCode() {
104 return hashCode;
105 }
106
107 @Override
108 public boolean equals(Object obj) {
109 if (obj == null) return false;
110 if (!(obj instanceof NavLinkPair)) return false;
111 if (hashCode != obj.hashCode()) return false;
112 NavLinkPair pair = (NavLinkPair)obj;
113 return SafeEquals.equals(link1, pair.getNavLink1()) && SafeEquals.equals(link2, pair.getNavLink2());
114 }
115
116
117
118
119
120 public NavPointNeighbourLink getNavLink1() {
121 return link1;
122 }
123
124
125
126
127
128 public NavPointNeighbourLink getNavLink2() {
129 return link2;
130 }
131
132
133
134
135
136 public NavPoint getNavPoint1() {
137 return nav1;
138 }
139
140
141
142
143
144 public NavPoint getNavPoint2() {
145 return nav2;
146 }
147
148
149
150
151
152
153 public boolean isLinkNavPoint(NavPoint navPoint) {
154 if (navPoint == null) return false;
155 return SafeEquals.equals(navPoint.getId(), nav1.getId()) || SafeEquals.equals(navPoint.getId(), nav2.getId());
156 }
157
158
159
160
161
162
163 public boolean isLinkNavPoint(UnrealId navPointId) {
164 if (navPointId == null) return false;
165 return SafeEquals.equals(navPointId, nav1.getId()) || SafeEquals.equals(navPointId, nav2.getId());
166 }
167
168
169
170
171
172
173
174 public NavPointNeighbourLink getLinkLeadingTo(UnrealId navPointLinkEndsInId) {
175 if (navPointLinkEndsInId == null) return null;
176 if (link1 != null && link1.getToNavPoint() != null) {
177 if (link1.getToNavPoint().getId().equals(navPointLinkEndsInId)) return link1;
178 }
179 if (link2 != null && link2.getToNavPoint() != null) {
180 if (link2.getToNavPoint().getId().equals(navPointLinkEndsInId)) return link1;
181 }
182 return null;
183 }
184
185
186
187
188
189
190
191 public NavPointNeighbourLink getLinkComingFrom(UnrealId navPointLinkIsComingFromId) {
192 if (navPointLinkIsComingFromId == null) return null;
193 if (link1 != null && link1.getFromNavPoint() != null) {
194 if (link1.getFromNavPoint().getId().equals(navPointLinkIsComingFromId)) return link1;
195 }
196 if (link2 != null && link2.getFromNavPoint() != null) {
197 if (link2.getFromNavPoint().getId().equals(navPointLinkIsComingFromId)) return link1;
198 }
199 return null;
200 }
201
202
203
204
205
206
207
208 public NavPointNeighbourLink getLinkLeadingTo(NavPoint navPointLinkEndsIn) {
209 if (navPointLinkEndsIn == null) return null;
210 if (link1 != null && link1.getToNavPoint() != null) {
211 if (link1.getToNavPoint().getId().equals(navPointLinkEndsIn.getId())) return link1;
212 }
213 if (link2 != null && link2.getToNavPoint() != null) {
214 if (link2.getToNavPoint().getId().equals(navPointLinkEndsIn.getId())) return link1;
215 }
216 return null;
217 }
218
219
220
221
222
223
224
225 public NavPointNeighbourLink getLinkComingFrom(NavPoint navPointLinkIsComingFrom) {
226 if (navPointLinkIsComingFrom == null) return null;
227 if (link1 != null && link1.getFromNavPoint() != null) {
228 if (link1.getFromNavPoint().getId().equals(navPointLinkIsComingFrom.getId())) return link1;
229 }
230 if (link2 != null && link2.getFromNavPoint() != null) {
231 if (link2.getFromNavPoint().getId().equals(navPointLinkIsComingFrom.getId())) return link1;
232 }
233 return null;
234 }
235
236
237
238
239
240 public Location getFirstVector() {
241 if (link1 == null) return null;
242 return link1.getToNavPoint().getLocation().sub(link1.getFromNavPoint().getLocation());
243 }
244
245
246
247
248
249 public Location getSecondVector() {
250 if (link2 == null) return null;
251 return link2.getToNavPoint().getLocation().sub(link2.getFromNavPoint().getLocation());
252 }
253
254
255
256
257
258
259
260
261
262 public double getDistance(ILocated point) {
263 if (point == null) return Double.MAX_VALUE;
264 if (x1 == null || x2 == null) return Double.MAX_VALUE;
265
266
267 Location x0 = point.getLocation();
268 double distance = x0.sub(x1).cross(x0.sub(x2)).getLength() / x2.sub(x1).getLength();
269
270 return distance;
271 }
272
273 }