1/**************************************************************************/
2/* aabb.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
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/string/ustring.h"
34#include "core/variant/variant.h"
35
36real_t AABB::get_volume() const {
37 return size.x * size.y * size.z;
38}
39
40bool AABB::operator==(const AABB &p_rval) const {
41 return ((position == p_rval.position) && (size == p_rval.size));
42}
43
44bool AABB::operator!=(const AABB &p_rval) const {
45 return ((position != p_rval.position) || (size != p_rval.size));
46}
47
48void AABB::merge_with(const AABB &p_aabb) {
49#ifdef MATH_CHECKS
50 if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
51 ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
52 }
53#endif
54 Vector3 beg_1, beg_2;
55 Vector3 end_1, end_2;
56 Vector3 min, max;
57
58 beg_1 = position;
59 beg_2 = p_aabb.position;
60 end_1 = size + beg_1;
61 end_2 = p_aabb.size + beg_2;
62
63 min.x = (beg_1.x < beg_2.x) ? beg_1.x : beg_2.x;
64 min.y = (beg_1.y < beg_2.y) ? beg_1.y : beg_2.y;
65 min.z = (beg_1.z < beg_2.z) ? beg_1.z : beg_2.z;
66
67 max.x = (end_1.x > end_2.x) ? end_1.x : end_2.x;
68 max.y = (end_1.y > end_2.y) ? end_1.y : end_2.y;
69 max.z = (end_1.z > end_2.z) ? end_1.z : end_2.z;
70
71 position = min;
72 size = max - min;
73}
74
75bool AABB::is_equal_approx(const AABB &p_aabb) const {
76 return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size);
77}
78
79bool AABB::is_finite() const {
80 return position.is_finite() && size.is_finite();
81}
82
83AABB AABB::intersection(const AABB &p_aabb) const {
84#ifdef MATH_CHECKS
85 if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
86 ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
87 }
88#endif
89 Vector3 src_min = position;
90 Vector3 src_max = position + size;
91 Vector3 dst_min = p_aabb.position;
92 Vector3 dst_max = p_aabb.position + p_aabb.size;
93
94 Vector3 min, max;
95
96 if (src_min.x > dst_max.x || src_max.x < dst_min.x) {
97 return AABB();
98 } else {
99 min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x;
100 max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x;
101 }
102
103 if (src_min.y > dst_max.y || src_max.y < dst_min.y) {
104 return AABB();
105 } else {
106 min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y;
107 max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y;
108 }
109
110 if (src_min.z > dst_max.z || src_max.z < dst_min.z) {
111 return AABB();
112 } else {
113 min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z;
114 max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z;
115 }
116
117 return AABB(min, max - min);
118}
119
120bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const {
121#ifdef MATH_CHECKS
122 if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
123 ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
124 }
125#endif
126 Vector3 c1, c2;
127 Vector3 end = position + size;
128 real_t near = -1e20;
129 real_t far = 1e20;
130 int axis = 0;
131
132 for (int i = 0; i < 3; i++) {
133 if (p_dir[i] == 0) {
134 if ((p_from[i] < position[i]) || (p_from[i] > end[i])) {
135 return false;
136 }
137 } else { // ray not parallel to planes in this direction
138 c1[i] = (position[i] - p_from[i]) / p_dir[i];
139 c2[i] = (end[i] - p_from[i]) / p_dir[i];
140
141 if (c1[i] > c2[i]) {
142 SWAP(c1, c2);
143 }
144 if (c1[i] > near) {
145 near = c1[i];
146 axis = i;
147 }
148 if (c2[i] < far) {
149 far = c2[i];
150 }
151 if ((near > far) || (far < 0)) {
152 return false;
153 }
154 }
155 }
156
157 if (r_clip) {
158 *r_clip = c1;
159 }
160 if (r_normal) {
161 *r_normal = Vector3();
162 (*r_normal)[axis] = p_dir[axis] ? -1 : 1;
163 }
164
165 return true;
166}
167
168bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const {
169#ifdef MATH_CHECKS
170 if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
171 ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
172 }
173#endif
174 real_t min = 0, max = 1;
175 int axis = 0;
176 real_t sign = 0;
177
178 for (int i = 0; i < 3; i++) {
179 real_t seg_from = p_from[i];
180 real_t seg_to = p_to[i];
181 real_t box_begin = position[i];
182 real_t box_end = box_begin + size[i];
183 real_t cmin, cmax;
184 real_t csign;
185
186 if (seg_from < seg_to) {
187 if (seg_from > box_end || seg_to < box_begin) {
188 return false;
189 }
190 real_t length = seg_to - seg_from;
191 cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
192 cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
193 csign = -1.0;
194
195 } else {
196 if (seg_to > box_end || seg_from < box_begin) {
197 return false;
198 }
199 real_t length = seg_to - seg_from;
200 cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
201 cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
202 csign = 1.0;
203 }
204
205 if (cmin > min) {
206 min = cmin;
207 axis = i;
208 sign = csign;
209 }
210 if (cmax < max) {
211 max = cmax;
212 }
213 if (max < min) {
214 return false;
215 }
216 }
217
218 Vector3 rel = p_to - p_from;
219
220 if (r_normal) {
221 Vector3 normal;
222 normal[axis] = sign;
223 *r_normal = normal;
224 }
225
226 if (r_clip) {
227 *r_clip = p_from + rel * min;
228 }
229
230 return true;
231}
232
233bool AABB::intersects_plane(const Plane &p_plane) const {
234 Vector3 points[8] = {
235 Vector3(position.x, position.y, position.z),
236 Vector3(position.x, position.y, position.z + size.z),
237 Vector3(position.x, position.y + size.y, position.z),
238 Vector3(position.x, position.y + size.y, position.z + size.z),
239 Vector3(position.x + size.x, position.y, position.z),
240 Vector3(position.x + size.x, position.y, position.z + size.z),
241 Vector3(position.x + size.x, position.y + size.y, position.z),
242 Vector3(position.x + size.x, position.y + size.y, position.z + size.z),
243 };
244
245 bool over = false;
246 bool under = false;
247
248 for (int i = 0; i < 8; i++) {
249 if (p_plane.distance_to(points[i]) > 0) {
250 over = true;
251 } else {
252 under = true;
253 }
254 }
255
256 return under && over;
257}
258
259Vector3 AABB::get_longest_axis() const {
260 Vector3 axis(1, 0, 0);
261 real_t max_size = size.x;
262
263 if (size.y > max_size) {
264 axis = Vector3(0, 1, 0);
265 max_size = size.y;
266 }
267
268 if (size.z > max_size) {
269 axis = Vector3(0, 0, 1);
270 }
271
272 return axis;
273}
274
275int AABB::get_longest_axis_index() const {
276 int axis = 0;
277 real_t max_size = size.x;
278
279 if (size.y > max_size) {
280 axis = 1;
281 max_size = size.y;
282 }
283
284 if (size.z > max_size) {
285 axis = 2;
286 }
287
288 return axis;
289}
290
291Vector3 AABB::get_shortest_axis() const {
292 Vector3 axis(1, 0, 0);
293 real_t min_size = size.x;
294
295 if (size.y < min_size) {
296 axis = Vector3(0, 1, 0);
297 min_size = size.y;
298 }
299
300 if (size.z < min_size) {
301 axis = Vector3(0, 0, 1);
302 }
303
304 return axis;
305}
306
307int AABB::get_shortest_axis_index() const {
308 int axis = 0;
309 real_t min_size = size.x;
310
311 if (size.y < min_size) {
312 axis = 1;
313 min_size = size.y;
314 }
315
316 if (size.z < min_size) {
317 axis = 2;
318 }
319
320 return axis;
321}
322
323AABB AABB::merge(const AABB &p_with) const {
324 AABB aabb = *this;
325 aabb.merge_with(p_with);
326 return aabb;
327}
328
329AABB AABB::expand(const Vector3 &p_vector) const {
330 AABB aabb = *this;
331 aabb.expand_to(p_vector);
332 return aabb;
333}
334
335AABB AABB::grow(real_t p_by) const {
336 AABB aabb = *this;
337 aabb.grow_by(p_by);
338 return aabb;
339}
340
341void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const {
342 ERR_FAIL_INDEX(p_edge, 12);
343 switch (p_edge) {
344 case 0: {
345 r_from = Vector3(position.x + size.x, position.y, position.z);
346 r_to = Vector3(position.x, position.y, position.z);
347 } break;
348 case 1: {
349 r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
350 r_to = Vector3(position.x + size.x, position.y, position.z);
351 } break;
352 case 2: {
353 r_from = Vector3(position.x, position.y, position.z + size.z);
354 r_to = Vector3(position.x + size.x, position.y, position.z + size.z);
355
356 } break;
357 case 3: {
358 r_from = Vector3(position.x, position.y, position.z);
359 r_to = Vector3(position.x, position.y, position.z + size.z);
360
361 } break;
362 case 4: {
363 r_from = Vector3(position.x, position.y + size.y, position.z);
364 r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
365 } break;
366 case 5: {
367 r_from = Vector3(position.x + size.x, position.y + size.y, position.z);
368 r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
369 } break;
370 case 6: {
371 r_from = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
372 r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
373
374 } break;
375 case 7: {
376 r_from = Vector3(position.x, position.y + size.y, position.z + size.z);
377 r_to = Vector3(position.x, position.y + size.y, position.z);
378
379 } break;
380 case 8: {
381 r_from = Vector3(position.x, position.y, position.z + size.z);
382 r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
383
384 } break;
385 case 9: {
386 r_from = Vector3(position.x, position.y, position.z);
387 r_to = Vector3(position.x, position.y + size.y, position.z);
388
389 } break;
390 case 10: {
391 r_from = Vector3(position.x + size.x, position.y, position.z);
392 r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
393
394 } break;
395 case 11: {
396 r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
397 r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
398
399 } break;
400 }
401}
402
403Variant AABB::intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const {
404 Vector3 inters;
405 if (intersects_segment(p_from, p_to, &inters)) {
406 return inters;
407 }
408 return Variant();
409}
410
411Variant AABB::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const {
412 Vector3 inters;
413 if (intersects_ray(p_from, p_dir, &inters)) {
414 return inters;
415 }
416 return Variant();
417}
418
419AABB::operator String() const {
420 return "[P: " + position.operator String() + ", S: " + size + "]";
421}
422