diff options
Diffstat (limited to 'core/math/geometry_3d.cpp')
| -rw-r--r-- | core/math/geometry_3d.cpp | 1013 |
1 files changed, 1013 insertions, 0 deletions
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<int, int> 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<Vector3> 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<Vector<Face3>> Geometry3D::separate_objects(Vector<Face3> p_array) { + Vector<Vector<Face3>> 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<Vector<Face3>>(), "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<Face3> *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<Face3> &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<Face3> Geometry3D::wrap_geometry(Vector<Face3> 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<Face3> 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<Plane> &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<Vector3> 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<Vector3> 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<Plane> Geometry3D::build_box_planes(const Vector3 &p_extents) { + Vector<Plane> 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<Plane> Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { + Vector<Plane> 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<Plane> Geometry3D::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) { + Vector<Plane> 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<Plane> Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { + Vector<Plane> 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<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) { + Vector<Vector3> 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<uint32_t> Geometry3D::generate_edf(const Vector<bool> &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<uint32_t>()); + + 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<uint32_t> 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<int8_t> Geometry3D::generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative) { + ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector<int8_t>()); + Vector<int8_t> 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; +} |