1 /*************************************************************************/
2 /* aabb.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
10 /* */
11 /* Permission is hereby granted, free of charge, to any person obtaining */
12 /* a copy of this software and associated documentation files (the */
13 /* "Software"), to deal in the Software without restriction, including */
14 /* without limitation the rights to use, copy, modify, merge, publish, */
15 /* distribute, sublicense, and/or sell copies of the Software, and to */
16 /* permit persons to whom the Software is furnished to do so, subject to */
17 /* the following conditions: */
18 /* */
19 /* The above copyright notice and this permission notice shall be */
20 /* included in all copies or substantial portions of the Software. */
21 /* */
22 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24 /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
25 /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26 /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29 /*************************************************************************/
30
31 #include "aabb.h"
32
33 #include "core/print_string.h"
34
get_area() const35 real_t AABB::get_area() const {
36
37 return size.x * size.y * size.z;
38 }
39
operator ==(const AABB & p_rval) const40 bool AABB::operator==(const AABB &p_rval) const {
41
42 return ((position == p_rval.position) && (size == p_rval.size));
43 }
operator !=(const AABB & p_rval) const44 bool AABB::operator!=(const AABB &p_rval) const {
45
46 return ((position != p_rval.position) || (size != p_rval.size));
47 }
48
merge_with(const AABB & p_aabb)49 void AABB::merge_with(const AABB &p_aabb) {
50
51 Vector3 beg_1, beg_2;
52 Vector3 end_1, end_2;
53 Vector3 min, max;
54
55 beg_1 = position;
56 beg_2 = p_aabb.position;
57 end_1 = Vector3(size.x, size.y, size.z) + beg_1;
58 end_2 = Vector3(p_aabb.size.x, p_aabb.size.y, p_aabb.size.z) + beg_2;
59
60 min.x = (beg_1.x < beg_2.x) ? beg_1.x : beg_2.x;
61 min.y = (beg_1.y < beg_2.y) ? beg_1.y : beg_2.y;
62 min.z = (beg_1.z < beg_2.z) ? beg_1.z : beg_2.z;
63
64 max.x = (end_1.x > end_2.x) ? end_1.x : end_2.x;
65 max.y = (end_1.y > end_2.y) ? end_1.y : end_2.y;
66 max.z = (end_1.z > end_2.z) ? end_1.z : end_2.z;
67
68 position = min;
69 size = max - min;
70 }
71
is_equal_approx(const AABB & p_aabb) const72 bool AABB::is_equal_approx(const AABB &p_aabb) const {
73
74 return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size);
75 }
76
intersection(const AABB & p_aabb) const77 AABB AABB::intersection(const AABB &p_aabb) const {
78
79 Vector3 src_min = position;
80 Vector3 src_max = position + size;
81 Vector3 dst_min = p_aabb.position;
82 Vector3 dst_max = p_aabb.position + p_aabb.size;
83
84 Vector3 min, max;
85
86 if (src_min.x > dst_max.x || src_max.x < dst_min.x)
87 return AABB();
88 else {
89
90 min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x;
91 max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x;
92 }
93
94 if (src_min.y > dst_max.y || src_max.y < dst_min.y)
95 return AABB();
96 else {
97
98 min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y;
99 max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y;
100 }
101
102 if (src_min.z > dst_max.z || src_max.z < dst_min.z)
103 return AABB();
104 else {
105
106 min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z;
107 max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z;
108 }
109
110 return AABB(min, max - min);
111 }
112
intersects_ray(const Vector3 & p_from,const Vector3 & p_dir,Vector3 * r_clip,Vector3 * r_normal) const113 bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const {
114
115 Vector3 c1, c2;
116 Vector3 end = position + size;
117 real_t near = -1e20;
118 real_t far = 1e20;
119 int axis = 0;
120
121 for (int i = 0; i < 3; i++) {
122 if (p_dir[i] == 0) {
123 if ((p_from[i] < position[i]) || (p_from[i] > end[i])) {
124 return false;
125 }
126 } else { // ray not parallel to planes in this direction
127 c1[i] = (position[i] - p_from[i]) / p_dir[i];
128 c2[i] = (end[i] - p_from[i]) / p_dir[i];
129
130 if (c1[i] > c2[i]) {
131 SWAP(c1, c2);
132 }
133 if (c1[i] > near) {
134 near = c1[i];
135 axis = i;
136 }
137 if (c2[i] < far) {
138 far = c2[i];
139 }
140 if ((near > far) || (far < 0)) {
141 return false;
142 }
143 }
144 }
145
146 if (r_clip)
147 *r_clip = c1;
148 if (r_normal) {
149 *r_normal = Vector3();
150 (*r_normal)[axis] = p_dir[axis] ? -1 : 1;
151 }
152
153 return true;
154 }
155
intersects_segment(const Vector3 & p_from,const Vector3 & p_to,Vector3 * r_clip,Vector3 * r_normal) const156 bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const {
157
158 real_t min = 0, max = 1;
159 int axis = 0;
160 real_t sign = 0;
161
162 for (int i = 0; i < 3; i++) {
163 real_t seg_from = p_from[i];
164 real_t seg_to = p_to[i];
165 real_t box_begin = position[i];
166 real_t box_end = box_begin + size[i];
167 real_t cmin, cmax;
168 real_t csign;
169
170 if (seg_from < seg_to) {
171
172 if (seg_from > box_end || seg_to < box_begin)
173 return false;
174 real_t length = seg_to - seg_from;
175 cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
176 cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
177 csign = -1.0;
178
179 } else {
180
181 if (seg_to > box_end || seg_from < box_begin)
182 return false;
183 real_t length = seg_to - seg_from;
184 cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
185 cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
186 csign = 1.0;
187 }
188
189 if (cmin > min) {
190 min = cmin;
191 axis = i;
192 sign = csign;
193 }
194 if (cmax < max)
195 max = cmax;
196 if (max < min)
197 return false;
198 }
199
200 Vector3 rel = p_to - p_from;
201
202 if (r_normal) {
203 Vector3 normal;
204 normal[axis] = sign;
205 *r_normal = normal;
206 }
207
208 if (r_clip)
209 *r_clip = p_from + rel * min;
210
211 return true;
212 }
213
intersects_plane(const Plane & p_plane) const214 bool AABB::intersects_plane(const Plane &p_plane) const {
215
216 Vector3 points[8] = {
217 Vector3(position.x, position.y, position.z),
218 Vector3(position.x, position.y, position.z + size.z),
219 Vector3(position.x, position.y + size.y, position.z),
220 Vector3(position.x, position.y + size.y, position.z + size.z),
221 Vector3(position.x + size.x, position.y, position.z),
222 Vector3(position.x + size.x, position.y, position.z + size.z),
223 Vector3(position.x + size.x, position.y + size.y, position.z),
224 Vector3(position.x + size.x, position.y + size.y, position.z + size.z),
225 };
226
227 bool over = false;
228 bool under = false;
229
230 for (int i = 0; i < 8; i++) {
231
232 if (p_plane.distance_to(points[i]) > 0)
233 over = true;
234 else
235 under = true;
236 }
237
238 return under && over;
239 }
240
get_longest_axis() const241 Vector3 AABB::get_longest_axis() const {
242
243 Vector3 axis(1, 0, 0);
244 real_t max_size = size.x;
245
246 if (size.y > max_size) {
247 axis = Vector3(0, 1, 0);
248 max_size = size.y;
249 }
250
251 if (size.z > max_size) {
252 axis = Vector3(0, 0, 1);
253 }
254
255 return axis;
256 }
get_longest_axis_index() const257 int AABB::get_longest_axis_index() const {
258
259 int axis = 0;
260 real_t max_size = size.x;
261
262 if (size.y > max_size) {
263 axis = 1;
264 max_size = size.y;
265 }
266
267 if (size.z > max_size) {
268 axis = 2;
269 }
270
271 return axis;
272 }
273
get_shortest_axis() const274 Vector3 AABB::get_shortest_axis() const {
275
276 Vector3 axis(1, 0, 0);
277 real_t max_size = size.x;
278
279 if (size.y < max_size) {
280 axis = Vector3(0, 1, 0);
281 max_size = size.y;
282 }
283
284 if (size.z < max_size) {
285 axis = Vector3(0, 0, 1);
286 }
287
288 return axis;
289 }
get_shortest_axis_index() const290 int AABB::get_shortest_axis_index() const {
291
292 int axis = 0;
293 real_t max_size = size.x;
294
295 if (size.y < max_size) {
296 axis = 1;
297 max_size = size.y;
298 }
299
300 if (size.z < max_size) {
301 axis = 2;
302 }
303
304 return axis;
305 }
306
merge(const AABB & p_with) const307 AABB AABB::merge(const AABB &p_with) const {
308
309 AABB aabb = *this;
310 aabb.merge_with(p_with);
311 return aabb;
312 }
expand(const Vector3 & p_vector) const313 AABB AABB::expand(const Vector3 &p_vector) const {
314 AABB aabb = *this;
315 aabb.expand_to(p_vector);
316 return aabb;
317 }
grow(real_t p_by) const318 AABB AABB::grow(real_t p_by) const {
319
320 AABB aabb = *this;
321 aabb.grow_by(p_by);
322 return aabb;
323 }
324
get_edge(int p_edge,Vector3 & r_from,Vector3 & r_to) const325 void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const {
326
327 ERR_FAIL_INDEX(p_edge, 12);
328 switch (p_edge) {
329
330 case 0: {
331
332 r_from = Vector3(position.x + size.x, position.y, position.z);
333 r_to = Vector3(position.x, position.y, position.z);
334 } break;
335 case 1: {
336
337 r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
338 r_to = Vector3(position.x + size.x, position.y, position.z);
339 } break;
340 case 2: {
341 r_from = Vector3(position.x, position.y, position.z + size.z);
342 r_to = Vector3(position.x + size.x, position.y, position.z + size.z);
343
344 } break;
345 case 3: {
346
347 r_from = Vector3(position.x, position.y, position.z);
348 r_to = Vector3(position.x, position.y, position.z + size.z);
349
350 } break;
351 case 4: {
352
353 r_from = Vector3(position.x, position.y + size.y, position.z);
354 r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
355 } break;
356 case 5: {
357
358 r_from = Vector3(position.x + size.x, position.y + size.y, position.z);
359 r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
360 } break;
361 case 6: {
362 r_from = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
363 r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
364
365 } break;
366 case 7: {
367
368 r_from = Vector3(position.x, position.y + size.y, position.z + size.z);
369 r_to = Vector3(position.x, position.y + size.y, position.z);
370
371 } break;
372 case 8: {
373
374 r_from = Vector3(position.x, position.y, position.z + size.z);
375 r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
376
377 } break;
378 case 9: {
379
380 r_from = Vector3(position.x, position.y, position.z);
381 r_to = Vector3(position.x, position.y + size.y, position.z);
382
383 } break;
384 case 10: {
385
386 r_from = Vector3(position.x + size.x, position.y, position.z);
387 r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
388
389 } break;
390 case 11: {
391
392 r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
393 r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
394
395 } break;
396 }
397 }
398
operator String() const399 AABB::operator String() const {
400
401 return String() + position + " - " + size;
402 }
403