summaryrefslogtreecommitdiffstats
path: root/core/math/geometry_3d.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'core/math/geometry_3d.cpp')
-rw-r--r--core/math/geometry_3d.cpp157
1 files changed, 133 insertions, 24 deletions
diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp
index ec96753c79..548b9e4620 100644
--- a/core/math/geometry_3d.cpp
+++ b/core/math/geometry_3d.cpp
@@ -30,29 +30,132 @@
#include "geometry_3d.h"
-#include "core/string/print_string.h"
-
#include "thirdparty/misc/clipper.hpp"
#include "thirdparty/misc/polypartition.h"
+void Geometry3D::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) {
+ // Based on David Eberly's Computation of Distance Between Line Segments algorithm.
+
+ Vector3 p = p_p1 - p_p0;
+ Vector3 q = p_q1 - p_q0;
+ Vector3 r = p_p0 - p_q0;
+
+ real_t a = p.dot(p);
+ real_t b = p.dot(q);
+ real_t c = q.dot(q);
+ real_t d = p.dot(r);
+ real_t e = q.dot(r);
+
+ real_t s = 0.0f;
+ real_t t = 0.0f;
+
+ real_t det = a * c - b * b;
+ if (det > CMP_EPSILON) {
+ // Non-parallel segments
+ real_t bte = b * e;
+ real_t ctd = c * d;
+
+ if (bte <= ctd) {
+ // s <= 0.0f
+ if (e <= 0.0f) {
+ // t <= 0.0f
+ s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f));
+ t = 0.0f;
+ } else if (e < c) {
+ // 0.0f < t < 1
+ s = 0.0f;
+ t = e / c;
+ } else {
+ // t >= 1
+ s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f));
+ t = 1;
+ }
+ } else {
+ // s > 0.0f
+ s = bte - ctd;
+ if (s >= det) {
+ // s >= 1
+ if (b + e <= 0.0f) {
+ // t <= 0.0f
+ s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1));
+ t = 0.0f;
+ } else if (b + e < c) {
+ // 0.0f < t < 1
+ s = 1;
+ t = (b + e) / c;
+ } else {
+ // t >= 1
+ s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1));
+ t = 1;
+ }
+ } else {
+ // 0.0f < s < 1
+ real_t ate = a * e;
+ real_t btd = b * d;
+
+ if (ate <= btd) {
+ // t <= 0.0f
+ s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+ t = 0.0f;
+ } else {
+ // t > 0.0f
+ t = ate - btd;
+ if (t >= det) {
+ // t >= 1
+ s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+ t = 1;
+ } else {
+ // 0.0f < t < 1
+ s /= det;
+ t /= det;
+ }
+ }
+ }
+ }
+ } else {
+ // Parallel segments
+ if (e <= 0.0f) {
+ s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+ t = 0.0f;
+ } else if (e >= c) {
+ s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+ t = 1;
+ } else {
+ s = 0.0f;
+ t = e / c;
+ }
+ }
+
+ r_ps = (1 - s) * p_p0 + s * p_p1;
+ r_qt = (1 - t) * p_q0 + t * p_q1;
+}
+
+real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) {
+ Vector3 ps;
+ Vector3 qt;
+ get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt);
+ Vector3 st = qt - ps;
+ return st.length();
+}
+
void Geometry3D::MeshData::optimize_vertices() {
HashMap<int, int> vtx_remap;
- for (int i = 0; i < faces.size(); i++) {
- for (int j = 0; j < faces[i].indices.size(); j++) {
+ for (uint32_t i = 0; i < faces.size(); i++) {
+ for (uint32_t 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];
+ faces[i].indices[j] = vtx_remap[idx];
}
}
- for (int i = 0; i < edges.size(); i++) {
- int a = edges[i].a;
- int b = edges[i].b;
+ for (uint32_t i = 0; i < edges.size(); i++) {
+ int a = edges[i].vertex_a;
+ int b = edges[i].vertex_b;
if (!vtx_remap.has(a)) {
int ni = vtx_remap.size();
@@ -63,16 +166,16 @@ void Geometry3D::MeshData::optimize_vertices() {
vtx_remap[b] = ni;
}
- edges.write[i].a = vtx_remap[a];
- edges.write[i].b = vtx_remap[b];
+ edges[i].vertex_a = vtx_remap[a];
+ edges[i].vertex_b = vtx_remap[b];
}
- Vector<Vector3> new_vertices;
+ LocalVector<Vector3> new_vertices;
new_vertices.resize(vtx_remap.size());
- for (int i = 0; i < vertices.size(); i++) {
+ for (uint32_t i = 0; i < vertices.size(); i++) {
if (vtx_remap.has(i)) {
- new_vertices.write[vtx_remap[i]] = vertices[i];
+ new_vertices[vtx_remap[i]] = vertices[i];
}
}
vertices = new_vertices;
@@ -648,7 +751,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
Vector3 center = p.center();
// make a quad clockwise
- Vector<Vector3> vertices = {
+ LocalVector<Vector3> vertices = {
center - up * subplane_size + right * subplane_size,
center - up * subplane_size - right * subplane_size,
center + up * subplane_size - right * subplane_size,
@@ -660,7 +763,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
continue;
}
- Vector<Vector3> new_vertices;
+ LocalVector<Vector3> new_vertices;
Plane clip = p_planes[j];
if (clip.normal.dot(p.normal) > 0.95f) {
@@ -671,7 +774,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
break;
}
- for (int k = 0; k < vertices.size(); k++) {
+ for (uint32_t k = 0; k < vertices.size(); k++) {
int k_n = (k + 1) % vertices.size();
Vector3 edge0_A = vertices[k];
@@ -713,9 +816,9 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
MeshData::Face face;
// Add face indices.
- for (int j = 0; j < vertices.size(); j++) {
+ for (uint32_t j = 0; j < vertices.size(); j++) {
int idx = -1;
- for (int k = 0; k < mesh.vertices.size(); k++) {
+ for (uint32_t k = 0; k < mesh.vertices.size(); k++) {
if (mesh.vertices[k].distance_to(vertices[j]) < 0.001f) {
idx = k;
break;
@@ -734,28 +837,34 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
// Add edge.
- for (int j = 0; j < face.indices.size(); j++) {
+ for (uint32_t 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) {
+ int found_idx = -1;
+ for (uint32_t k = 0; k < mesh.edges.size(); k++) {
+ if (mesh.edges[k].vertex_a == a && mesh.edges[k].vertex_b == b) {
found = true;
+ found_idx = k;
break;
}
- if (mesh.edges[k].b == a && mesh.edges[k].a == b) {
+ if (mesh.edges[k].vertex_b == a && mesh.edges[k].vertex_a == b) {
found = true;
+ found_idx = k;
break;
}
}
if (found) {
+ mesh.edges[found_idx].face_b = j;
continue;
}
MeshData::Edge edge;
- edge.a = a;
- edge.b = b;
+ edge.vertex_a = a;
+ edge.vertex_b = b;
+ edge.face_a = j;
+ edge.face_b = -1;
mesh.edges.push_back(edge);
}
}