From 69d5de632e04d48a247d50c1dc2c09322129c73a Mon Sep 17 00:00:00 2001 From: "Andrii Doroshenko (Xrayez)" Date: Mon, 25 May 2020 20:20:45 +0300 Subject: Split `Geometry` singleton into `Geometry2D` and `Geometry3D` Extra `_2d` suffixes are removed from 2D methods accoringly. --- core/math/a_star.cpp | 4 +- core/math/face3.cpp | 6 +- core/math/geometry.cpp | 1377 --------------------------------------------- core/math/geometry.h | 1310 ------------------------------------------ core/math/geometry_2d.cpp | 384 +++++++++++++ core/math/geometry_2d.h | 398 +++++++++++++ core/math/geometry_3d.cpp | 1013 +++++++++++++++++++++++++++++++++ core/math/geometry_3d.h | 950 +++++++++++++++++++++++++++++++ core/math/octree.h | 4 +- core/math/quick_hull.cpp | 18 +- core/math/quick_hull.h | 6 +- 11 files changed, 2764 insertions(+), 2706 deletions(-) delete mode 100644 core/math/geometry.cpp delete mode 100644 core/math/geometry.h create mode 100644 core/math/geometry_2d.cpp create mode 100644 core/math/geometry_2d.h create mode 100644 core/math/geometry_3d.cpp create mode 100644 core/math/geometry_3d.h (limited to 'core/math') diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 45c4a207c3..580a7cf7bb 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -30,7 +30,7 @@ #include "a_star.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/script_language.h" #include "scene/scene_string_names.h" @@ -309,7 +309,7 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { to_point->pos, }; - Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment); + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, segment); real_t d = p_point.distance_squared_to(p); if (!found || d < closest_dist) { closest_point = p; diff --git a/core/math/face3.cpp b/core/math/face3.cpp index 6d76e116be..db2bfaa58b 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -30,7 +30,7 @@ #include "face3.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_over[3]) const { ERR_FAIL_COND_V(is_degenerate(), 0); @@ -108,11 +108,11 @@ int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_ } bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { - return Geometry::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); + return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); } bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { - return Geometry::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); + return Geometry3D::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection); } bool Face3::is_degenerate() const { diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp deleted file mode 100644 index a4e9169a6f..0000000000 --- a/core/math/geometry.cpp +++ /dev/null @@ -1,1377 +0,0 @@ -/*************************************************************************/ -/* geometry.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "geometry.h" - -#include "core/print_string.h" - -#include "thirdparty/misc/clipper.hpp" -#include "thirdparty/misc/triangulator.h" -#define STB_RECT_PACK_IMPLEMENTATION -#include "thirdparty/misc/stb_rect_pack.h" - -#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. - -// This implementation is very inefficient, commenting unless bugs happen. See the other one. -/* -bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector &p_polygon) { - Vector indices = Geometry::triangulate_polygon(p_polygon); - for (int j = 0; j + 3 <= indices.size(); j += 3) { - int i1 = indices[j], i2 = indices[j + 1], i3 = indices[j + 2]; - if (Geometry::is_point_in_triangle(p_point, p_polygon[i1], p_polygon[i2], p_polygon[i3])) { - return true; - } - } - return false; -} -*/ - -void Geometry::MeshData::optimize_vertices() { - Map vtx_remap; - - for (int i = 0; i < faces.size(); i++) { - for (int j = 0; j < faces[i].indices.size(); j++) { - int idx = faces[i].indices[j]; - if (!vtx_remap.has(idx)) { - int ni = vtx_remap.size(); - vtx_remap[idx] = ni; - } - - faces.write[i].indices.write[j] = vtx_remap[idx]; - } - } - - for (int i = 0; i < edges.size(); i++) { - int a = edges[i].a; - int b = edges[i].b; - - if (!vtx_remap.has(a)) { - int ni = vtx_remap.size(); - vtx_remap[a] = ni; - } - if (!vtx_remap.has(b)) { - int ni = vtx_remap.size(); - vtx_remap[b] = ni; - } - - edges.write[i].a = vtx_remap[a]; - edges.write[i].b = vtx_remap[b]; - } - - Vector new_vertices; - new_vertices.resize(vtx_remap.size()); - - for (int i = 0; i < vertices.size(); i++) { - if (vtx_remap.has(i)) { - new_vertices.write[vtx_remap[i]] = vertices[i]; - } - } - vertices = new_vertices; -} - -struct _FaceClassify { - struct _Link { - int face = -1; - int edge = -1; - void clear() { - face = -1; - edge = -1; - } - _Link() {} - }; - bool valid = false; - int group = -1; - _Link links[3]; - Face3 face; - _FaceClassify() {} -}; - -static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { - // Connect faces, error will occur if an edge is shared between more than 2 faces. - // Clear connections. - - bool error = false; - - for (int i = 0; i < len; i++) { - for (int j = 0; j < 3; j++) { - p_faces[i].links[j].clear(); - } - } - - for (int i = 0; i < len; i++) { - if (p_faces[i].group != p_group) { - continue; - } - for (int j = i + 1; j < len; j++) { - if (p_faces[j].group != p_group) { - continue; - } - - for (int k = 0; k < 3; k++) { - Vector3 vi1 = p_faces[i].face.vertex[k]; - Vector3 vi2 = p_faces[i].face.vertex[(k + 1) % 3]; - - for (int l = 0; l < 3; l++) { - Vector3 vj2 = p_faces[j].face.vertex[l]; - Vector3 vj1 = p_faces[j].face.vertex[(l + 1) % 3]; - - if (vi1.distance_to(vj1) < 0.00001 && - vi2.distance_to(vj2) < 0.00001) { - if (p_faces[i].links[k].face != -1) { - ERR_PRINT("already linked\n"); - error = true; - break; - } - if (p_faces[j].links[l].face != -1) { - ERR_PRINT("already linked\n"); - error = true; - break; - } - - p_faces[i].links[k].face = j; - p_faces[i].links[k].edge = l; - p_faces[j].links[l].face = i; - p_faces[j].links[l].edge = k; - } - } - if (error) { - break; - } - } - if (error) { - break; - } - } - if (error) { - break; - } - } - - for (int i = 0; i < len; i++) { - p_faces[i].valid = true; - for (int j = 0; j < 3; j++) { - if (p_faces[i].links[j].face == -1) { - p_faces[i].valid = false; - } - } - } - return error; -} - -static bool _group_face(_FaceClassify *p_faces, int len, int p_index, int p_group) { - if (p_faces[p_index].group >= 0) { - return false; - } - - p_faces[p_index].group = p_group; - - for (int i = 0; i < 3; i++) { - ERR_FAIL_INDEX_V(p_faces[p_index].links[i].face, len, true); - _group_face(p_faces, len, p_faces[p_index].links[i].face, p_group); - } - - return true; -} - -Vector> Geometry::separate_objects(Vector p_array) { - Vector> objects; - - int len = p_array.size(); - - const Face3 *arrayptr = p_array.ptr(); - - Vector<_FaceClassify> fc; - - fc.resize(len); - - _FaceClassify *_fcptr = fc.ptrw(); - - for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; - } - - bool error = _connect_faces(_fcptr, len, -1); - - ERR_FAIL_COND_V_MSG(error, Vector>(), "Invalid geometry."); - - // Group connected faces in separate objects. - - int group = 0; - for (int i = 0; i < len; i++) { - if (!_fcptr[i].valid) { - continue; - } - if (_group_face(_fcptr, len, i, group)) { - group++; - } - } - - // Group connected faces in separate objects. - - for (int i = 0; i < len; i++) { - _fcptr[i].face = arrayptr[i]; - } - - if (group >= 0) { - objects.resize(group); - Vector *group_faces = objects.ptrw(); - - for (int i = 0; i < len; i++) { - if (!_fcptr[i].valid) { - continue; - } - if (_fcptr[i].group >= 0 && _fcptr[i].group < group) { - group_faces[_fcptr[i].group].push_back(_fcptr[i].face); - } - } - } - - return objects; -} - -/*** GEOMETRY WRAPPER ***/ - -enum _CellFlags { - - _CELL_SOLID = 1, - _CELL_EXTERIOR = 2, - _CELL_STEP_MASK = 0x1C, - _CELL_STEP_NONE = 0 << 2, - _CELL_STEP_Y_POS = 1 << 2, - _CELL_STEP_Y_NEG = 2 << 2, - _CELL_STEP_X_POS = 3 << 2, - _CELL_STEP_X_NEG = 4 << 2, - _CELL_STEP_Z_POS = 5 << 2, - _CELL_STEP_Z_NEG = 6 << 2, - _CELL_STEP_DONE = 7 << 2, - _CELL_PREV_MASK = 0xE0, - _CELL_PREV_NONE = 0 << 5, - _CELL_PREV_Y_POS = 1 << 5, - _CELL_PREV_Y_NEG = 2 << 5, - _CELL_PREV_X_POS = 3 << 5, - _CELL_PREV_X_NEG = 4 << 5, - _CELL_PREV_Z_POS = 5 << 5, - _CELL_PREV_Z_NEG = 6 << 5, - _CELL_PREV_FIRST = 7 << 5, - -}; - -static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, const Vector3 &voxelsize, const Face3 &p_face) { - AABB aabb(Vector3(x, y, z), Vector3(len_x, len_y, len_z)); - aabb.position = aabb.position * voxelsize; - aabb.size = aabb.size * voxelsize; - - if (!p_face.intersects_aabb(aabb)) { - return; - } - - if (len_x == 1 && len_y == 1 && len_z == 1) { - p_cell_status[x][y][z] = _CELL_SOLID; - return; - } - - int div_x = len_x > 1 ? 2 : 1; - int div_y = len_y > 1 ? 2 : 1; - int div_z = len_z > 1 ? 2 : 1; - -#define _SPLIT(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \ - if (m_div == 1) { \ - m_new_v = m_v; \ - m_new_len_v = 1; \ - } else if (m_i == 0) { \ - m_new_v = m_v; \ - m_new_len_v = m_len_v / 2; \ - } else { \ - m_new_v = m_v + m_len_v / 2; \ - m_new_len_v = m_len_v - m_len_v / 2; \ - } - - int new_x; - int new_len_x; - int new_y; - int new_len_y; - int new_z; - int new_len_z; - - for (int i = 0; i < div_x; i++) { - _SPLIT(i, div_x, x, len_x, new_x, new_len_x); - - for (int j = 0; j < div_y; j++) { - _SPLIT(j, div_y, y, len_y, new_y, new_len_y); - - for (int k = 0; k < div_z; k++) { - _SPLIT(k, div_z, z, len_z, new_z, new_len_z); - - _plot_face(p_cell_status, new_x, new_y, new_z, new_len_x, new_len_y, new_len_z, voxelsize, p_face); - } - } - } -} - -static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) { - if (p_cell_status[x][y][z] & 3) { - return; // Nothing to do, already used and/or visited. - } - - p_cell_status[x][y][z] = _CELL_PREV_FIRST; - - while (true) { - uint8_t &c = p_cell_status[x][y][z]; - - if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) { - // Haven't been in here, mark as outside. - p_cell_status[x][y][z] |= _CELL_EXTERIOR; - } - - if ((c & _CELL_STEP_MASK) != _CELL_STEP_DONE) { - // If not done, increase step. - c += 1 << 2; - } - - if ((c & _CELL_STEP_MASK) == _CELL_STEP_DONE) { - // Go back. - switch (c & _CELL_PREV_MASK) { - case _CELL_PREV_FIRST: { - return; - } break; - case _CELL_PREV_Y_POS: { - y++; - ERR_FAIL_COND(y >= len_y); - } break; - case _CELL_PREV_Y_NEG: { - y--; - ERR_FAIL_COND(y < 0); - } break; - case _CELL_PREV_X_POS: { - x++; - ERR_FAIL_COND(x >= len_x); - } break; - case _CELL_PREV_X_NEG: { - x--; - ERR_FAIL_COND(x < 0); - } break; - case _CELL_PREV_Z_POS: { - z++; - ERR_FAIL_COND(z >= len_z); - } break; - case _CELL_PREV_Z_NEG: { - z--; - ERR_FAIL_COND(z < 0); - } break; - default: { - ERR_FAIL(); - } - } - continue; - } - - int next_x = x, next_y = y, next_z = z; - uint8_t prev = 0; - - switch (c & _CELL_STEP_MASK) { - case _CELL_STEP_Y_POS: { - next_y++; - prev = _CELL_PREV_Y_NEG; - } break; - case _CELL_STEP_Y_NEG: { - next_y--; - prev = _CELL_PREV_Y_POS; - } break; - case _CELL_STEP_X_POS: { - next_x++; - prev = _CELL_PREV_X_NEG; - } break; - case _CELL_STEP_X_NEG: { - next_x--; - prev = _CELL_PREV_X_POS; - } break; - case _CELL_STEP_Z_POS: { - next_z++; - prev = _CELL_PREV_Z_NEG; - } break; - case _CELL_STEP_Z_NEG: { - next_z--; - prev = _CELL_PREV_Z_POS; - } break; - default: - ERR_FAIL(); - } - - if (next_x < 0 || next_x >= len_x) { - continue; - } - if (next_y < 0 || next_y >= len_y) { - continue; - } - if (next_z < 0 || next_z >= len_z) { - continue; - } - - if (p_cell_status[next_x][next_y][next_z] & 3) { - continue; - } - - x = next_x; - y = next_y; - z = next_z; - p_cell_status[x][y][z] |= prev; - } -} - -static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, Vector &p_faces) { - ERR_FAIL_INDEX(x, len_x); - ERR_FAIL_INDEX(y, len_y); - ERR_FAIL_INDEX(z, len_z); - - if (p_cell_status[x][y][z] & _CELL_EXTERIOR) { - return; - } - -#define vert(m_idx) Vector3(((m_idx)&4) >> 2, ((m_idx)&2) >> 1, (m_idx)&1) - - static const uint8_t indices[6][4] = { - { 7, 6, 4, 5 }, - { 7, 3, 2, 6 }, - { 7, 5, 1, 3 }, - { 0, 2, 3, 1 }, - { 0, 1, 5, 4 }, - { 0, 4, 6, 2 }, - - }; - - for (int i = 0; i < 6; i++) { - Vector3 face_points[4]; - int disp_x = x + ((i % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); - int disp_y = y + (((i - 1) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); - int disp_z = z + (((i - 2) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); - - bool plot = false; - - if (disp_x < 0 || disp_x >= len_x) { - plot = true; - } - if (disp_y < 0 || disp_y >= len_y) { - plot = true; - } - if (disp_z < 0 || disp_z >= len_z) { - plot = true; - } - - if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) { - plot = true; - } - - if (!plot) { - continue; - } - - for (int j = 0; j < 4; j++) { - face_points[j] = vert(indices[i][j]) + Vector3(x, y, z); - } - - p_faces.push_back( - Face3( - face_points[0], - face_points[1], - face_points[2])); - - p_faces.push_back( - Face3( - face_points[2], - face_points[3], - face_points[0])); - } -} - -Vector Geometry::wrap_geometry(Vector p_array, real_t *p_error) { -#define _MIN_SIZE 1.0 -#define _MAX_LENGTH 20 - - int face_count = p_array.size(); - const Face3 *faces = p_array.ptr(); - - AABB global_aabb; - - for (int i = 0; i < face_count; i++) { - if (i == 0) { - global_aabb = faces[i].get_aabb(); - } else { - global_aabb.merge_with(faces[i].get_aabb()); - } - } - - global_aabb.grow_by(0.01); // Avoid numerical error. - - // Determine amount of cells in grid axis. - int div_x, div_y, div_z; - - if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) { - div_x = (int)(global_aabb.size.x / _MIN_SIZE) + 1; - } else { - div_x = _MAX_LENGTH; - } - - if (global_aabb.size.y / _MIN_SIZE < _MAX_LENGTH) { - div_y = (int)(global_aabb.size.y / _MIN_SIZE) + 1; - } else { - div_y = _MAX_LENGTH; - } - - if (global_aabb.size.z / _MIN_SIZE < _MAX_LENGTH) { - div_z = (int)(global_aabb.size.z / _MIN_SIZE) + 1; - } else { - div_z = _MAX_LENGTH; - } - - Vector3 voxelsize = global_aabb.size; - voxelsize.x /= div_x; - voxelsize.y /= div_y; - voxelsize.z /= div_z; - - // Create and initialize cells to zero. - - uint8_t ***cell_status = memnew_arr(uint8_t **, div_x); - for (int i = 0; i < div_x; i++) { - cell_status[i] = memnew_arr(uint8_t *, div_y); - - for (int j = 0; j < div_y; j++) { - cell_status[i][j] = memnew_arr(uint8_t, div_z); - - for (int k = 0; k < div_z; k++) { - cell_status[i][j][k] = 0; - } - } - } - - // Plot faces into cells. - - for (int i = 0; i < face_count; i++) { - Face3 f = faces[i]; - for (int j = 0; j < 3; j++) { - f.vertex[j] -= global_aabb.position; - } - _plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f); - } - - // Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking. - - for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - _mark_outside(cell_status, i, j, 0, div_x, div_y, div_z); - _mark_outside(cell_status, i, j, div_z - 1, div_x, div_y, div_z); - } - } - - for (int i = 0; i < div_z; i++) { - for (int j = 0; j < div_y; j++) { - _mark_outside(cell_status, 0, j, i, div_x, div_y, div_z); - _mark_outside(cell_status, div_x - 1, j, i, div_x, div_y, div_z); - } - } - - for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_z; j++) { - _mark_outside(cell_status, i, 0, j, div_x, div_y, div_z); - _mark_outside(cell_status, i, div_y - 1, j, div_x, div_y, div_z); - } - } - - // Build faces for the inside-outside cell divisors. - - Vector wrapped_faces; - - for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - for (int k = 0; k < div_z; k++) { - _build_faces(cell_status, i, j, k, div_x, div_y, div_z, wrapped_faces); - } - } - } - - // Transform face vertices to global coords. - - int wrapped_faces_count = wrapped_faces.size(); - Face3 *wrapped_faces_ptr = wrapped_faces.ptrw(); - - for (int i = 0; i < wrapped_faces_count; i++) { - for (int j = 0; j < 3; j++) { - Vector3 &v = wrapped_faces_ptr[i].vertex[j]; - v = v * voxelsize; - v += global_aabb.position; - } - } - - // clean up grid - - for (int i = 0; i < div_x; i++) { - for (int j = 0; j < div_y; j++) { - memdelete_arr(cell_status[i][j]); - } - - memdelete_arr(cell_status[i]); - } - - memdelete_arr(cell_status); - if (p_error) { - *p_error = voxelsize.length(); - } - - return wrapped_faces; -} - -Vector> Geometry::decompose_polygon_in_convex(Vector polygon) { - Vector> decomp; - List in_poly, out_poly; - - TriangulatorPoly inp; - inp.Init(polygon.size()); - for (int i = 0; i < polygon.size(); i++) { - inp.GetPoint(i) = polygon[i]; - } - inp.SetOrientation(TRIANGULATOR_CCW); - in_poly.push_back(inp); - TriangulatorPartition tpart; - if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed. - ERR_PRINT("Convex decomposing failed!"); - return decomp; - } - - decomp.resize(out_poly.size()); - int idx = 0; - for (List::Element *I = out_poly.front(); I; I = I->next()) { - TriangulatorPoly &tp = I->get(); - - decomp.write[idx].resize(tp.GetNumPoints()); - - for (int64_t i = 0; i < tp.GetNumPoints(); i++) { - decomp.write[idx].write[i] = tp.GetPoint(i); - } - - idx++; - } - - return decomp; -} - -Geometry::MeshData Geometry::build_convex_mesh(const Vector &p_planes) { - MeshData mesh; - -#define SUBPLANE_SIZE 1024.0 - - real_t subplane_size = 1024.0; // Should compute this from the actual plane. - for (int i = 0; i < p_planes.size(); i++) { - Plane p = p_planes[i]; - - Vector3 ref = Vector3(0.0, 1.0, 0.0); - - if (ABS(p.normal.dot(ref)) > 0.95) { - ref = Vector3(0.0, 0.0, 1.0); // Change axis. - } - - Vector3 right = p.normal.cross(ref).normalized(); - Vector3 up = p.normal.cross(right).normalized(); - - Vector vertices; - - Vector3 center = p.get_any_point(); - // make a quad clockwise - vertices.push_back(center - up * subplane_size + right * subplane_size); - vertices.push_back(center - up * subplane_size - right * subplane_size); - vertices.push_back(center + up * subplane_size - right * subplane_size); - vertices.push_back(center + up * subplane_size + right * subplane_size); - - for (int j = 0; j < p_planes.size(); j++) { - if (j == i) { - continue; - } - - Vector new_vertices; - Plane clip = p_planes[j]; - - if (clip.normal.dot(p.normal) > 0.95) { - continue; - } - - if (vertices.size() < 3) { - break; - } - - for (int k = 0; k < vertices.size(); k++) { - int k_n = (k + 1) % vertices.size(); - - Vector3 edge0_A = vertices[k]; - Vector3 edge1_A = vertices[k_n]; - - real_t dist0 = clip.distance_to(edge0_A); - real_t dist1 = clip.distance_to(edge1_A); - - if (dist0 <= 0) { // Behind plane. - - new_vertices.push_back(vertices[k]); - } - - // Check for different sides and non coplanar. - if ((dist0 * dist1) < 0) { - // Calculate intersection. - Vector3 rel = edge1_A - edge0_A; - - real_t den = clip.normal.dot(rel); - if (Math::is_zero_approx(den)) { - continue; // Point too short. - } - - real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; - Vector3 inters = edge0_A + rel * dist; - new_vertices.push_back(inters); - } - } - - vertices = new_vertices; - } - - if (vertices.size() < 3) { - continue; - } - - // Result is a clockwise face. - - MeshData::Face face; - - // Add face indices. - for (int j = 0; j < vertices.size(); j++) { - int idx = -1; - for (int k = 0; k < mesh.vertices.size(); k++) { - if (mesh.vertices[k].distance_to(vertices[j]) < 0.001) { - idx = k; - break; - } - } - - if (idx == -1) { - idx = mesh.vertices.size(); - mesh.vertices.push_back(vertices[j]); - } - - face.indices.push_back(idx); - } - face.plane = p; - mesh.faces.push_back(face); - - // Add edge. - - for (int j = 0; j < face.indices.size(); j++) { - int a = face.indices[j]; - int b = face.indices[(j + 1) % face.indices.size()]; - - bool found = false; - for (int k = 0; k < mesh.edges.size(); k++) { - if (mesh.edges[k].a == a && mesh.edges[k].b == b) { - found = true; - break; - } - if (mesh.edges[k].b == a && mesh.edges[k].a == b) { - found = true; - break; - } - } - - if (found) { - continue; - } - MeshData::Edge edge; - edge.a = a; - edge.b = b; - mesh.edges.push_back(edge); - } - } - - return mesh; -} - -Vector Geometry::build_box_planes(const Vector3 &p_extents) { - Vector planes; - - planes.push_back(Plane(Vector3(1, 0, 0), p_extents.x)); - planes.push_back(Plane(Vector3(-1, 0, 0), p_extents.x)); - planes.push_back(Plane(Vector3(0, 1, 0), p_extents.y)); - planes.push_back(Plane(Vector3(0, -1, 0), p_extents.y)); - planes.push_back(Plane(Vector3(0, 0, 1), p_extents.z)); - planes.push_back(Plane(Vector3(0, 0, -1), p_extents.z)); - - return planes; -} - -Vector Geometry::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { - Vector planes; - - for (int i = 0; i < p_sides; i++) { - Vector3 normal; - normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); - normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); - - planes.push_back(Plane(normal, p_radius)); - } - - Vector3 axis; - axis[p_axis] = 1.0; - - planes.push_back(Plane(axis, p_height * 0.5)); - planes.push_back(Plane(-axis, p_height * 0.5)); - - return planes; -} - -Vector Geometry::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) { - Vector planes; - - Vector3 axis; - axis[p_axis] = 1.0; - - Vector3 axis_neg; - axis_neg[(p_axis + 1) % 3] = 1.0; - axis_neg[(p_axis + 2) % 3] = 1.0; - axis_neg[p_axis] = -1.0; - - for (int i = 0; i < p_lons; i++) { - Vector3 normal; - normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_lons); - normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_lons); - - planes.push_back(Plane(normal, p_radius)); - - for (int j = 1; j <= p_lats; j++) { - // FIXME: This is stupid. - Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); - Vector3 pos = angle * p_radius; - planes.push_back(Plane(pos, angle)); - planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); - } - } - - return planes; -} - -Vector Geometry::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { - Vector planes; - - Vector3 axis; - axis[p_axis] = 1.0; - - Vector3 axis_neg; - axis_neg[(p_axis + 1) % 3] = 1.0; - axis_neg[(p_axis + 2) % 3] = 1.0; - axis_neg[p_axis] = -1.0; - - for (int i = 0; i < p_sides; i++) { - Vector3 normal; - normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); - normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); - - planes.push_back(Plane(normal, p_radius)); - - for (int j = 1; j <= p_lats; j++) { - Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); - Vector3 pos = axis * p_height * 0.5 + angle * p_radius; - planes.push_back(Plane(pos, angle)); - planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); - } - } - - return planes; -} - -struct _AtlasWorkRect { - Size2i s; - Point2i p; - int idx; - _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }; -}; - -struct _AtlasWorkRectResult { - Vector<_AtlasWorkRect> result; - int max_w; - int max_h; -}; - -void Geometry::make_atlas(const Vector &p_rects, Vector &r_result, Size2i &r_size) { - // Super simple, almost brute force scanline stacking fitter. - // It's pretty basic for now, but it tries to make sure that the aspect ratio of the - // resulting atlas is somehow square. This is necessary because video cards have limits. - // On texture size (usually 2048 or 4096), so the more square a texture, the more chances. - // It will work in every hardware. - // For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a - // 256x8192 atlas (won't work anywhere). - - ERR_FAIL_COND(p_rects.size() == 0); - - Vector<_AtlasWorkRect> wrects; - wrects.resize(p_rects.size()); - for (int i = 0; i < p_rects.size(); i++) { - wrects.write[i].s = p_rects[i]; - wrects.write[i].idx = i; - } - wrects.sort(); - int widest = wrects[0].s.width; - - Vector<_AtlasWorkRectResult> results; - - for (int i = 0; i <= 12; i++) { - int w = 1 << i; - int max_h = 0; - int max_w = 0; - if (w < widest) { - continue; - } - - Vector hmax; - hmax.resize(w); - for (int j = 0; j < w; j++) { - hmax.write[j] = 0; - } - - // Place them. - int ofs = 0; - int limit_h = 0; - for (int j = 0; j < wrects.size(); j++) { - if (ofs + wrects[j].s.width > w) { - ofs = 0; - } - - int from_y = 0; - for (int k = 0; k < wrects[j].s.width; k++) { - if (hmax[ofs + k] > from_y) { - from_y = hmax[ofs + k]; - } - } - - wrects.write[j].p.x = ofs; - wrects.write[j].p.y = from_y; - int end_h = from_y + wrects[j].s.height; - int end_w = ofs + wrects[j].s.width; - if (ofs == 0) { - limit_h = end_h; - } - - for (int k = 0; k < wrects[j].s.width; k++) { - hmax.write[ofs + k] = end_h; - } - - if (end_h > max_h) { - max_h = end_h; - } - - if (end_w > max_w) { - max_w = end_w; - } - - if (ofs == 0 || end_h > limit_h) { // While h limit not reached, keep stacking. - ofs += wrects[j].s.width; - } - } - - _AtlasWorkRectResult result; - result.result = wrects; - result.max_h = max_h; - result.max_w = max_w; - results.push_back(result); - } - - // Find the result with the best aspect ratio. - - int best = -1; - real_t best_aspect = 1e20; - - for (int i = 0; i < results.size(); i++) { - real_t h = next_power_of_2(results[i].max_h); - real_t w = next_power_of_2(results[i].max_w); - real_t aspect = h > w ? h / w : w / h; - if (aspect < best_aspect) { - best = i; - best_aspect = aspect; - } - } - - r_result.resize(p_rects.size()); - - for (int i = 0; i < p_rects.size(); i++) { - r_result.write[results[best].result[i].idx] = results[best].result[i].p; - } - - r_size = Size2(results[best].max_w, results[best].max_h); -} - -Vector> Geometry::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector &p_polypath_a, const Vector &p_polypath_b, bool is_a_open) { - using namespace ClipperLib; - - ClipType op = ctUnion; - - switch (p_op) { - case OPERATION_UNION: - op = ctUnion; - break; - case OPERATION_DIFFERENCE: - op = ctDifference; - break; - case OPERATION_INTERSECTION: - op = ctIntersection; - break; - case OPERATION_XOR: - op = ctXor; - break; - } - Path path_a, path_b; - - // Need to scale points (Clipper's requirement for robust computation). - for (int i = 0; i != p_polypath_a.size(); ++i) { - path_a << IntPoint(p_polypath_a[i].x * SCALE_FACTOR, p_polypath_a[i].y * SCALE_FACTOR); - } - for (int i = 0; i != p_polypath_b.size(); ++i) { - path_b << IntPoint(p_polypath_b[i].x * SCALE_FACTOR, p_polypath_b[i].y * SCALE_FACTOR); - } - Clipper clp; - clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. - clp.AddPath(path_b, ptClip, true); // Polylines cannot be set as clip. - - Paths paths; - - if (is_a_open) { - PolyTree tree; // Needed to populate polylines. - clp.Execute(op, tree); - OpenPathsFromPolyTree(tree, paths); - } else { - clp.Execute(op, paths); // Works on closed polygons only. - } - // Have to scale points down now. - Vector> polypaths; - - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector polypath; - - const Path &scaled_path = paths[i]; - - for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { - polypath.push_back(Point2( - static_cast(scaled_path[j].X) / SCALE_FACTOR, - static_cast(scaled_path[j].Y) / SCALE_FACTOR)); - } - polypaths.push_back(polypath); - } - return polypaths; -} - -Vector> Geometry::_polypath_offset(const Vector &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { - using namespace ClipperLib; - - JoinType jt = jtSquare; - - switch (p_join_type) { - case JOIN_SQUARE: - jt = jtSquare; - break; - case JOIN_ROUND: - jt = jtRound; - break; - case JOIN_MITER: - jt = jtMiter; - break; - } - - EndType et = etClosedPolygon; - - switch (p_end_type) { - case END_POLYGON: - et = etClosedPolygon; - break; - case END_JOINED: - et = etClosedLine; - break; - case END_BUTT: - et = etOpenButt; - break; - case END_SQUARE: - et = etOpenSquare; - break; - case END_ROUND: - et = etOpenRound; - break; - } - ClipperOffset co(2.0, 0.25 * SCALE_FACTOR); // Defaults from ClipperOffset. - Path path; - - // Need to scale points (Clipper's requirement for robust computation). - for (int i = 0; i != p_polypath.size(); ++i) { - path << IntPoint(p_polypath[i].x * SCALE_FACTOR, p_polypath[i].y * SCALE_FACTOR); - } - co.AddPath(path, jt, et); - - Paths paths; - co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. - - // Have to scale points down now. - Vector> polypaths; - - for (Paths::size_type i = 0; i < paths.size(); ++i) { - Vector polypath; - - const Path &scaled_path = paths[i]; - - for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { - polypath.push_back(Point2( - static_cast(scaled_path[j].X) / SCALE_FACTOR, - static_cast(scaled_path[j].Y) / SCALE_FACTOR)); - } - polypaths.push_back(polypath); - } - return polypaths; -} - -Vector Geometry::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { - Vector points; - - // Iterate through every unique combination of any three planes. - for (int i = p_plane_count - 1; i >= 0; i--) { - for (int j = i - 1; j >= 0; j--) { - for (int k = j - 1; k >= 0; k--) { - // Find the point where these planes all cross over (if they - // do at all). - Vector3 convex_shape_point; - if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) { - // See if any *other* plane excludes this point because it's - // on the wrong side. - bool excluded = false; - for (int n = 0; n < p_plane_count; n++) { - if (n != i && n != j && n != k) { - real_t dp = p_planes[n].normal.dot(convex_shape_point); - if (dp - p_planes[n].d > CMP_EPSILON) { - excluded = true; - break; - } - } - } - - // Only add the point if it passed all tests. - if (!excluded) { - points.push_back(convex_shape_point); - } - } - } - } - } - - return points; -} - -Vector Geometry::pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size) { - Vector nodes; - nodes.resize(p_atlas_size.width); - - stbrp_context context; - stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); - - Vector rects; - rects.resize(p_sizes.size()); - - for (int i = 0; i < p_sizes.size(); i++) { - rects.write[i].id = 0; - rects.write[i].w = p_sizes[i].width; - rects.write[i].h = p_sizes[i].height; - rects.write[i].x = 0; - rects.write[i].y = 0; - rects.write[i].was_packed = 0; - } - - int res = stbrp_pack_rects(&context, rects.ptrw(), rects.size()); - if (res == 0) { //pack failed - return Vector(); - } - - Vector ret; - ret.resize(p_sizes.size()); - - for (int i = 0; i < p_sizes.size(); i++) { - Point2i r(rects[i].x, rects[i].y); - ret.write[i] = r; - } - - return ret; -} - -Vector Geometry::partial_pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size) { - Vector nodes; - nodes.resize(p_atlas_size.width); - zeromem(nodes.ptrw(), sizeof(stbrp_node) * nodes.size()); - - stbrp_context context; - stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); - - Vector rects; - rects.resize(p_sizes.size()); - - for (int i = 0; i < p_sizes.size(); i++) { - rects.write[i].id = i; - rects.write[i].w = p_sizes[i].width; - rects.write[i].h = p_sizes[i].height; - rects.write[i].x = 0; - rects.write[i].y = 0; - rects.write[i].was_packed = 0; - } - - stbrp_pack_rects(&context, rects.ptrw(), rects.size()); - - Vector ret; - ret.resize(p_sizes.size()); - - for (int i = 0; i < p_sizes.size(); i++) { - ret.write[rects[i].id] = Vector3i(rects[i].x, rects[i].y, rects[i].was_packed != 0 ? 1 : 0); - } - - return ret; -} - -#define square(m_s) ((m_s) * (m_s)) -#define INF 1e20 - -/* dt of 1d function using squared distance */ -static void edt(float *f, int stride, int n) { - float *d = (float *)alloca(sizeof(float) * n + sizeof(int) * n + sizeof(float) * (n + 1)); - int *v = (int *)&(d[n]); - float *z = (float *)&v[n]; - - int k = 0; - v[0] = 0; - z[0] = -INF; - z[1] = +INF; - for (int q = 1; q <= n - 1; q++) { - float s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); - while (s <= z[k]) { - k--; - s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); - } - k++; - v[k] = q; - - z[k] = s; - z[k + 1] = +INF; - } - - k = 0; - for (int q = 0; q <= n - 1; q++) { - while (z[k + 1] < q) { - k++; - } - d[q] = square(q - v[k]) + f[v[k] * stride]; - } - - for (int i = 0; i < n; i++) { - f[i * stride] = d[i]; - } -} - -#undef square - -Vector Geometry::generate_edf(const Vector &p_voxels, const Vector3i &p_size, bool p_negative) { - uint32_t float_count = p_size.x * p_size.y * p_size.z; - - ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector()); - - float *work_memory = memnew_arr(float, float_count); - for (uint32_t i = 0; i < float_count; i++) { - work_memory[i] = INF; - } - - uint32_t y_mult = p_size.x; - uint32_t z_mult = y_mult * p_size.y; - - //plot solid cells - { - const bool *voxr = p_voxels.ptr(); - for (uint32_t i = 0; i < float_count; i++) { - bool plot = voxr[i]; - if (p_negative) { - plot = !plot; - } - if (plot) { - work_memory[i] = 0; - } - } - } - - //process in each direction - - //xy->z - - for (int i = 0; i < p_size.x; i++) { - for (int j = 0; j < p_size.y; j++) { - edt(&work_memory[i + j * y_mult], z_mult, p_size.z); - } - } - - //xz->y - - for (int i = 0; i < p_size.x; i++) { - for (int j = 0; j < p_size.z; j++) { - edt(&work_memory[i + j * z_mult], y_mult, p_size.y); - } - } - - //yz->x - for (int i = 0; i < p_size.y; i++) { - for (int j = 0; j < p_size.z; j++) { - edt(&work_memory[i * y_mult + j * z_mult], 1, p_size.x); - } - } - - Vector ret; - ret.resize(float_count); - { - uint32_t *w = ret.ptrw(); - for (uint32_t i = 0; i < float_count; i++) { - w[i] = uint32_t(Math::sqrt(work_memory[i])); - } - } - - return ret; -} - -Vector Geometry::generate_sdf8(const Vector &p_positive, const Vector &p_negative) { - ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector()); - Vector sdf8; - int s = p_positive.size(); - sdf8.resize(s); - - const uint32_t *rpos = p_positive.ptr(); - const uint32_t *rneg = p_negative.ptr(); - int8_t *wsdf = sdf8.ptrw(); - for (int i = 0; i < s; i++) { - int32_t diff = int32_t(rpos[i]) - int32_t(rneg[i]); - wsdf[i] = CLAMP(diff, -128, 127); - } - return sdf8; -} diff --git a/core/math/geometry.h b/core/math/geometry.h deleted file mode 100644 index a61bf20c4c..0000000000 --- a/core/math/geometry.h +++ /dev/null @@ -1,1310 +0,0 @@ -/*************************************************************************/ -/* geometry.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef GEOMETRY_H -#define GEOMETRY_H - -#include "core/math/delaunay_2d.h" -#include "core/math/face3.h" -#include "core/math/rect2.h" -#include "core/math/triangulate.h" -#include "core/math/vector3.h" -#include "core/object.h" -#include "core/print_string.h" -#include "core/vector.h" - -class Geometry { - Geometry(); - -public: - static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) { - Vector2 d1 = q1 - p1; // Direction vector of segment S1. - Vector2 d2 = q2 - p2; // Direction vector of segment S2. - Vector2 r = p1 - p2; - real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative. - real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative. - real_t f = d2.dot(r); - real_t s, t; - // Check if either or both segments degenerate into points. - if (a <= CMP_EPSILON && e <= CMP_EPSILON) { - // Both segments degenerate into points. - c1 = p1; - c2 = p2; - return Math::sqrt((c1 - c2).dot(c1 - c2)); - } - if (a <= CMP_EPSILON) { - // First segment degenerates into a point. - s = 0.0; - t = f / e; // s = 0 => t = (b*s + f) / e = f / e - t = CLAMP(t, 0.0, 1.0); - } else { - real_t c = d1.dot(r); - if (e <= CMP_EPSILON) { - // Second segment degenerates into a point. - t = 0.0; - s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a - } else { - // The general nondegenerate case starts here. - real_t b = d1.dot(d2); - real_t denom = a * e - b * b; // Always nonnegative. - // If segments not parallel, compute closest point on L1 to L2 and - // clamp to segment S1. Else pick arbitrary s (here 0). - if (denom != 0.0) { - s = CLAMP((b * f - c * e) / denom, 0.0, 1.0); - } else { - s = 0.0; - } - // Compute point on L2 closest to S1(s) using - // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e - t = (b * s + f) / e; - - //If t in [0,1] done. Else clamp t, recompute s for the new value - // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a - // and clamp s to [0, 1]. - if (t < 0.0) { - t = 0.0; - s = CLAMP(-c / a, 0.0, 1.0); - } else if (t > 1.0) { - t = 1.0; - s = CLAMP((b - c) / a, 0.0, 1.0); - } - } - } - c1 = p1 + d1 * s; - c2 = p2 + d2 * t; - return Math::sqrt((c1 - c2).dot(c1 - c2)); - } - - static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { -// Do the function 'd' as defined by pb. I think is is dot product of some sort. -#define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) - - // Calculate the parametric position on the 2 curves, mua and mub. - real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); - real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); - - // Clip the value between [0..1] constraining the solution to lie on the original curves. - if (mua < 0) { - mua = 0; - } - if (mub < 0) { - mub = 0; - } - if (mua > 1) { - mua = 1; - } - if (mub > 1) { - mub = 1; - } - c1 = p1.lerp(p2, mua); - c2 = q1.lerp(q2, mub); - } - - static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { - Vector3 u = p_to_a - p_from_a; - Vector3 v = p_to_b - p_from_b; - Vector3 w = p_from_a - p_to_a; - real_t a = u.dot(u); // Always >= 0 - real_t b = u.dot(v); - real_t c = v.dot(v); // Always >= 0 - real_t d = u.dot(w); - real_t e = v.dot(w); - real_t D = a * c - b * b; // Always >= 0 - real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 - real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - - // Compute the line parameters of the two closest points. - if (D < CMP_EPSILON) { // The lines are almost parallel. - sN = 0.0; // Force using point P0 on segment S1 - sD = 1.0; // to prevent possible division by 0.0 later. - tN = e; - tD = c; - } else { // Get the closest points on the infinite lines - sN = (b * e - c * d); - tN = (a * e - b * d); - if (sN < 0.0) { // sc < 0 => the s=0 edge is visible. - sN = 0.0; - tN = e; - tD = c; - } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. - tN = 0.0; - // Recompute sc for this edge. - if (-d < 0.0) { - sN = 0.0; - } else if (-d > a) { - sN = sD; - } else { - sN = -d; - sD = a; - } - } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. - tN = tD; - // Recompute sc for this edge. - if ((-d + b) < 0.0) { - sN = 0; - } else if ((-d + b) > a) { - sN = sD; - } else { - sN = (-d + b); - sD = a; - } - } - // Finally do the division to get sc and tc. - sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD); - tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD); - - // Get the difference of the two closest points. - Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - - return dP.length(); // Return the closest distance. - } - - static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { - Vector3 e1 = p_v1 - p_v0; - Vector3 e2 = p_v2 - p_v0; - Vector3 h = p_dir.cross(e2); - real_t a = e1.dot(h); - if (Math::is_zero_approx(a)) { // Parallel test. - return false; - } - - real_t f = 1.0 / a; - - Vector3 s = p_from - p_v0; - real_t u = f * s.dot(h); - - if (u < 0.0 || u > 1.0) { - return false; - } - - Vector3 q = s.cross(e1); - - real_t v = f * p_dir.dot(q); - - if (v < 0.0 || u + v > 1.0) { - return false; - } - - // At this stage we can compute t to find out where - // the intersection point is on the line. - real_t t = f * e2.dot(q); - - if (t > 0.00001) { // ray intersection - if (r_res) { - *r_res = p_from + p_dir * t; - } - return true; - } else { // This means that there is a line intersection but not a ray intersection. - return false; - } - } - - static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { - Vector3 rel = p_to - p_from; - Vector3 e1 = p_v1 - p_v0; - Vector3 e2 = p_v2 - p_v0; - Vector3 h = rel.cross(e2); - real_t a = e1.dot(h); - if (Math::is_zero_approx(a)) { // Parallel test. - return false; - } - - real_t f = 1.0 / a; - - Vector3 s = p_from - p_v0; - real_t u = f * s.dot(h); - - if (u < 0.0 || u > 1.0) { - return false; - } - - Vector3 q = s.cross(e1); - - real_t v = f * rel.dot(q); - - if (v < 0.0 || u + v > 1.0) { - return false; - } - - // At this stage we can compute t to find out where - // the intersection point is on the line. - real_t t = f * e2.dot(q); - - if (t > CMP_EPSILON && t <= 1.0) { // Ray intersection. - if (r_res) { - *r_res = p_from + rel * t; - } - return true; - } else { // This means that there is a line intersection but not a ray intersection. - return false; - } - } - - static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { - Vector3 sphere_pos = p_sphere_pos - p_from; - Vector3 rel = (p_to - p_from); - real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) { - return false; // Both points are the same. - } - Vector3 normal = rel / rel_l; - - real_t sphere_d = normal.dot(sphere_pos); - - real_t ray_distance = sphere_pos.distance_to(normal * sphere_d); - - if (ray_distance >= p_sphere_radius) { - return false; - } - - real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance; - real_t inters_d = sphere_d; - - if (inters_d2 >= CMP_EPSILON) { - inters_d -= Math::sqrt(inters_d2); - } - - // Check in segment. - if (inters_d < 0 || inters_d > rel_l) { - return false; - } - - Vector3 result = p_from + normal * inters_d; - - if (r_res) { - *r_res = result; - } - if (r_norm) { - *r_norm = (result - p_sphere_pos).normalized(); - } - - return true; - } - - static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { - Vector3 rel = (p_to - p_from); - real_t rel_l = rel.length(); - if (rel_l < CMP_EPSILON) { - return false; // Both points are the same. - } - - // First check if they are parallel. - Vector3 normal = (rel / rel_l); - Vector3 crs = normal.cross(Vector3(0, 0, 1)); - real_t crs_l = crs.length(); - - Vector3 z_dir; - - if (crs_l < CMP_EPSILON) { - z_dir = Vector3(1, 0, 0); // Any x/y vector OK. - } else { - z_dir = crs / crs_l; - } - - real_t dist = z_dir.dot(p_from); - - if (dist >= p_radius) { - return false; // Too far away. - } - - // Convert to 2D. - real_t w2 = p_radius * p_radius - dist * dist; - if (w2 < CMP_EPSILON) { - return false; // Avoid numerical error. - } - Size2 size(Math::sqrt(w2), p_height * 0.5); - - Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized(); - - Vector2 from2D(x_dir.dot(p_from), p_from.z); - Vector2 to2D(x_dir.dot(p_to), p_to.z); - - real_t min = 0, max = 1; - - int axis = -1; - - for (int i = 0; i < 2; i++) { - real_t seg_from = from2D[i]; - real_t seg_to = to2D[i]; - real_t box_begin = -size[i]; - real_t box_end = size[i]; - real_t cmin, cmax; - - if (seg_from < seg_to) { - if (seg_from > box_end || seg_to < box_begin) { - return false; - } - real_t length = seg_to - seg_from; - cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; - cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; - - } else { - if (seg_to > box_end || seg_from < box_begin) { - return false; - } - real_t length = seg_to - seg_from; - cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; - cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; - } - - if (cmin > min) { - min = cmin; - axis = i; - } - if (cmax < max) { - max = cmax; - } - if (max < min) { - return false; - } - } - - // Convert to 3D again. - Vector3 result = p_from + (rel * min); - Vector3 res_normal = result; - - if (axis == 0) { - res_normal.z = 0; - } else { - res_normal.x = 0; - res_normal.y = 0; - } - - res_normal.normalize(); - - if (r_res) { - *r_res = result; - } - if (r_norm) { - *r_norm = res_normal; - } - - return true; - } - - static bool segment_intersects_convex(const Vector3 &p_from, const Vector3 &p_to, const Plane *p_planes, int p_plane_count, Vector3 *p_res, Vector3 *p_norm) { - real_t min = -1e20, max = 1e20; - - Vector3 rel = p_to - p_from; - real_t rel_l = rel.length(); - - if (rel_l < CMP_EPSILON) { - return false; - } - - Vector3 dir = rel / rel_l; - - int min_index = -1; - - for (int i = 0; i < p_plane_count; i++) { - const Plane &p = p_planes[i]; - - real_t den = p.normal.dot(dir); - - if (Math::abs(den) <= CMP_EPSILON) { - continue; // Ignore parallel plane. - } - - real_t dist = -p.distance_to(p_from) / den; - - if (den > 0) { - // Backwards facing plane. - if (dist < max) { - max = dist; - } - } else { - // Front facing plane. - if (dist > min) { - min = dist; - min_index = i; - } - } - } - - if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions. - return false; // No intersection. - } - - if (p_res) { - *p_res = p_from + dir * min; - } - if (p_norm) { - *p_norm = p_planes[min_index].normal; - } - - return true; - } - - static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 *p_segment) { - Vector3 p = p_point - p_segment[0]; - Vector3 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) { - return p_segment[0]; // Both points are the same, just give any. - } - - real_t d = n.dot(p) / l2; - - if (d <= 0.0) { - return p_segment[0]; // Before first point. - } else if (d >= 1.0) { - return p_segment[1]; // After first point. - } else { - return p_segment[0] + n * d; // Inside. - } - } - - static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) { - Vector3 p = p_point - p_segment[0]; - Vector3 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) { - return p_segment[0]; // Both points are the same, just give any. - } - - real_t d = n.dot(p) / l2; - - return p_segment[0] + n * d; // Inside. - } - - static Vector2 get_closest_point_to_segment_2d(const Vector2 &p_point, const Vector2 *p_segment) { - Vector2 p = p_point - p_segment[0]; - Vector2 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) { - return p_segment[0]; // Both points are the same, just give any. - } - - real_t d = n.dot(p) / l2; - - if (d <= 0.0) { - return p_segment[0]; // Before first point. - } else if (d >= 1.0) { - return p_segment[1]; // After first point. - } else { - return p_segment[0] + n * d; // Inside. - } - } - - static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { - Vector2 an = a - s; - Vector2 bn = b - s; - Vector2 cn = c - s; - - bool orientation = an.cross(bn) > 0; - - if ((bn.cross(cn) > 0) != orientation) { - return false; - } - - return (cn.cross(an) > 0) == orientation; - } - - static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2 &p_point, const Vector2 *p_segment) { - Vector2 p = p_point - p_segment[0]; - Vector2 n = p_segment[1] - p_segment[0]; - real_t l2 = n.length_squared(); - if (l2 < 1e-20) { - return p_segment[0]; // Both points are the same, just give any. - } - - real_t d = n.dot(p) / l2; - - return p_segment[0] + n * d; // Inside. - } - - static bool line_intersects_line_2d(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) { - // See http://paulbourke.net/geometry/pointlineplane/ - - const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y; - if (Math::is_zero_approx(denom)) { // Parallel? - return false; - } - - const Vector2 v = p_from_a - p_from_b; - const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom; - r_result = p_from_a + t * p_dir_a; - return true; - } - - static bool segment_intersects_segment_2d(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) { - Vector2 B = p_to_a - p_from_a; - Vector2 C = p_from_b - p_from_a; - Vector2 D = p_to_b - p_from_a; - - real_t ABlen = B.dot(B); - if (ABlen <= 0) { - return false; - } - Vector2 Bn = B / ABlen; - C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y); - D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y); - - if ((C.y < 0 && D.y < 0) || (C.y >= 0 && D.y >= 0)) { - return false; - } - - real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y); - - // Fail if segment C-D crosses line A-B outside of segment A-B. - if (ABpos < 0 || ABpos > 1.0) { - return false; - } - - // (4) Apply the discovered position to line A-B in the original coordinate system. - if (r_result) { - *r_result = p_from_a + B * ABpos; - } - - return true; - } - - static inline bool point_in_projected_triangle(const Vector3 &p_point, const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) { - Vector3 face_n = (p_v1 - p_v3).cross(p_v1 - p_v2); - - Vector3 n1 = (p_point - p_v3).cross(p_point - p_v2); - - if (face_n.dot(n1) < 0) { - return false; - } - - Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point); - - if (face_n.dot(n2) < 0) { - return false; - } - - Vector3 n3 = (p_v1 - p_point).cross(p_v1 - p_v2); - - if (face_n.dot(n3) < 0) { - return false; - } - - return true; - } - - static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) { - real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle[0]); - - if (d > p_sphere_radius || d < -p_sphere_radius) { - // Not touching the plane of the face, return. - return false; - } - - Vector3 contact = p_sphere_pos - (p_normal * d); - - /** 2nd) TEST INSIDE TRIANGLE **/ - - if (Geometry::point_in_projected_triangle(contact, p_triangle[0], p_triangle[1], p_triangle[2])) { - r_triangle_contact = contact; - r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius; - //printf("solved inside triangle\n"); - return true; - } - - /** 3rd TEST INSIDE EDGE CYLINDERS **/ - - const Vector3 verts[4] = { p_triangle[0], p_triangle[1], p_triangle[2], p_triangle[0] }; // for() friendly - - for (int i = 0; i < 3; i++) { - // Check edge cylinder. - - Vector3 n1 = verts[i] - verts[i + 1]; - Vector3 n2 = p_sphere_pos - verts[i + 1]; - - ///@TODO Maybe discard by range here to make the algorithm quicker. - - // Check point within cylinder radius. - Vector3 axis = n1.cross(n2).cross(n1); - axis.normalize(); - - real_t ad = axis.dot(n2); - - if (ABS(ad) > p_sphere_radius) { - // No chance with this edge, too far away. - continue; - } - - // Check point within edge capsule cylinder. - /** 4th TEST INSIDE EDGE POINTS **/ - - real_t sphere_at = n1.dot(n2); - - if (sphere_at >= 0 && sphere_at < n1.dot(n1)) { - r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2)); - r_sphere_contact = p_sphere_pos - axis * p_sphere_radius; - // Point inside here. - return true; - } - - real_t r2 = p_sphere_radius * p_sphere_radius; - - if (n2.length_squared() < r2) { - Vector3 n = (p_sphere_pos - verts[i + 1]).normalized(); - - r_triangle_contact = verts[i + 1]; - r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - return true; - } - - if (n2.distance_squared_to(n1) < r2) { - Vector3 n = (p_sphere_pos - verts[i]).normalized(); - - r_triangle_contact = verts[i]; - r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - return true; - } - - break; // It's pointless to continue at this point, so save some CPU cycles. - } - - return false; - } - - static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) { - return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius; - } - - static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) { - Vector2 line_vec = p_to - p_from; - Vector2 vec_to_line = p_from - p_circle_pos; - - // Create a quadratic formula of the form ax^2 + bx + c = 0 - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * vec_to_line.dot(line_vec); - c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius; - - // Solve for t. - real_t sqrtterm = b * b - 4 * a * c; - - // If the term we intend to square root is less than 0 then the answer won't be real, - // so it definitely won't be t in the range 0 to 1. - if (sqrtterm < 0) { - return -1; - } - - // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) - // then the following can be skipped and we can just return the equivalent of res1. - sqrtterm = Math::sqrt(sqrtterm); - real_t res1 = (-b - sqrtterm) / (2 * a); - real_t res2 = (-b + sqrtterm) / (2 * a); - - if (res1 >= 0 && res1 <= 1) { - return res1; - } - if (res2 >= 0 && res2 <= 1) { - return res2; - } - return -1; - } - - static inline Vector clip_polygon(const Vector &polygon, const Plane &p_plane) { - enum LocationCache { - LOC_INSIDE = 1, - LOC_BOUNDARY = 0, - LOC_OUTSIDE = -1 - }; - - if (polygon.size() == 0) { - return polygon; - } - - int *location_cache = (int *)alloca(sizeof(int) * polygon.size()); - int inside_count = 0; - int outside_count = 0; - - for (int a = 0; a < polygon.size(); a++) { - real_t dist = p_plane.distance_to(polygon[a]); - if (dist < -CMP_POINT_IN_PLANE_EPSILON) { - location_cache[a] = LOC_INSIDE; - inside_count++; - } else { - if (dist > CMP_POINT_IN_PLANE_EPSILON) { - location_cache[a] = LOC_OUTSIDE; - outside_count++; - } else { - location_cache[a] = LOC_BOUNDARY; - } - } - } - - if (outside_count == 0) { - return polygon; // No changes. - } else if (inside_count == 0) { - return Vector(); // Empty. - } - - long previous = polygon.size() - 1; - Vector clipped; - - for (int index = 0; index < polygon.size(); index++) { - int loc = location_cache[index]; - if (loc == LOC_OUTSIDE) { - if (location_cache[previous] == LOC_INSIDE) { - const Vector3 &v1 = polygon[previous]; - const Vector3 &v2 = polygon[index]; - - Vector3 segment = v1 - v2; - real_t den = p_plane.normal.dot(segment); - real_t dist = p_plane.distance_to(v1) / den; - dist = -dist; - clipped.push_back(v1 + segment * dist); - } - } else { - const Vector3 &v1 = polygon[index]; - if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) { - const Vector3 &v2 = polygon[previous]; - Vector3 segment = v1 - v2; - real_t den = p_plane.normal.dot(segment); - real_t dist = p_plane.distance_to(v1) / den; - dist = -dist; - clipped.push_back(v1 + segment * dist); - } - - clipped.push_back(v1); - } - - previous = index; - } - - return clipped; - } - - enum PolyBooleanOperation { - OPERATION_UNION, - OPERATION_DIFFERENCE, - OPERATION_INTERSECTION, - OPERATION_XOR - }; - enum PolyJoinType { - JOIN_SQUARE, - JOIN_ROUND, - JOIN_MITER - }; - enum PolyEndType { - END_POLYGON, - END_JOINED, - END_BUTT, - END_SQUARE, - END_ROUND - }; - - static Vector> merge_polygons_2d(const Vector &p_polygon_a, const Vector &p_polygon_b) { - return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b); - } - - static Vector> clip_polygons_2d(const Vector &p_polygon_a, const Vector &p_polygon_b) { - return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b); - } - - static Vector> intersect_polygons_2d(const Vector &p_polygon_a, const Vector &p_polygon_b) { - return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b); - } - - static Vector> exclude_polygons_2d(const Vector &p_polygon_a, const Vector &p_polygon_b) { - return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b); - } - - static Vector> clip_polyline_with_polygon_2d(const Vector &p_polyline, const Vector &p_polygon) { - return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true); - } - - static Vector> intersect_polyline_with_polygon_2d(const Vector &p_polyline, const Vector &p_polygon) { - return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true); - } - - static Vector> offset_polygon_2d(const Vector &p_polygon, real_t p_delta, PolyJoinType p_join_type) { - return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON); - } - - static Vector> offset_polyline_2d(const Vector &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { - ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector>(), "Attempt to offset a polyline like a polygon (use offset_polygon_2d instead)."); - - return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); - } - - static Vector triangulate_delaunay_2d(const Vector &p_points) { - Vector tr = Delaunay2D::triangulate(p_points); - Vector triangles; - - for (int i = 0; i < tr.size(); i++) { - triangles.push_back(tr[i].points[0]); - triangles.push_back(tr[i].points[1]); - triangles.push_back(tr[i].points[2]); - } - return triangles; - } - - static Vector triangulate_polygon(const Vector &p_polygon) { - Vector triangles; - if (!Triangulate::triangulate(p_polygon, triangles)) { - return Vector(); //fail - } - return triangles; - } - - static bool is_polygon_clockwise(const Vector &p_polygon) { - int c = p_polygon.size(); - if (c < 3) { - return false; - } - const Vector2 *p = p_polygon.ptr(); - real_t sum = 0; - for (int i = 0; i < c; i++) { - const Vector2 &v1 = p[i]; - const Vector2 &v2 = p[(i + 1) % c]; - sum += (v2.x - v1.x) * (v2.y + v1.y); - } - - return sum > 0.0f; - } - - // Alternate implementation that should be faster. - static bool is_point_in_polygon(const Vector2 &p_point, const Vector &p_polygon) { - int c = p_polygon.size(); - if (c < 3) { - return false; - } - const Vector2 *p = p_polygon.ptr(); - Vector2 further_away(-1e20, -1e20); - Vector2 further_away_opposite(1e20, 1e20); - - for (int i = 0; i < c; i++) { - further_away.x = MAX(p[i].x, further_away.x); - further_away.y = MAX(p[i].y, further_away.y); - further_away_opposite.x = MIN(p[i].x, further_away_opposite.x); - further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); - } - - // Make point outside that won't intersect with points in segment from p_point. - further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); - - int intersections = 0; - for (int i = 0; i < c; i++) { - const Vector2 &v1 = p[i]; - const Vector2 &v2 = p[(i + 1) % c]; - if (segment_intersects_segment_2d(v1, v2, p_point, further_away, nullptr)) { - intersections++; - } - } - - return (intersections & 1); - } - - static Vector> separate_objects(Vector p_array); - - // Create a "wrap" that encloses the given geometry. - static Vector wrap_geometry(Vector p_array, real_t *p_error = nullptr); - - struct MeshData { - struct Face { - Plane plane; - Vector indices; - }; - - Vector faces; - - struct Edge { - int a, b; - }; - - Vector edges; - - Vector vertices; - - void optimize_vertices(); - }; - - _FORCE_INLINE_ static int get_uv84_normal_bit(const Vector3 &p_vector) { - int lat = Math::fast_ftoi(Math::floor(Math::acos(p_vector.dot(Vector3(0, 1, 0))) * 4.0 / Math_PI + 0.5)); - - if (lat == 0) { - return 24; - } else if (lat == 4) { - return 25; - } - - int lon = Math::fast_ftoi(Math::floor((Math_PI + Math::atan2(p_vector.x, p_vector.z)) * 8.0 / (Math_PI * 2.0) + 0.5)) % 8; - - return lon + (lat - 1) * 8; - } - - _FORCE_INLINE_ static int get_uv84_normal_bit_neighbors(int p_idx) { - if (p_idx == 24) { - return 1 | 2 | 4 | 8; - } else if (p_idx == 25) { - return (1 << 23) | (1 << 22) | (1 << 21) | (1 << 20); - } else { - int ret = 0; - if ((p_idx % 8) == 0) { - ret |= (1 << (p_idx + 7)); - } else { - ret |= (1 << (p_idx - 1)); - } - if ((p_idx % 8) == 7) { - ret |= (1 << (p_idx - 7)); - } else { - ret |= (1 << (p_idx + 1)); - } - - int mask = ret | (1 << p_idx); - if (p_idx < 8) { - ret |= 24; - } else { - ret |= mask >> 8; - } - - if (p_idx >= 16) { - ret |= 25; - } else { - ret |= mask << 8; - } - - return ret; - } - } - - static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) { - return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x); - } - - // Returns a list of points on the convex hull in counter-clockwise order. - // Note: the last point in the returned list is the same as the first one. - static Vector convex_hull_2d(Vector P) { - int n = P.size(), k = 0; - Vector H; - H.resize(2 * n); - - // Sort points lexicographically. - P.sort(); - - // Build lower hull. - for (int i = 0; i < n; ++i) { - while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { - k--; - } - H.write[k++] = P[i]; - } - - // Build upper hull. - for (int i = n - 2, t = k + 1; i >= 0; i--) { - while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { - k--; - } - H.write[k++] = P[i]; - } - - H.resize(k); - return H; - } - static Vector> decompose_polygon_in_convex(Vector polygon); - - static MeshData build_convex_mesh(const Vector &p_planes); - static Vector build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); - static Vector build_box_planes(const Vector3 &p_extents); - static Vector build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z); - static Vector build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z); - - static void make_atlas(const Vector &p_rects, Vector &r_result, Size2i &r_size); - - static Vector compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); - -#define FINDMINMAX(x0, x1, x2, min, max) \ - min = max = x0; \ - if (x1 < min) { \ - min = x1; \ - } \ - if (x1 > max) { \ - max = x1; \ - } \ - if (x2 < min) { \ - min = x2; \ - } \ - if (x2 > max) { \ - max = x2; \ - } - - _FORCE_INLINE_ static bool planeBoxOverlap(Vector3 normal, float d, Vector3 maxbox) { - int q; - Vector3 vmin, vmax; - for (q = 0; q <= 2; q++) { - if (normal[q] > 0.0f) { - vmin[q] = -maxbox[q]; - vmax[q] = maxbox[q]; - } else { - vmin[q] = maxbox[q]; - vmax[q] = -maxbox[q]; - } - } - if (normal.dot(vmin) + d > 0.0f) { - return false; - } - if (normal.dot(vmax) + d >= 0.0f) { - return true; - } - - return false; - } - -/*======================== X-tests ========================*/ -#define AXISTEST_X01(a, b, fa, fb) \ - p0 = a * v0.y - b * v0.z; \ - p2 = a * v2.y - b * v2.z; \ - if (p0 < p2) { \ - min = p0; \ - max = p2; \ - } else { \ - min = p2; \ - max = p0; \ - } \ - rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ - if (min > rad || max < -rad) { \ - return false; \ - } - -#define AXISTEST_X2(a, b, fa, fb) \ - p0 = a * v0.y - b * v0.z; \ - p1 = a * v1.y - b * v1.z; \ - if (p0 < p1) { \ - min = p0; \ - max = p1; \ - } else { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ - if (min > rad || max < -rad) { \ - return false; \ - } - -/*======================== Y-tests ========================*/ -#define AXISTEST_Y02(a, b, fa, fb) \ - p0 = -a * v0.x + b * v0.z; \ - p2 = -a * v2.x + b * v2.z; \ - if (p0 < p2) { \ - min = p0; \ - max = p2; \ - } else { \ - min = p2; \ - max = p0; \ - } \ - rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ - if (min > rad || max < -rad) { \ - return false; \ - } - -#define AXISTEST_Y1(a, b, fa, fb) \ - p0 = -a * v0.x + b * v0.z; \ - p1 = -a * v1.x + b * v1.z; \ - if (p0 < p1) { \ - min = p0; \ - max = p1; \ - } else { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ - if (min > rad || max < -rad) { \ - return false; \ - } - - /*======================== Z-tests ========================*/ - -#define AXISTEST_Z12(a, b, fa, fb) \ - p1 = a * v1.x - b * v1.y; \ - p2 = a * v2.x - b * v2.y; \ - if (p2 < p1) { \ - min = p2; \ - max = p1; \ - } else { \ - min = p1; \ - max = p2; \ - } \ - rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ - if (min > rad || max < -rad) { \ - return false; \ - } - -#define AXISTEST_Z0(a, b, fa, fb) \ - p0 = a * v0.x - b * v0.y; \ - p1 = a * v1.x - b * v1.y; \ - if (p0 < p1) { \ - min = p0; \ - max = p1; \ - } else { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ - if (min > rad || max < -rad) { \ - return false; \ - } - - _FORCE_INLINE_ static bool triangle_box_overlap(const Vector3 &boxcenter, const Vector3 boxhalfsize, const Vector3 *triverts) { - /* use separating axis theorem to test overlap between triangle and box */ - /* need to test for overlap in these directions: */ - /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ - /* we do not even need to test these) */ - /* 2) normal of the triangle */ - /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ - /* this gives 3x3=9 more tests */ - Vector3 v0, v1, v2; - float min, max, d, p0, p1, p2, rad, fex, fey, fez; - Vector3 normal, e0, e1, e2; - - /* This is the fastest branch on Sun */ - /* move everything so that the boxcenter is in (0,0,0) */ - - v0 = triverts[0] - boxcenter; - v1 = triverts[1] - boxcenter; - v2 = triverts[2] - boxcenter; - - /* compute triangle edges */ - e0 = v1 - v0; /* tri edge 0 */ - e1 = v2 - v1; /* tri edge 1 */ - e2 = v0 - v2; /* tri edge 2 */ - - /* Bullet 3: */ - /* test the 9 tests first (this was faster) */ - fex = Math::abs(e0.x); - fey = Math::abs(e0.y); - fez = Math::abs(e0.z); - AXISTEST_X01(e0.z, e0.y, fez, fey); - AXISTEST_Y02(e0.z, e0.x, fez, fex); - AXISTEST_Z12(e0.y, e0.x, fey, fex); - - fex = Math::abs(e1.x); - fey = Math::abs(e1.y); - fez = Math::abs(e1.z); - AXISTEST_X01(e1.z, e1.y, fez, fey); - AXISTEST_Y02(e1.z, e1.x, fez, fex); - AXISTEST_Z0(e1.y, e1.x, fey, fex); - - fex = Math::abs(e2.x); - fey = Math::abs(e2.y); - fez = Math::abs(e2.z); - AXISTEST_X2(e2.z, e2.y, fez, fey); - AXISTEST_Y1(e2.z, e2.x, fez, fex); - AXISTEST_Z12(e2.y, e2.x, fey, fex); - - /* Bullet 1: */ - /* first test overlap in the {x,y,z}-directions */ - /* find min, max of the triangle each direction, and test for overlap in */ - /* that direction -- this is equivalent to testing a minimal AABB around */ - /* the triangle against the AABB */ - - /* test in X-direction */ - FINDMINMAX(v0.x, v1.x, v2.x, min, max); - if (min > boxhalfsize.x || max < -boxhalfsize.x) { - return false; - } - - /* test in Y-direction */ - FINDMINMAX(v0.y, v1.y, v2.y, min, max); - if (min > boxhalfsize.y || max < -boxhalfsize.y) { - return false; - } - - /* test in Z-direction */ - FINDMINMAX(v0.z, v1.z, v2.z, min, max); - if (min > boxhalfsize.z || max < -boxhalfsize.z) { - return false; - } - - /* Bullet 2: */ - /* test if the box intersects the plane of the triangle */ - /* compute plane equation of triangle: normal*x+d=0 */ - normal = e0.cross(e1); - d = -normal.dot(v0); /* plane eq: normal.x+d=0 */ - return planeBoxOverlap(normal, d, boxhalfsize); /* if true, box and triangle overlaps */ - } - - static Vector pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size); - static Vector partial_pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size); - - static Vector generate_edf(const Vector &p_voxels, const Vector3i &p_size, bool p_negative); - static Vector generate_sdf8(const Vector &p_positive, const Vector &p_negative); - - static Vector3 triangle_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_pos) { - Vector3 v0 = p_b - p_a; - Vector3 v1 = p_c - p_a; - Vector3 v2 = p_pos - p_a; - - float d00 = v0.dot(v0); - float d01 = v0.dot(v1); - float d11 = v1.dot(v1); - float d20 = v2.dot(v0); - float d21 = v2.dot(v1); - float denom = (d00 * d11 - d01 * d01); - if (denom == 0) { - return Vector3(); //invalid triangle, return empty - } - float v = (d11 * d20 - d01 * d21) / denom; - float w = (d00 * d21 - d01 * d20) / denom; - float u = 1.0f - v - w; - return Vector3(u, v, w); - } - - static Color tetrahedron_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_d, const Vector3 &p_pos) { - Vector3 vap = p_pos - p_a; - Vector3 vbp = p_pos - p_b; - - Vector3 vab = p_b - p_a; - Vector3 vac = p_c - p_a; - Vector3 vad = p_d - p_a; - - Vector3 vbc = p_c - p_b; - Vector3 vbd = p_d - p_b; - // ScTP computes the scalar triple product -#define STP(m_a, m_b, m_c) ((m_a).dot((m_b).cross((m_c)))) - float va6 = STP(vbp, vbd, vbc); - float vb6 = STP(vap, vac, vad); - float vc6 = STP(vap, vad, vab); - float vd6 = STP(vap, vab, vac); - float v6 = 1 / STP(vab, vac, vad); - return Color(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); -#undef STP - } - -private: - static Vector> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector &p_polypath_a, const Vector &p_polypath_b, bool is_a_open = false); - static Vector> _polypath_offset(const Vector &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); -}; - -#endif // GEOMETRY_H diff --git a/core/math/geometry_2d.cpp b/core/math/geometry_2d.cpp new file mode 100644 index 0000000000..7d8fde8bcc --- /dev/null +++ b/core/math/geometry_2d.cpp @@ -0,0 +1,384 @@ +/*************************************************************************/ +/* geometry.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "geometry_2d.h" + +#include "thirdparty/misc/clipper.hpp" +#include "thirdparty/misc/triangulator.h" +#define STB_RECT_PACK_IMPLEMENTATION +#include "thirdparty/misc/stb_rect_pack.h" + +#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. + +Vector> Geometry2D::decompose_polygon_in_convex(Vector polygon) { + Vector> decomp; + List in_poly, out_poly; + + TriangulatorPoly inp; + inp.Init(polygon.size()); + for (int i = 0; i < polygon.size(); i++) { + inp.GetPoint(i) = polygon[i]; + } + inp.SetOrientation(TRIANGULATOR_CCW); + in_poly.push_back(inp); + TriangulatorPartition tpart; + if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed. + ERR_PRINT("Convex decomposing failed!"); + return decomp; + } + + decomp.resize(out_poly.size()); + int idx = 0; + for (List::Element *I = out_poly.front(); I; I = I->next()) { + TriangulatorPoly &tp = I->get(); + + decomp.write[idx].resize(tp.GetNumPoints()); + + for (int64_t i = 0; i < tp.GetNumPoints(); i++) { + decomp.write[idx].write[i] = tp.GetPoint(i); + } + + idx++; + } + + return decomp; +} + +struct _AtlasWorkRect { + Size2i s; + Point2i p; + int idx; + _FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }; +}; + +struct _AtlasWorkRectResult { + Vector<_AtlasWorkRect> result; + int max_w; + int max_h; +}; + +void Geometry2D::make_atlas(const Vector &p_rects, Vector &r_result, Size2i &r_size) { + // Super simple, almost brute force scanline stacking fitter. + // It's pretty basic for now, but it tries to make sure that the aspect ratio of the + // resulting atlas is somehow square. This is necessary because video cards have limits. + // On texture size (usually 2048 or 4096), so the more square a texture, the more chances. + // It will work in every hardware. + // For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a + // 256x8192 atlas (won't work anywhere). + + ERR_FAIL_COND(p_rects.size() == 0); + + Vector<_AtlasWorkRect> wrects; + wrects.resize(p_rects.size()); + for (int i = 0; i < p_rects.size(); i++) { + wrects.write[i].s = p_rects[i]; + wrects.write[i].idx = i; + } + wrects.sort(); + int widest = wrects[0].s.width; + + Vector<_AtlasWorkRectResult> results; + + for (int i = 0; i <= 12; i++) { + int w = 1 << i; + int max_h = 0; + int max_w = 0; + if (w < widest) { + continue; + } + + Vector hmax; + hmax.resize(w); + for (int j = 0; j < w; j++) { + hmax.write[j] = 0; + } + + // Place them. + int ofs = 0; + int limit_h = 0; + for (int j = 0; j < wrects.size(); j++) { + if (ofs + wrects[j].s.width > w) { + ofs = 0; + } + + int from_y = 0; + for (int k = 0; k < wrects[j].s.width; k++) { + if (hmax[ofs + k] > from_y) { + from_y = hmax[ofs + k]; + } + } + + wrects.write[j].p.x = ofs; + wrects.write[j].p.y = from_y; + int end_h = from_y + wrects[j].s.height; + int end_w = ofs + wrects[j].s.width; + if (ofs == 0) { + limit_h = end_h; + } + + for (int k = 0; k < wrects[j].s.width; k++) { + hmax.write[ofs + k] = end_h; + } + + if (end_h > max_h) { + max_h = end_h; + } + + if (end_w > max_w) { + max_w = end_w; + } + + if (ofs == 0 || end_h > limit_h) { // While h limit not reached, keep stacking. + ofs += wrects[j].s.width; + } + } + + _AtlasWorkRectResult result; + result.result = wrects; + result.max_h = max_h; + result.max_w = max_w; + results.push_back(result); + } + + // Find the result with the best aspect ratio. + + int best = -1; + real_t best_aspect = 1e20; + + for (int i = 0; i < results.size(); i++) { + real_t h = next_power_of_2(results[i].max_h); + real_t w = next_power_of_2(results[i].max_w); + real_t aspect = h > w ? h / w : w / h; + if (aspect < best_aspect) { + best = i; + best_aspect = aspect; + } + } + + r_result.resize(p_rects.size()); + + for (int i = 0; i < p_rects.size(); i++) { + r_result.write[results[best].result[i].idx] = results[best].result[i].p; + } + + r_size = Size2(results[best].max_w, results[best].max_h); +} + +Vector> Geometry2D::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector &p_polypath_a, const Vector &p_polypath_b, bool is_a_open) { + using namespace ClipperLib; + + ClipType op = ctUnion; + + switch (p_op) { + case OPERATION_UNION: + op = ctUnion; + break; + case OPERATION_DIFFERENCE: + op = ctDifference; + break; + case OPERATION_INTERSECTION: + op = ctIntersection; + break; + case OPERATION_XOR: + op = ctXor; + break; + } + Path path_a, path_b; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath_a.size(); ++i) { + path_a << IntPoint(p_polypath_a[i].x * SCALE_FACTOR, p_polypath_a[i].y * SCALE_FACTOR); + } + for (int i = 0; i != p_polypath_b.size(); ++i) { + path_b << IntPoint(p_polypath_b[i].x * SCALE_FACTOR, p_polypath_b[i].y * SCALE_FACTOR); + } + Clipper clp; + clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. + clp.AddPath(path_b, ptClip, true); // Polylines cannot be set as clip. + + Paths paths; + + if (is_a_open) { + PolyTree tree; // Needed to populate polylines. + clp.Execute(op, tree); + OpenPathsFromPolyTree(tree, paths); + } else { + clp.Execute(op, paths); // Works on closed polygons only. + } + // Have to scale points down now. + Vector> polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast(scaled_path[j].X) / SCALE_FACTOR, + static_cast(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} + +Vector> Geometry2D::_polypath_offset(const Vector &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + using namespace ClipperLib; + + JoinType jt = jtSquare; + + switch (p_join_type) { + case JOIN_SQUARE: + jt = jtSquare; + break; + case JOIN_ROUND: + jt = jtRound; + break; + case JOIN_MITER: + jt = jtMiter; + break; + } + + EndType et = etClosedPolygon; + + switch (p_end_type) { + case END_POLYGON: + et = etClosedPolygon; + break; + case END_JOINED: + et = etClosedLine; + break; + case END_BUTT: + et = etOpenButt; + break; + case END_SQUARE: + et = etOpenSquare; + break; + case END_ROUND: + et = etOpenRound; + break; + } + ClipperOffset co(2.0, 0.25 * SCALE_FACTOR); // Defaults from ClipperOffset. + Path path; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath.size(); ++i) { + path << IntPoint(p_polypath[i].x * SCALE_FACTOR, p_polypath[i].y * SCALE_FACTOR); + } + co.AddPath(path, jt, et); + + Paths paths; + co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. + + // Have to scale points down now. + Vector> polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast(scaled_path[j].X) / SCALE_FACTOR, + static_cast(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} + +Vector Geometry2D::pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size) { + Vector nodes; + nodes.resize(p_atlas_size.width); + + stbrp_context context; + stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); + + Vector rects; + rects.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + rects.write[i].id = 0; + rects.write[i].w = p_sizes[i].width; + rects.write[i].h = p_sizes[i].height; + rects.write[i].x = 0; + rects.write[i].y = 0; + rects.write[i].was_packed = 0; + } + + int res = stbrp_pack_rects(&context, rects.ptrw(), rects.size()); + if (res == 0) { //pack failed + return Vector(); + } + + Vector ret; + ret.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + Point2i r(rects[i].x, rects[i].y); + ret.write[i] = r; + } + + return ret; +} + +Vector Geometry2D::partial_pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size) { + Vector nodes; + nodes.resize(p_atlas_size.width); + zeromem(nodes.ptrw(), sizeof(stbrp_node) * nodes.size()); + + stbrp_context context; + stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width); + + Vector rects; + rects.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + rects.write[i].id = i; + rects.write[i].w = p_sizes[i].width; + rects.write[i].h = p_sizes[i].height; + rects.write[i].x = 0; + rects.write[i].y = 0; + rects.write[i].was_packed = 0; + } + + stbrp_pack_rects(&context, rects.ptrw(), rects.size()); + + Vector ret; + ret.resize(p_sizes.size()); + + for (int i = 0; i < p_sizes.size(); i++) { + ret.write[rects[i].id] = Vector3i(rects[i].x, rects[i].y, rects[i].was_packed != 0 ? 1 : 0); + } + + return ret; +} diff --git a/core/math/geometry_2d.h b/core/math/geometry_2d.h new file mode 100644 index 0000000000..cfd7abfacb --- /dev/null +++ b/core/math/geometry_2d.h @@ -0,0 +1,398 @@ +/*************************************************************************/ +/* geometry_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GEOMETRY_2D_H +#define GEOMETRY_2D_H + +#include "core/math/delaunay_2d.h" +#include "core/math/rect2.h" +#include "core/math/triangulate.h" +#include "core/object.h" +#include "core/vector.h" + +class Geometry2D { + Geometry2D(); + +public: + static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) { + Vector2 d1 = q1 - p1; // Direction vector of segment S1. + Vector2 d2 = q2 - p2; // Direction vector of segment S2. + Vector2 r = p1 - p2; + real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative. + real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative. + real_t f = d2.dot(r); + real_t s, t; + // Check if either or both segments degenerate into points. + if (a <= CMP_EPSILON && e <= CMP_EPSILON) { + // Both segments degenerate into points. + c1 = p1; + c2 = p2; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + if (a <= CMP_EPSILON) { + // First segment degenerates into a point. + s = 0.0; + t = f / e; // s = 0 => t = (b*s + f) / e = f / e + t = CLAMP(t, 0.0, 1.0); + } else { + real_t c = d1.dot(r); + if (e <= CMP_EPSILON) { + // Second segment degenerates into a point. + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a + } else { + // The general nondegenerate case starts here. + real_t b = d1.dot(d2); + real_t denom = a * e - b * b; // Always nonnegative. + // If segments not parallel, compute closest point on L1 to L2 and + // clamp to segment S1. Else pick arbitrary s (here 0). + if (denom != 0.0) { + s = CLAMP((b * f - c * e) / denom, 0.0, 1.0); + } else { + s = 0.0; + } + // Compute point on L2 closest to S1(s) using + // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e + t = (b * s + f) / e; + + //If t in [0,1] done. Else clamp t, recompute s for the new value + // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a + // and clamp s to [0, 1]. + if (t < 0.0) { + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); + } else if (t > 1.0) { + t = 1.0; + s = CLAMP((b - c) / a, 0.0, 1.0); + } + } + } + c1 = p1 + d1 * s; + c2 = p2 + d2 * t; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + + static Vector2 get_closest_point_to_segment(const Vector2 &p_point, const Vector2 *p_segment) { + Vector2 p = p_point - p_segment[0]; + Vector2 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + if (d <= 0.0) { + return p_segment[0]; // Before first point. + } else if (d >= 1.0) { + return p_segment[1]; // After first point. + } else { + return p_segment[0] + n * d; // Inside. + } + } + + static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { + Vector2 an = a - s; + Vector2 bn = b - s; + Vector2 cn = c - s; + + bool orientation = an.cross(bn) > 0; + + if ((bn.cross(cn) > 0) != orientation) { + return false; + } + + return (cn.cross(an) > 0) == orientation; + } + + static Vector2 get_closest_point_to_segment_uncapped(const Vector2 &p_point, const Vector2 *p_segment) { + Vector2 p = p_point - p_segment[0]; + Vector2 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + return p_segment[0] + n * d; // Inside. + } + + static bool line_intersects_line(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) { + // See http://paulbourke.net/geometry/pointlineplane/ + + const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y; + if (Math::is_zero_approx(denom)) { // Parallel? + return false; + } + + const Vector2 v = p_from_a - p_from_b; + const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom; + r_result = p_from_a + t * p_dir_a; + return true; + } + + static bool segment_intersects_segment(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) { + Vector2 B = p_to_a - p_from_a; + Vector2 C = p_from_b - p_from_a; + Vector2 D = p_to_b - p_from_a; + + real_t ABlen = B.dot(B); + if (ABlen <= 0) { + return false; + } + Vector2 Bn = B / ABlen; + C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y); + D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y); + + if ((C.y < 0 && D.y < 0) || (C.y >= 0 && D.y >= 0)) { + return false; + } + + real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y); + + // Fail if segment C-D crosses line A-B outside of segment A-B. + if (ABpos < 0 || ABpos > 1.0) { + return false; + } + + // (4) Apply the discovered position to line A-B in the original coordinate system. + if (r_result) { + *r_result = p_from_a + B * ABpos; + } + + return true; + } + + static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) { + return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius; + } + + static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) { + Vector2 line_vec = p_to - p_from; + Vector2 vec_to_line = p_from - p_circle_pos; + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * vec_to_line.dot(line_vec); + c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so it definitely won't be t in the range 0 to 1. + if (sqrtterm < 0) { + return -1; + } + + // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) + // then the following can be skipped and we can just return the equivalent of res1. + sqrtterm = Math::sqrt(sqrtterm); + real_t res1 = (-b - sqrtterm) / (2 * a); + real_t res2 = (-b + sqrtterm) / (2 * a); + + if (res1 >= 0 && res1 <= 1) { + return res1; + } + if (res2 >= 0 && res2 <= 1) { + return res2; + } + return -1; + } + + enum PolyBooleanOperation { + OPERATION_UNION, + OPERATION_DIFFERENCE, + OPERATION_INTERSECTION, + OPERATION_XOR + }; + enum PolyJoinType { + JOIN_SQUARE, + JOIN_ROUND, + JOIN_MITER + }; + enum PolyEndType { + END_POLYGON, + END_JOINED, + END_BUTT, + END_SQUARE, + END_ROUND + }; + + static Vector> merge_polygons(const Vector &p_polygon_a, const Vector &p_polygon_b) { + return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b); + } + + static Vector> clip_polygons(const Vector &p_polygon_a, const Vector &p_polygon_b) { + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b); + } + + static Vector> intersect_polygons(const Vector &p_polygon_a, const Vector &p_polygon_b) { + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b); + } + + static Vector> exclude_polygons(const Vector &p_polygon_a, const Vector &p_polygon_b) { + return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b); + } + + static Vector> clip_polyline_with_polygon(const Vector &p_polyline, const Vector &p_polygon) { + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true); + } + + static Vector> intersect_polyline_with_polygon(const Vector &p_polyline, const Vector &p_polygon) { + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true); + } + + static Vector> offset_polygon(const Vector &p_polygon, real_t p_delta, PolyJoinType p_join_type) { + return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON); + } + + static Vector> offset_polyline(const Vector &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector>(), "Attempt to offset a polyline like a polygon (use offset_polygon instead)."); + + return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); + } + + static Vector triangulate_delaunay(const Vector &p_points) { + Vector tr = Delaunay2D::triangulate(p_points); + Vector triangles; + + for (int i = 0; i < tr.size(); i++) { + triangles.push_back(tr[i].points[0]); + triangles.push_back(tr[i].points[1]); + triangles.push_back(tr[i].points[2]); + } + return triangles; + } + + static Vector triangulate_polygon(const Vector &p_polygon) { + Vector triangles; + if (!Triangulate::triangulate(p_polygon, triangles)) { + return Vector(); //fail + } + return triangles; + } + + static bool is_polygon_clockwise(const Vector &p_polygon) { + int c = p_polygon.size(); + if (c < 3) { + return false; + } + const Vector2 *p = p_polygon.ptr(); + real_t sum = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + sum += (v2.x - v1.x) * (v2.y + v1.y); + } + + return sum > 0.0f; + } + + // Alternate implementation that should be faster. + static bool is_point_in_polygon(const Vector2 &p_point, const Vector &p_polygon) { + int c = p_polygon.size(); + if (c < 3) { + return false; + } + const Vector2 *p = p_polygon.ptr(); + Vector2 further_away(-1e20, -1e20); + Vector2 further_away_opposite(1e20, 1e20); + + for (int i = 0; i < c; i++) { + further_away.x = MAX(p[i].x, further_away.x); + further_away.y = MAX(p[i].y, further_away.y); + further_away_opposite.x = MIN(p[i].x, further_away_opposite.x); + further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); + } + + // Make point outside that won't intersect with points in segment from p_point. + further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); + + int intersections = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + if (segment_intersects_segment(v1, v2, p_point, further_away, nullptr)) { + intersections++; + } + } + + return (intersections & 1); + } + + static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) { + return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x); + } + + // Returns a list of points on the convex hull in counter-clockwise order. + // Note: the last point in the returned list is the same as the first one. + static Vector convex_hull(Vector P) { + int n = P.size(), k = 0; + Vector H; + H.resize(2 * n); + + // Sort points lexicographically. + P.sort(); + + // Build lower hull. + for (int i = 0; i < n; ++i) { + while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { + k--; + } + H.write[k++] = P[i]; + } + + // Build upper hull. + for (int i = n - 2, t = k + 1; i >= 0; i--) { + while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) { + k--; + } + H.write[k++] = P[i]; + } + + H.resize(k); + return H; + } + static Vector> decompose_polygon_in_convex(Vector polygon); + + static void make_atlas(const Vector &p_rects, Vector &r_result, Size2i &r_size); + static Vector pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size); + static Vector partial_pack_rects(const Vector &p_sizes, const Size2i &p_atlas_size); + +private: + static Vector> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector &p_polypath_a, const Vector &p_polypath_b, bool is_a_open = false); + static Vector> _polypath_offset(const Vector &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); +}; + +#endif // GEOMETRY_2D_H diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp new file mode 100644 index 0000000000..3b30f4b1fe --- /dev/null +++ b/core/math/geometry_3d.cpp @@ -0,0 +1,1013 @@ +/*************************************************************************/ +/* geometry.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "geometry_3d.h" + +#include "core/print_string.h" + +#include "thirdparty/misc/clipper.hpp" +#include "thirdparty/misc/triangulator.h" + +void Geometry3D::MeshData::optimize_vertices() { + Map vtx_remap; + + for (int i = 0; i < faces.size(); i++) { + for (int j = 0; j < faces[i].indices.size(); j++) { + int idx = faces[i].indices[j]; + if (!vtx_remap.has(idx)) { + int ni = vtx_remap.size(); + vtx_remap[idx] = ni; + } + + faces.write[i].indices.write[j] = vtx_remap[idx]; + } + } + + for (int i = 0; i < edges.size(); i++) { + int a = edges[i].a; + int b = edges[i].b; + + if (!vtx_remap.has(a)) { + int ni = vtx_remap.size(); + vtx_remap[a] = ni; + } + if (!vtx_remap.has(b)) { + int ni = vtx_remap.size(); + vtx_remap[b] = ni; + } + + edges.write[i].a = vtx_remap[a]; + edges.write[i].b = vtx_remap[b]; + } + + Vector new_vertices; + new_vertices.resize(vtx_remap.size()); + + for (int i = 0; i < vertices.size(); i++) { + if (vtx_remap.has(i)) { + new_vertices.write[vtx_remap[i]] = vertices[i]; + } + } + vertices = new_vertices; +} + +struct _FaceClassify { + struct _Link { + int face = -1; + int edge = -1; + void clear() { + face = -1; + edge = -1; + } + _Link() {} + }; + bool valid = false; + int group = -1; + _Link links[3]; + Face3 face; + _FaceClassify() {} +}; + +static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { + // Connect faces, error will occur if an edge is shared between more than 2 faces. + // Clear connections. + + bool error = false; + + for (int i = 0; i < len; i++) { + for (int j = 0; j < 3; j++) { + p_faces[i].links[j].clear(); + } + } + + for (int i = 0; i < len; i++) { + if (p_faces[i].group != p_group) { + continue; + } + for (int j = i + 1; j < len; j++) { + if (p_faces[j].group != p_group) { + continue; + } + + for (int k = 0; k < 3; k++) { + Vector3 vi1 = p_faces[i].face.vertex[k]; + Vector3 vi2 = p_faces[i].face.vertex[(k + 1) % 3]; + + for (int l = 0; l < 3; l++) { + Vector3 vj2 = p_faces[j].face.vertex[l]; + Vector3 vj1 = p_faces[j].face.vertex[(l + 1) % 3]; + + if (vi1.distance_to(vj1) < 0.00001 && + vi2.distance_to(vj2) < 0.00001) { + if (p_faces[i].links[k].face != -1) { + ERR_PRINT("already linked\n"); + error = true; + break; + } + if (p_faces[j].links[l].face != -1) { + ERR_PRINT("already linked\n"); + error = true; + break; + } + + p_faces[i].links[k].face = j; + p_faces[i].links[k].edge = l; + p_faces[j].links[l].face = i; + p_faces[j].links[l].edge = k; + } + } + if (error) { + break; + } + } + if (error) { + break; + } + } + if (error) { + break; + } + } + + for (int i = 0; i < len; i++) { + p_faces[i].valid = true; + for (int j = 0; j < 3; j++) { + if (p_faces[i].links[j].face == -1) { + p_faces[i].valid = false; + } + } + } + return error; +} + +static bool _group_face(_FaceClassify *p_faces, int len, int p_index, int p_group) { + if (p_faces[p_index].group >= 0) { + return false; + } + + p_faces[p_index].group = p_group; + + for (int i = 0; i < 3; i++) { + ERR_FAIL_INDEX_V(p_faces[p_index].links[i].face, len, true); + _group_face(p_faces, len, p_faces[p_index].links[i].face, p_group); + } + + return true; +} + +Vector> Geometry3D::separate_objects(Vector p_array) { + Vector> objects; + + int len = p_array.size(); + + const Face3 *arrayptr = p_array.ptr(); + + Vector<_FaceClassify> fc; + + fc.resize(len); + + _FaceClassify *_fcptr = fc.ptrw(); + + for (int i = 0; i < len; i++) { + _fcptr[i].face = arrayptr[i]; + } + + bool error = _connect_faces(_fcptr, len, -1); + + ERR_FAIL_COND_V_MSG(error, Vector>(), "Invalid geometry."); + + // Group connected faces in separate objects. + + int group = 0; + for (int i = 0; i < len; i++) { + if (!_fcptr[i].valid) { + continue; + } + if (_group_face(_fcptr, len, i, group)) { + group++; + } + } + + // Group connected faces in separate objects. + + for (int i = 0; i < len; i++) { + _fcptr[i].face = arrayptr[i]; + } + + if (group >= 0) { + objects.resize(group); + Vector *group_faces = objects.ptrw(); + + for (int i = 0; i < len; i++) { + if (!_fcptr[i].valid) { + continue; + } + if (_fcptr[i].group >= 0 && _fcptr[i].group < group) { + group_faces[_fcptr[i].group].push_back(_fcptr[i].face); + } + } + } + + return objects; +} + +/*** GEOMETRY WRAPPER ***/ + +enum _CellFlags { + + _CELL_SOLID = 1, + _CELL_EXTERIOR = 2, + _CELL_STEP_MASK = 0x1C, + _CELL_STEP_NONE = 0 << 2, + _CELL_STEP_Y_POS = 1 << 2, + _CELL_STEP_Y_NEG = 2 << 2, + _CELL_STEP_X_POS = 3 << 2, + _CELL_STEP_X_NEG = 4 << 2, + _CELL_STEP_Z_POS = 5 << 2, + _CELL_STEP_Z_NEG = 6 << 2, + _CELL_STEP_DONE = 7 << 2, + _CELL_PREV_MASK = 0xE0, + _CELL_PREV_NONE = 0 << 5, + _CELL_PREV_Y_POS = 1 << 5, + _CELL_PREV_Y_NEG = 2 << 5, + _CELL_PREV_X_POS = 3 << 5, + _CELL_PREV_X_NEG = 4 << 5, + _CELL_PREV_Z_POS = 5 << 5, + _CELL_PREV_Z_NEG = 6 << 5, + _CELL_PREV_FIRST = 7 << 5, + +}; + +static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, const Vector3 &voxelsize, const Face3 &p_face) { + AABB aabb(Vector3(x, y, z), Vector3(len_x, len_y, len_z)); + aabb.position = aabb.position * voxelsize; + aabb.size = aabb.size * voxelsize; + + if (!p_face.intersects_aabb(aabb)) { + return; + } + + if (len_x == 1 && len_y == 1 && len_z == 1) { + p_cell_status[x][y][z] = _CELL_SOLID; + return; + } + + int div_x = len_x > 1 ? 2 : 1; + int div_y = len_y > 1 ? 2 : 1; + int div_z = len_z > 1 ? 2 : 1; + +#define _SPLIT(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \ + if (m_div == 1) { \ + m_new_v = m_v; \ + m_new_len_v = 1; \ + } else if (m_i == 0) { \ + m_new_v = m_v; \ + m_new_len_v = m_len_v / 2; \ + } else { \ + m_new_v = m_v + m_len_v / 2; \ + m_new_len_v = m_len_v - m_len_v / 2; \ + } + + int new_x; + int new_len_x; + int new_y; + int new_len_y; + int new_z; + int new_len_z; + + for (int i = 0; i < div_x; i++) { + _SPLIT(i, div_x, x, len_x, new_x, new_len_x); + + for (int j = 0; j < div_y; j++) { + _SPLIT(j, div_y, y, len_y, new_y, new_len_y); + + for (int k = 0; k < div_z; k++) { + _SPLIT(k, div_z, z, len_z, new_z, new_len_z); + + _plot_face(p_cell_status, new_x, new_y, new_z, new_len_x, new_len_y, new_len_z, voxelsize, p_face); + } + } + } +} + +static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) { + if (p_cell_status[x][y][z] & 3) { + return; // Nothing to do, already used and/or visited. + } + + p_cell_status[x][y][z] = _CELL_PREV_FIRST; + + while (true) { + uint8_t &c = p_cell_status[x][y][z]; + + if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) { + // Haven't been in here, mark as outside. + p_cell_status[x][y][z] |= _CELL_EXTERIOR; + } + + if ((c & _CELL_STEP_MASK) != _CELL_STEP_DONE) { + // If not done, increase step. + c += 1 << 2; + } + + if ((c & _CELL_STEP_MASK) == _CELL_STEP_DONE) { + // Go back. + switch (c & _CELL_PREV_MASK) { + case _CELL_PREV_FIRST: { + return; + } break; + case _CELL_PREV_Y_POS: { + y++; + ERR_FAIL_COND(y >= len_y); + } break; + case _CELL_PREV_Y_NEG: { + y--; + ERR_FAIL_COND(y < 0); + } break; + case _CELL_PREV_X_POS: { + x++; + ERR_FAIL_COND(x >= len_x); + } break; + case _CELL_PREV_X_NEG: { + x--; + ERR_FAIL_COND(x < 0); + } break; + case _CELL_PREV_Z_POS: { + z++; + ERR_FAIL_COND(z >= len_z); + } break; + case _CELL_PREV_Z_NEG: { + z--; + ERR_FAIL_COND(z < 0); + } break; + default: { + ERR_FAIL(); + } + } + continue; + } + + int next_x = x, next_y = y, next_z = z; + uint8_t prev = 0; + + switch (c & _CELL_STEP_MASK) { + case _CELL_STEP_Y_POS: { + next_y++; + prev = _CELL_PREV_Y_NEG; + } break; + case _CELL_STEP_Y_NEG: { + next_y--; + prev = _CELL_PREV_Y_POS; + } break; + case _CELL_STEP_X_POS: { + next_x++; + prev = _CELL_PREV_X_NEG; + } break; + case _CELL_STEP_X_NEG: { + next_x--; + prev = _CELL_PREV_X_POS; + } break; + case _CELL_STEP_Z_POS: { + next_z++; + prev = _CELL_PREV_Z_NEG; + } break; + case _CELL_STEP_Z_NEG: { + next_z--; + prev = _CELL_PREV_Z_POS; + } break; + default: + ERR_FAIL(); + } + + if (next_x < 0 || next_x >= len_x) { + continue; + } + if (next_y < 0 || next_y >= len_y) { + continue; + } + if (next_z < 0 || next_z >= len_z) { + continue; + } + + if (p_cell_status[next_x][next_y][next_z] & 3) { + continue; + } + + x = next_x; + y = next_y; + z = next_z; + p_cell_status[x][y][z] |= prev; + } +} + +static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, Vector &p_faces) { + ERR_FAIL_INDEX(x, len_x); + ERR_FAIL_INDEX(y, len_y); + ERR_FAIL_INDEX(z, len_z); + + if (p_cell_status[x][y][z] & _CELL_EXTERIOR) { + return; + } + +#define vert(m_idx) Vector3(((m_idx)&4) >> 2, ((m_idx)&2) >> 1, (m_idx)&1) + + static const uint8_t indices[6][4] = { + { 7, 6, 4, 5 }, + { 7, 3, 2, 6 }, + { 7, 5, 1, 3 }, + { 0, 2, 3, 1 }, + { 0, 1, 5, 4 }, + { 0, 4, 6, 2 }, + + }; + + for (int i = 0; i < 6; i++) { + Vector3 face_points[4]; + int disp_x = x + ((i % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); + int disp_y = y + (((i - 1) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); + int disp_z = z + (((i - 2) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0); + + bool plot = false; + + if (disp_x < 0 || disp_x >= len_x) { + plot = true; + } + if (disp_y < 0 || disp_y >= len_y) { + plot = true; + } + if (disp_z < 0 || disp_z >= len_z) { + plot = true; + } + + if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) { + plot = true; + } + + if (!plot) { + continue; + } + + for (int j = 0; j < 4; j++) { + face_points[j] = vert(indices[i][j]) + Vector3(x, y, z); + } + + p_faces.push_back( + Face3( + face_points[0], + face_points[1], + face_points[2])); + + p_faces.push_back( + Face3( + face_points[2], + face_points[3], + face_points[0])); + } +} + +Vector Geometry3D::wrap_geometry(Vector p_array, real_t *p_error) { +#define _MIN_SIZE 1.0 +#define _MAX_LENGTH 20 + + int face_count = p_array.size(); + const Face3 *faces = p_array.ptr(); + + AABB global_aabb; + + for (int i = 0; i < face_count; i++) { + if (i == 0) { + global_aabb = faces[i].get_aabb(); + } else { + global_aabb.merge_with(faces[i].get_aabb()); + } + } + + global_aabb.grow_by(0.01); // Avoid numerical error. + + // Determine amount of cells in grid axis. + int div_x, div_y, div_z; + + if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) { + div_x = (int)(global_aabb.size.x / _MIN_SIZE) + 1; + } else { + div_x = _MAX_LENGTH; + } + + if (global_aabb.size.y / _MIN_SIZE < _MAX_LENGTH) { + div_y = (int)(global_aabb.size.y / _MIN_SIZE) + 1; + } else { + div_y = _MAX_LENGTH; + } + + if (global_aabb.size.z / _MIN_SIZE < _MAX_LENGTH) { + div_z = (int)(global_aabb.size.z / _MIN_SIZE) + 1; + } else { + div_z = _MAX_LENGTH; + } + + Vector3 voxelsize = global_aabb.size; + voxelsize.x /= div_x; + voxelsize.y /= div_y; + voxelsize.z /= div_z; + + // Create and initialize cells to zero. + + uint8_t ***cell_status = memnew_arr(uint8_t **, div_x); + for (int i = 0; i < div_x; i++) { + cell_status[i] = memnew_arr(uint8_t *, div_y); + + for (int j = 0; j < div_y; j++) { + cell_status[i][j] = memnew_arr(uint8_t, div_z); + + for (int k = 0; k < div_z; k++) { + cell_status[i][j][k] = 0; + } + } + } + + // Plot faces into cells. + + for (int i = 0; i < face_count; i++) { + Face3 f = faces[i]; + for (int j = 0; j < 3; j++) { + f.vertex[j] -= global_aabb.position; + } + _plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f); + } + + // Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking. + + for (int i = 0; i < div_x; i++) { + for (int j = 0; j < div_y; j++) { + _mark_outside(cell_status, i, j, 0, div_x, div_y, div_z); + _mark_outside(cell_status, i, j, div_z - 1, div_x, div_y, div_z); + } + } + + for (int i = 0; i < div_z; i++) { + for (int j = 0; j < div_y; j++) { + _mark_outside(cell_status, 0, j, i, div_x, div_y, div_z); + _mark_outside(cell_status, div_x - 1, j, i, div_x, div_y, div_z); + } + } + + for (int i = 0; i < div_x; i++) { + for (int j = 0; j < div_z; j++) { + _mark_outside(cell_status, i, 0, j, div_x, div_y, div_z); + _mark_outside(cell_status, i, div_y - 1, j, div_x, div_y, div_z); + } + } + + // Build faces for the inside-outside cell divisors. + + Vector wrapped_faces; + + for (int i = 0; i < div_x; i++) { + for (int j = 0; j < div_y; j++) { + for (int k = 0; k < div_z; k++) { + _build_faces(cell_status, i, j, k, div_x, div_y, div_z, wrapped_faces); + } + } + } + + // Transform face vertices to global coords. + + int wrapped_faces_count = wrapped_faces.size(); + Face3 *wrapped_faces_ptr = wrapped_faces.ptrw(); + + for (int i = 0; i < wrapped_faces_count; i++) { + for (int j = 0; j < 3; j++) { + Vector3 &v = wrapped_faces_ptr[i].vertex[j]; + v = v * voxelsize; + v += global_aabb.position; + } + } + + // clean up grid + + for (int i = 0; i < div_x; i++) { + for (int j = 0; j < div_y; j++) { + memdelete_arr(cell_status[i][j]); + } + + memdelete_arr(cell_status[i]); + } + + memdelete_arr(cell_status); + if (p_error) { + *p_error = voxelsize.length(); + } + + return wrapped_faces; +} + +Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector &p_planes) { + MeshData mesh; + +#define SUBPLANE_SIZE 1024.0 + + real_t subplane_size = 1024.0; // Should compute this from the actual plane. + for (int i = 0; i < p_planes.size(); i++) { + Plane p = p_planes[i]; + + Vector3 ref = Vector3(0.0, 1.0, 0.0); + + if (ABS(p.normal.dot(ref)) > 0.95) { + ref = Vector3(0.0, 0.0, 1.0); // Change axis. + } + + Vector3 right = p.normal.cross(ref).normalized(); + Vector3 up = p.normal.cross(right).normalized(); + + Vector vertices; + + Vector3 center = p.get_any_point(); + // make a quad clockwise + vertices.push_back(center - up * subplane_size + right * subplane_size); + vertices.push_back(center - up * subplane_size - right * subplane_size); + vertices.push_back(center + up * subplane_size - right * subplane_size); + vertices.push_back(center + up * subplane_size + right * subplane_size); + + for (int j = 0; j < p_planes.size(); j++) { + if (j == i) { + continue; + } + + Vector new_vertices; + Plane clip = p_planes[j]; + + if (clip.normal.dot(p.normal) > 0.95) { + continue; + } + + if (vertices.size() < 3) { + break; + } + + for (int k = 0; k < vertices.size(); k++) { + int k_n = (k + 1) % vertices.size(); + + Vector3 edge0_A = vertices[k]; + Vector3 edge1_A = vertices[k_n]; + + real_t dist0 = clip.distance_to(edge0_A); + real_t dist1 = clip.distance_to(edge1_A); + + if (dist0 <= 0) { // Behind plane. + + new_vertices.push_back(vertices[k]); + } + + // Check for different sides and non coplanar. + if ((dist0 * dist1) < 0) { + // Calculate intersection. + Vector3 rel = edge1_A - edge0_A; + + real_t den = clip.normal.dot(rel); + if (Math::is_zero_approx(den)) { + continue; // Point too short. + } + + real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; + Vector3 inters = edge0_A + rel * dist; + new_vertices.push_back(inters); + } + } + + vertices = new_vertices; + } + + if (vertices.size() < 3) { + continue; + } + + // Result is a clockwise face. + + MeshData::Face face; + + // Add face indices. + for (int j = 0; j < vertices.size(); j++) { + int idx = -1; + for (int k = 0; k < mesh.vertices.size(); k++) { + if (mesh.vertices[k].distance_to(vertices[j]) < 0.001) { + idx = k; + break; + } + } + + if (idx == -1) { + idx = mesh.vertices.size(); + mesh.vertices.push_back(vertices[j]); + } + + face.indices.push_back(idx); + } + face.plane = p; + mesh.faces.push_back(face); + + // Add edge. + + for (int j = 0; j < face.indices.size(); j++) { + int a = face.indices[j]; + int b = face.indices[(j + 1) % face.indices.size()]; + + bool found = false; + for (int k = 0; k < mesh.edges.size(); k++) { + if (mesh.edges[k].a == a && mesh.edges[k].b == b) { + found = true; + break; + } + if (mesh.edges[k].b == a && mesh.edges[k].a == b) { + found = true; + break; + } + } + + if (found) { + continue; + } + MeshData::Edge edge; + edge.a = a; + edge.b = b; + mesh.edges.push_back(edge); + } + } + + return mesh; +} + +Vector Geometry3D::build_box_planes(const Vector3 &p_extents) { + Vector planes; + + planes.push_back(Plane(Vector3(1, 0, 0), p_extents.x)); + planes.push_back(Plane(Vector3(-1, 0, 0), p_extents.x)); + planes.push_back(Plane(Vector3(0, 1, 0), p_extents.y)); + planes.push_back(Plane(Vector3(0, -1, 0), p_extents.y)); + planes.push_back(Plane(Vector3(0, 0, 1), p_extents.z)); + planes.push_back(Plane(Vector3(0, 0, -1), p_extents.z)); + + return planes; +} + +Vector Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { + Vector planes; + + for (int i = 0; i < p_sides; i++) { + Vector3 normal; + normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); + normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); + + planes.push_back(Plane(normal, p_radius)); + } + + Vector3 axis; + axis[p_axis] = 1.0; + + planes.push_back(Plane(axis, p_height * 0.5)); + planes.push_back(Plane(-axis, p_height * 0.5)); + + return planes; +} + +Vector Geometry3D::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) { + Vector planes; + + Vector3 axis; + axis[p_axis] = 1.0; + + Vector3 axis_neg; + axis_neg[(p_axis + 1) % 3] = 1.0; + axis_neg[(p_axis + 2) % 3] = 1.0; + axis_neg[p_axis] = -1.0; + + for (int i = 0; i < p_lons; i++) { + Vector3 normal; + normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_lons); + normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_lons); + + planes.push_back(Plane(normal, p_radius)); + + for (int j = 1; j <= p_lats; j++) { + // FIXME: This is stupid. + Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); + Vector3 pos = angle * p_radius; + planes.push_back(Plane(pos, angle)); + planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); + } + } + + return planes; +} + +Vector Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { + Vector planes; + + Vector3 axis; + axis[p_axis] = 1.0; + + Vector3 axis_neg; + axis_neg[(p_axis + 1) % 3] = 1.0; + axis_neg[(p_axis + 2) % 3] = 1.0; + axis_neg[p_axis] = -1.0; + + for (int i = 0; i < p_sides; i++) { + Vector3 normal; + normal[(p_axis + 1) % 3] = Math::cos(i * (2.0 * Math_PI) / p_sides); + normal[(p_axis + 2) % 3] = Math::sin(i * (2.0 * Math_PI) / p_sides); + + planes.push_back(Plane(normal, p_radius)); + + for (int j = 1; j <= p_lats; j++) { + Vector3 angle = normal.lerp(axis, j / (real_t)p_lats).normalized(); + Vector3 pos = axis * p_height * 0.5 + angle * p_radius; + planes.push_back(Plane(pos, angle)); + planes.push_back(Plane(pos * axis_neg, angle * axis_neg)); + } + } + + return planes; +} + +Vector Geometry3D::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { + Vector points; + + // Iterate through every unique combination of any three planes. + for (int i = p_plane_count - 1; i >= 0; i--) { + for (int j = i - 1; j >= 0; j--) { + for (int k = j - 1; k >= 0; k--) { + // Find the point where these planes all cross over (if they + // do at all). + Vector3 convex_shape_point; + if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) { + // See if any *other* plane excludes this point because it's + // on the wrong side. + bool excluded = false; + for (int n = 0; n < p_plane_count; n++) { + if (n != i && n != j && n != k) { + real_t dp = p_planes[n].normal.dot(convex_shape_point); + if (dp - p_planes[n].d > CMP_EPSILON) { + excluded = true; + break; + } + } + } + + // Only add the point if it passed all tests. + if (!excluded) { + points.push_back(convex_shape_point); + } + } + } + } + } + + return points; +} + +#define square(m_s) ((m_s) * (m_s)) +#define INF 1e20 + +/* dt of 1d function using squared distance */ +static void edt(float *f, int stride, int n) { + float *d = (float *)alloca(sizeof(float) * n + sizeof(int) * n + sizeof(float) * (n + 1)); + int *v = (int *)&(d[n]); + float *z = (float *)&v[n]; + + int k = 0; + v[0] = 0; + z[0] = -INF; + z[1] = +INF; + for (int q = 1; q <= n - 1; q++) { + float s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); + while (s <= z[k]) { + k--; + s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]); + } + k++; + v[k] = q; + + z[k] = s; + z[k + 1] = +INF; + } + + k = 0; + for (int q = 0; q <= n - 1; q++) { + while (z[k + 1] < q) { + k++; + } + d[q] = square(q - v[k]) + f[v[k] * stride]; + } + + for (int i = 0; i < n; i++) { + f[i * stride] = d[i]; + } +} + +#undef square + +Vector Geometry3D::generate_edf(const Vector &p_voxels, const Vector3i &p_size, bool p_negative) { + uint32_t float_count = p_size.x * p_size.y * p_size.z; + + ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector()); + + float *work_memory = memnew_arr(float, float_count); + for (uint32_t i = 0; i < float_count; i++) { + work_memory[i] = INF; + } + + uint32_t y_mult = p_size.x; + uint32_t z_mult = y_mult * p_size.y; + + //plot solid cells + { + const bool *voxr = p_voxels.ptr(); + for (uint32_t i = 0; i < float_count; i++) { + bool plot = voxr[i]; + if (p_negative) { + plot = !plot; + } + if (plot) { + work_memory[i] = 0; + } + } + } + + //process in each direction + + //xy->z + + for (int i = 0; i < p_size.x; i++) { + for (int j = 0; j < p_size.y; j++) { + edt(&work_memory[i + j * y_mult], z_mult, p_size.z); + } + } + + //xz->y + + for (int i = 0; i < p_size.x; i++) { + for (int j = 0; j < p_size.z; j++) { + edt(&work_memory[i + j * z_mult], y_mult, p_size.y); + } + } + + //yz->x + for (int i = 0; i < p_size.y; i++) { + for (int j = 0; j < p_size.z; j++) { + edt(&work_memory[i * y_mult + j * z_mult], 1, p_size.x); + } + } + + Vector ret; + ret.resize(float_count); + { + uint32_t *w = ret.ptrw(); + for (uint32_t i = 0; i < float_count; i++) { + w[i] = uint32_t(Math::sqrt(work_memory[i])); + } + } + + return ret; +} + +Vector Geometry3D::generate_sdf8(const Vector &p_positive, const Vector &p_negative) { + ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector()); + Vector sdf8; + int s = p_positive.size(); + sdf8.resize(s); + + const uint32_t *rpos = p_positive.ptr(); + const uint32_t *rneg = p_negative.ptr(); + int8_t *wsdf = sdf8.ptrw(); + for (int i = 0; i < s; i++) { + int32_t diff = int32_t(rpos[i]) - int32_t(rneg[i]); + wsdf[i] = CLAMP(diff, -128, 127); + } + return sdf8; +} diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h new file mode 100644 index 0000000000..64cd34892e --- /dev/null +++ b/core/math/geometry_3d.h @@ -0,0 +1,950 @@ +/*************************************************************************/ +/* geometry_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GEOMETRY_3D_H +#define GEOMETRY_3D_H + +#include "core/math/face3.h" +#include "core/object.h" +#include "core/vector.h" + +class Geometry3D { + Geometry3D(); + +public: + static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { +// Do the function 'd' as defined by pb. I think is is dot product of some sort. +#define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) + + // Calculate the parametric position on the 2 curves, mua and mub. + real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); + real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); + + // Clip the value between [0..1] constraining the solution to lie on the original curves. + if (mua < 0) { + mua = 0; + } + if (mub < 0) { + mub = 0; + } + if (mua > 1) { + mua = 1; + } + if (mub > 1) { + mub = 1; + } + c1 = p1.lerp(p2, mua); + c2 = q1.lerp(q2, mub); + } + + static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { + Vector3 u = p_to_a - p_from_a; + Vector3 v = p_to_b - p_from_b; + Vector3 w = p_from_a - p_to_a; + real_t a = u.dot(u); // Always >= 0 + real_t b = u.dot(v); + real_t c = v.dot(v); // Always >= 0 + real_t d = u.dot(w); + real_t e = v.dot(w); + real_t D = a * c - b * b; // Always >= 0 + real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // Compute the line parameters of the two closest points. + if (D < CMP_EPSILON) { // The lines are almost parallel. + sN = 0.0; // Force using point P0 on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later. + tN = e; + tD = c; + } else { // Get the closest points on the infinite lines + sN = (b * e - c * d); + tN = (a * e - b * d); + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible. + sN = 0.0; + tN = e; + tD = c; + } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. + tN = 0.0; + // Recompute sc for this edge. + if (-d < 0.0) { + sN = 0.0; + } else if (-d > a) { + sN = sD; + } else { + sN = -d; + sD = a; + } + } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. + tN = tD; + // Recompute sc for this edge. + if ((-d + b) < 0.0) { + sN = 0; + } else if ((-d + b) > a) { + sN = sD; + } else { + sN = (-d + b); + sD = a; + } + } + // Finally do the division to get sc and tc. + sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD); + tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD); + + // Get the difference of the two closest points. + Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) + + return dP.length(); // Return the closest distance. + } + + static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { + Vector3 e1 = p_v1 - p_v0; + Vector3 e2 = p_v2 - p_v0; + Vector3 h = p_dir.cross(e2); + real_t a = e1.dot(h); + if (Math::is_zero_approx(a)) { // Parallel test. + return false; + } + + real_t f = 1.0 / a; + + Vector3 s = p_from - p_v0; + real_t u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) { + return false; + } + + Vector3 q = s.cross(e1); + + real_t v = f * p_dir.dot(q); + + if (v < 0.0 || u + v > 1.0) { + return false; + } + + // At this stage we can compute t to find out where + // the intersection point is on the line. + real_t t = f * e2.dot(q); + + if (t > 0.00001) { // ray intersection + if (r_res) { + *r_res = p_from + p_dir * t; + } + return true; + } else { // This means that there is a line intersection but not a ray intersection. + return false; + } + } + + static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { + Vector3 rel = p_to - p_from; + Vector3 e1 = p_v1 - p_v0; + Vector3 e2 = p_v2 - p_v0; + Vector3 h = rel.cross(e2); + real_t a = e1.dot(h); + if (Math::is_zero_approx(a)) { // Parallel test. + return false; + } + + real_t f = 1.0 / a; + + Vector3 s = p_from - p_v0; + real_t u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) { + return false; + } + + Vector3 q = s.cross(e1); + + real_t v = f * rel.dot(q); + + if (v < 0.0 || u + v > 1.0) { + return false; + } + + // At this stage we can compute t to find out where + // the intersection point is on the line. + real_t t = f * e2.dot(q); + + if (t > CMP_EPSILON && t <= 1.0) { // Ray intersection. + if (r_res) { + *r_res = p_from + rel * t; + } + return true; + } else { // This means that there is a line intersection but not a ray intersection. + return false; + } + } + + static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { + Vector3 sphere_pos = p_sphere_pos - p_from; + Vector3 rel = (p_to - p_from); + real_t rel_l = rel.length(); + if (rel_l < CMP_EPSILON) { + return false; // Both points are the same. + } + Vector3 normal = rel / rel_l; + + real_t sphere_d = normal.dot(sphere_pos); + + real_t ray_distance = sphere_pos.distance_to(normal * sphere_d); + + if (ray_distance >= p_sphere_radius) { + return false; + } + + real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance; + real_t inters_d = sphere_d; + + if (inters_d2 >= CMP_EPSILON) { + inters_d -= Math::sqrt(inters_d2); + } + + // Check in segment. + if (inters_d < 0 || inters_d > rel_l) { + return false; + } + + Vector3 result = p_from + normal * inters_d; + + if (r_res) { + *r_res = result; + } + if (r_norm) { + *r_norm = (result - p_sphere_pos).normalized(); + } + + return true; + } + + static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) { + Vector3 rel = (p_to - p_from); + real_t rel_l = rel.length(); + if (rel_l < CMP_EPSILON) { + return false; // Both points are the same. + } + + // First check if they are parallel. + Vector3 normal = (rel / rel_l); + Vector3 crs = normal.cross(Vector3(0, 0, 1)); + real_t crs_l = crs.length(); + + Vector3 z_dir; + + if (crs_l < CMP_EPSILON) { + z_dir = Vector3(1, 0, 0); // Any x/y vector OK. + } else { + z_dir = crs / crs_l; + } + + real_t dist = z_dir.dot(p_from); + + if (dist >= p_radius) { + return false; // Too far away. + } + + // Convert to 2D. + real_t w2 = p_radius * p_radius - dist * dist; + if (w2 < CMP_EPSILON) { + return false; // Avoid numerical error. + } + Size2 size(Math::sqrt(w2), p_height * 0.5); + + Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized(); + + Vector2 from2D(x_dir.dot(p_from), p_from.z); + Vector2 to2D(x_dir.dot(p_to), p_to.z); + + real_t min = 0, max = 1; + + int axis = -1; + + for (int i = 0; i < 2; i++) { + real_t seg_from = from2D[i]; + real_t seg_to = to2D[i]; + real_t box_begin = -size[i]; + real_t box_end = size[i]; + real_t cmin, cmax; + + if (seg_from < seg_to) { + if (seg_from > box_end || seg_to < box_begin) { + return false; + } + real_t length = seg_to - seg_from; + cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; + cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; + + } else { + if (seg_to > box_end || seg_from < box_begin) { + return false; + } + real_t length = seg_to - seg_from; + cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; + cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; + } + + if (cmin > min) { + min = cmin; + axis = i; + } + if (cmax < max) { + max = cmax; + } + if (max < min) { + return false; + } + } + + // Convert to 3D again. + Vector3 result = p_from + (rel * min); + Vector3 res_normal = result; + + if (axis == 0) { + res_normal.z = 0; + } else { + res_normal.x = 0; + res_normal.y = 0; + } + + res_normal.normalize(); + + if (r_res) { + *r_res = result; + } + if (r_norm) { + *r_norm = res_normal; + } + + return true; + } + + static bool segment_intersects_convex(const Vector3 &p_from, const Vector3 &p_to, const Plane *p_planes, int p_plane_count, Vector3 *p_res, Vector3 *p_norm) { + real_t min = -1e20, max = 1e20; + + Vector3 rel = p_to - p_from; + real_t rel_l = rel.length(); + + if (rel_l < CMP_EPSILON) { + return false; + } + + Vector3 dir = rel / rel_l; + + int min_index = -1; + + for (int i = 0; i < p_plane_count; i++) { + const Plane &p = p_planes[i]; + + real_t den = p.normal.dot(dir); + + if (Math::abs(den) <= CMP_EPSILON) { + continue; // Ignore parallel plane. + } + + real_t dist = -p.distance_to(p_from) / den; + + if (den > 0) { + // Backwards facing plane. + if (dist < max) { + max = dist; + } + } else { + // Front facing plane. + if (dist > min) { + min = dist; + min_index = i; + } + } + } + + if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions. + return false; // No intersection. + } + + if (p_res) { + *p_res = p_from + dir * min; + } + if (p_norm) { + *p_norm = p_planes[min_index].normal; + } + + return true; + } + + static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 *p_segment) { + Vector3 p = p_point - p_segment[0]; + Vector3 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + if (d <= 0.0) { + return p_segment[0]; // Before first point. + } else if (d >= 1.0) { + return p_segment[1]; // After first point. + } else { + return p_segment[0] + n * d; // Inside. + } + } + + static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) { + Vector3 p = p_point - p_segment[0]; + Vector3 n = p_segment[1] - p_segment[0]; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) { + return p_segment[0]; // Both points are the same, just give any. + } + + real_t d = n.dot(p) / l2; + + return p_segment[0] + n * d; // Inside. + } + + static inline bool point_in_projected_triangle(const Vector3 &p_point, const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) { + Vector3 face_n = (p_v1 - p_v3).cross(p_v1 - p_v2); + + Vector3 n1 = (p_point - p_v3).cross(p_point - p_v2); + + if (face_n.dot(n1) < 0) { + return false; + } + + Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point); + + if (face_n.dot(n2) < 0) { + return false; + } + + Vector3 n3 = (p_v1 - p_point).cross(p_v1 - p_v2); + + if (face_n.dot(n3) < 0) { + return false; + } + + return true; + } + + static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) { + real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle[0]); + + if (d > p_sphere_radius || d < -p_sphere_radius) { + // Not touching the plane of the face, return. + return false; + } + + Vector3 contact = p_sphere_pos - (p_normal * d); + + /** 2nd) TEST INSIDE TRIANGLE **/ + + if (Geometry3D::point_in_projected_triangle(contact, p_triangle[0], p_triangle[1], p_triangle[2])) { + r_triangle_contact = contact; + r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius; + //printf("solved inside triangle\n"); + return true; + } + + /** 3rd TEST INSIDE EDGE CYLINDERS **/ + + const Vector3 verts[4] = { p_triangle[0], p_triangle[1], p_triangle[2], p_triangle[0] }; // for() friendly + + for (int i = 0; i < 3; i++) { + // Check edge cylinder. + + Vector3 n1 = verts[i] - verts[i + 1]; + Vector3 n2 = p_sphere_pos - verts[i + 1]; + + ///@TODO Maybe discard by range here to make the algorithm quicker. + + // Check point within cylinder radius. + Vector3 axis = n1.cross(n2).cross(n1); + axis.normalize(); + + real_t ad = axis.dot(n2); + + if (ABS(ad) > p_sphere_radius) { + // No chance with this edge, too far away. + continue; + } + + // Check point within edge capsule cylinder. + /** 4th TEST INSIDE EDGE POINTS **/ + + real_t sphere_at = n1.dot(n2); + + if (sphere_at >= 0 && sphere_at < n1.dot(n1)) { + r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2)); + r_sphere_contact = p_sphere_pos - axis * p_sphere_radius; + // Point inside here. + return true; + } + + real_t r2 = p_sphere_radius * p_sphere_radius; + + if (n2.length_squared() < r2) { + Vector3 n = (p_sphere_pos - verts[i + 1]).normalized(); + + r_triangle_contact = verts[i + 1]; + r_sphere_contact = p_sphere_pos - n * p_sphere_radius; + return true; + } + + if (n2.distance_squared_to(n1) < r2) { + Vector3 n = (p_sphere_pos - verts[i]).normalized(); + + r_triangle_contact = verts[i]; + r_sphere_contact = p_sphere_pos - n * p_sphere_radius; + return true; + } + + break; // It's pointless to continue at this point, so save some CPU cycles. + } + + return false; + } + + static inline Vector clip_polygon(const Vector &polygon, const Plane &p_plane) { + enum LocationCache { + LOC_INSIDE = 1, + LOC_BOUNDARY = 0, + LOC_OUTSIDE = -1 + }; + + if (polygon.size() == 0) { + return polygon; + } + + int *location_cache = (int *)alloca(sizeof(int) * polygon.size()); + int inside_count = 0; + int outside_count = 0; + + for (int a = 0; a < polygon.size(); a++) { + real_t dist = p_plane.distance_to(polygon[a]); + if (dist < -CMP_POINT_IN_PLANE_EPSILON) { + location_cache[a] = LOC_INSIDE; + inside_count++; + } else { + if (dist > CMP_POINT_IN_PLANE_EPSILON) { + location_cache[a] = LOC_OUTSIDE; + outside_count++; + } else { + location_cache[a] = LOC_BOUNDARY; + } + } + } + + if (outside_count == 0) { + return polygon; // No changes. + } else if (inside_count == 0) { + return Vector(); // Empty. + } + + long previous = polygon.size() - 1; + Vector clipped; + + for (int index = 0; index < polygon.size(); index++) { + int loc = location_cache[index]; + if (loc == LOC_OUTSIDE) { + if (location_cache[previous] == LOC_INSIDE) { + const Vector3 &v1 = polygon[previous]; + const Vector3 &v2 = polygon[index]; + + Vector3 segment = v1 - v2; + real_t den = p_plane.normal.dot(segment); + real_t dist = p_plane.distance_to(v1) / den; + dist = -dist; + clipped.push_back(v1 + segment * dist); + } + } else { + const Vector3 &v1 = polygon[index]; + if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) { + const Vector3 &v2 = polygon[previous]; + Vector3 segment = v1 - v2; + real_t den = p_plane.normal.dot(segment); + real_t dist = p_plane.distance_to(v1) / den; + dist = -dist; + clipped.push_back(v1 + segment * dist); + } + + clipped.push_back(v1); + } + + previous = index; + } + + return clipped; + } + + static Vector> separate_objects(Vector p_array); + + // Create a "wrap" that encloses the given geometry. + static Vector wrap_geometry(Vector p_array, real_t *p_error = nullptr); + + struct MeshData { + struct Face { + Plane plane; + Vector indices; + }; + + Vector faces; + + struct Edge { + int a, b; + }; + + Vector edges; + + Vector vertices; + + void optimize_vertices(); + }; + + _FORCE_INLINE_ static int get_uv84_normal_bit(const Vector3 &p_vector) { + int lat = Math::fast_ftoi(Math::floor(Math::acos(p_vector.dot(Vector3(0, 1, 0))) * 4.0 / Math_PI + 0.5)); + + if (lat == 0) { + return 24; + } else if (lat == 4) { + return 25; + } + + int lon = Math::fast_ftoi(Math::floor((Math_PI + Math::atan2(p_vector.x, p_vector.z)) * 8.0 / (Math_PI * 2.0) + 0.5)) % 8; + + return lon + (lat - 1) * 8; + } + + _FORCE_INLINE_ static int get_uv84_normal_bit_neighbors(int p_idx) { + if (p_idx == 24) { + return 1 | 2 | 4 | 8; + } else if (p_idx == 25) { + return (1 << 23) | (1 << 22) | (1 << 21) | (1 << 20); + } else { + int ret = 0; + if ((p_idx % 8) == 0) { + ret |= (1 << (p_idx + 7)); + } else { + ret |= (1 << (p_idx - 1)); + } + if ((p_idx % 8) == 7) { + ret |= (1 << (p_idx - 7)); + } else { + ret |= (1 << (p_idx + 1)); + } + + int mask = ret | (1 << p_idx); + if (p_idx < 8) { + ret |= 24; + } else { + ret |= mask >> 8; + } + + if (p_idx >= 16) { + ret |= 25; + } else { + ret |= mask << 8; + } + + return ret; + } + } + static MeshData build_convex_mesh(const Vector &p_planes); + static Vector build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); + static Vector build_box_planes(const Vector3 &p_extents); + static Vector build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z); + static Vector build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z); + + static Vector compute_convex_mesh_points(const Plane *p_planes, int p_plane_count); + +#define FINDMINMAX(x0, x1, x2, min, max) \ + min = max = x0; \ + if (x1 < min) { \ + min = x1; \ + } \ + if (x1 > max) { \ + max = x1; \ + } \ + if (x2 < min) { \ + min = x2; \ + } \ + if (x2 > max) { \ + max = x2; \ + } + + _FORCE_INLINE_ static bool planeBoxOverlap(Vector3 normal, float d, Vector3 maxbox) { + int q; + Vector3 vmin, vmax; + for (q = 0; q <= 2; q++) { + if (normal[q] > 0.0f) { + vmin[q] = -maxbox[q]; + vmax[q] = maxbox[q]; + } else { + vmin[q] = maxbox[q]; + vmax[q] = -maxbox[q]; + } + } + if (normal.dot(vmin) + d > 0.0f) { + return false; + } + if (normal.dot(vmax) + d >= 0.0f) { + return true; + } + + return false; + } + +/*======================== X-tests ========================*/ +#define AXISTEST_X01(a, b, fa, fb) \ + p0 = a * v0.y - b * v0.z; \ + p2 = a * v2.y - b * v2.z; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_X2(a, b, fa, fb) \ + p0 = a * v0.y - b * v0.z; \ + p1 = a * v1.y - b * v1.z; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +/*======================== Y-tests ========================*/ +#define AXISTEST_Y02(a, b, fa, fb) \ + p0 = -a * v0.x + b * v0.z; \ + p2 = -a * v2.x + b * v2.z; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_Y1(a, b, fa, fb) \ + p0 = -a * v0.x + b * v0.z; \ + p1 = -a * v1.x + b * v1.z; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \ + if (min > rad || max < -rad) { \ + return false; \ + } + + /*======================== Z-tests ========================*/ + +#define AXISTEST_Z12(a, b, fa, fb) \ + p1 = a * v1.x - b * v1.y; \ + p2 = a * v2.x - b * v2.y; \ + if (p2 < p1) { \ + min = p2; \ + max = p1; \ + } else { \ + min = p1; \ + max = p2; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ + if (min > rad || max < -rad) { \ + return false; \ + } + +#define AXISTEST_Z0(a, b, fa, fb) \ + p0 = a * v0.x - b * v0.y; \ + p1 = a * v1.x - b * v1.y; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \ + if (min > rad || max < -rad) { \ + return false; \ + } + + _FORCE_INLINE_ static bool triangle_box_overlap(const Vector3 &boxcenter, const Vector3 boxhalfsize, const Vector3 *triverts) { + /* use separating axis theorem to test overlap between triangle and box */ + /* need to test for overlap in these directions: */ + /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ + /* we do not even need to test these) */ + /* 2) normal of the triangle */ + /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ + /* this gives 3x3=9 more tests */ + Vector3 v0, v1, v2; + float min, max, d, p0, p1, p2, rad, fex, fey, fez; + Vector3 normal, e0, e1, e2; + + /* This is the fastest branch on Sun */ + /* move everything so that the boxcenter is in (0,0,0) */ + + v0 = triverts[0] - boxcenter; + v1 = triverts[1] - boxcenter; + v2 = triverts[2] - boxcenter; + + /* compute triangle edges */ + e0 = v1 - v0; /* tri edge 0 */ + e1 = v2 - v1; /* tri edge 1 */ + e2 = v0 - v2; /* tri edge 2 */ + + /* Bullet 3: */ + /* test the 9 tests first (this was faster) */ + fex = Math::abs(e0.x); + fey = Math::abs(e0.y); + fez = Math::abs(e0.z); + AXISTEST_X01(e0.z, e0.y, fez, fey); + AXISTEST_Y02(e0.z, e0.x, fez, fex); + AXISTEST_Z12(e0.y, e0.x, fey, fex); + + fex = Math::abs(e1.x); + fey = Math::abs(e1.y); + fez = Math::abs(e1.z); + AXISTEST_X01(e1.z, e1.y, fez, fey); + AXISTEST_Y02(e1.z, e1.x, fez, fex); + AXISTEST_Z0(e1.y, e1.x, fey, fex); + + fex = Math::abs(e2.x); + fey = Math::abs(e2.y); + fez = Math::abs(e2.z); + AXISTEST_X2(e2.z, e2.y, fez, fey); + AXISTEST_Y1(e2.z, e2.x, fez, fex); + AXISTEST_Z12(e2.y, e2.x, fey, fex); + + /* Bullet 1: */ + /* first test overlap in the {x,y,z}-directions */ + /* find min, max of the triangle each direction, and test for overlap in */ + /* that direction -- this is equivalent to testing a minimal AABB around */ + /* the triangle against the AABB */ + + /* test in X-direction */ + FINDMINMAX(v0.x, v1.x, v2.x, min, max); + if (min > boxhalfsize.x || max < -boxhalfsize.x) { + return false; + } + + /* test in Y-direction */ + FINDMINMAX(v0.y, v1.y, v2.y, min, max); + if (min > boxhalfsize.y || max < -boxhalfsize.y) { + return false; + } + + /* test in Z-direction */ + FINDMINMAX(v0.z, v1.z, v2.z, min, max); + if (min > boxhalfsize.z || max < -boxhalfsize.z) { + return false; + } + + /* Bullet 2: */ + /* test if the box intersects the plane of the triangle */ + /* compute plane equation of triangle: normal*x+d=0 */ + normal = e0.cross(e1); + d = -normal.dot(v0); /* plane eq: normal.x+d=0 */ + return planeBoxOverlap(normal, d, boxhalfsize); /* if true, box and triangle overlaps */ + } + + static Vector generate_edf(const Vector &p_voxels, const Vector3i &p_size, bool p_negative); + static Vector generate_sdf8(const Vector &p_positive, const Vector &p_negative); + + static Vector3 triangle_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_pos) { + Vector3 v0 = p_b - p_a; + Vector3 v1 = p_c - p_a; + Vector3 v2 = p_pos - p_a; + + float d00 = v0.dot(v0); + float d01 = v0.dot(v1); + float d11 = v1.dot(v1); + float d20 = v2.dot(v0); + float d21 = v2.dot(v1); + float denom = (d00 * d11 - d01 * d01); + if (denom == 0) { + return Vector3(); //invalid triangle, return empty + } + float v = (d11 * d20 - d01 * d21) / denom; + float w = (d00 * d21 - d01 * d20) / denom; + float u = 1.0f - v - w; + return Vector3(u, v, w); + } + + static Color tetrahedron_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_d, const Vector3 &p_pos) { + Vector3 vap = p_pos - p_a; + Vector3 vbp = p_pos - p_b; + + Vector3 vab = p_b - p_a; + Vector3 vac = p_c - p_a; + Vector3 vad = p_d - p_a; + + Vector3 vbc = p_c - p_b; + Vector3 vbd = p_d - p_b; + // ScTP computes the scalar triple product +#define STP(m_a, m_b, m_c) ((m_a).dot((m_b).cross((m_c)))) + float va6 = STP(vbp, vbd, vbc); + float vb6 = STP(vap, vac, vad); + float vc6 = STP(vap, vad, vab); + float vd6 = STP(vap, vab, vac); + float v6 = 1 / STP(vab, vac, vad); + return Color(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); +#undef STP + } +}; + +#endif // GEOMETRY_3D_H diff --git a/core/math/octree.h b/core/math/octree.h index c05fc4e9ed..5d9688d442 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -34,7 +34,7 @@ #include "core/list.h" #include "core/map.h" #include "core/math/aabb.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/math/vector3.h" #include "core/print_string.h" #include "core/variant.h" @@ -1201,7 +1201,7 @@ int Octree::cull_convex(const Vector &p_convex, T **p_r return 0; } - Vector convex_points = Geometry::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + Vector convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size()); if (convex_points.size() == 0) { return 0; } diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index fe16904448..8ba1ba9286 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -34,7 +34,7 @@ uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF; -Error QuickHull::build(const Vector &p_points, Geometry::MeshData &r_mesh) { +Error QuickHull::build(const Vector &p_points, Geometry3D::MeshData &r_mesh) { /* CREATE AABB VOLUME */ AABB aabb; @@ -334,17 +334,17 @@ Error QuickHull::build(const Vector &p_points, Geometry::MeshData &r_me //make a map of edges again Map ret_edges; - List ret_faces; + List ret_faces; for (List::Element *E = faces.front(); E; E = E->next()) { - Geometry::MeshData::Face f; + Geometry3D::MeshData::Face f; f.plane = E->get().plane; for (int i = 0; i < 3; i++) { f.indices.push_back(E->get().vertices[i]); } - List::Element *F = ret_faces.push_back(f); + List::Element *F = ret_faces.push_back(f); for (int i = 0; i < 3; i++) { uint32_t a = E->get().vertices[i]; @@ -366,8 +366,8 @@ Error QuickHull::build(const Vector &p_points, Geometry::MeshData &r_me //fill faces - for (List::Element *E = ret_faces.front(); E; E = E->next()) { - Geometry::MeshData::Face &f = E->get(); + for (List::Element *E = ret_faces.front(); E; E = E->next()) { + Geometry3D::MeshData::Face &f = E->get(); for (int i = 0; i < f.indices.size(); i++) { int a = E->get().indices[i]; @@ -377,7 +377,7 @@ Error QuickHull::build(const Vector &p_points, Geometry::MeshData &r_me Map::Element *F = ret_edges.find(e); ERR_CONTINUE(!F); - List::Element *O = F->get().left == E ? F->get().right : F->get().left; + List::Element *O = F->get().left == E ? F->get().right : F->get().left; ERR_CONTINUE(O == E); ERR_CONTINUE(O == nullptr); @@ -439,13 +439,13 @@ Error QuickHull::build(const Vector &p_points, Geometry::MeshData &r_me r_mesh.faces.resize(ret_faces.size()); int idx = 0; - for (List::Element *E = ret_faces.front(); E; E = E->next()) { + for (List::Element *E = ret_faces.front(); E; E = E->next()) { r_mesh.faces.write[idx++] = E->get(); } r_mesh.edges.resize(ret_edges.size()); idx = 0; for (Map::Element *E = ret_edges.front(); E; E = E->next()) { - Geometry::MeshData::Edge e; + Geometry3D::MeshData::Edge e; e.a = E->key().vertices[0]; e.b = E->key().vertices[1]; r_mesh.edges.write[idx++] = e; diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index 29f709febe..cac8e58d23 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -33,7 +33,7 @@ #include "core/list.h" #include "core/math/aabb.h" -#include "core/math/geometry.h" +#include "core/math/geometry_3d.h" #include "core/set.h" class QuickHull { @@ -74,13 +74,13 @@ private: FaceConnect() {} }; struct RetFaceConnect { - List::Element *left, *right = nullptr; + List::Element *left, *right = nullptr; RetFaceConnect() {} }; public: static uint32_t debug_stop_after; - static Error build(const Vector &p_points, Geometry::MeshData &r_mesh); + static Error build(const Vector &p_points, Geometry3D::MeshData &r_mesh); }; #endif // QUICK_HULL_H -- cgit v1.2.3