summaryrefslogtreecommitdiff
path: root/core/math/quick_hull.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'core/math/quick_hull.cpp')
-rw-r--r--core/math/quick_hull.cpp513
1 files changed, 513 insertions, 0 deletions
diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp
new file mode 100644
index 0000000000..ed30de6915
--- /dev/null
+++ b/core/math/quick_hull.cpp
@@ -0,0 +1,513 @@
+/*************************************************************************/
+/* quick_hull.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* http://www.godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* 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 "quick_hull.h"
+#include "map.h"
+
+uint32_t QuickHull::debug_stop_after=0xFFFFFFFF;
+
+Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_mesh) {
+
+
+ static const real_t over_tolerance = 0.0001;
+
+ /* CREATE AABB VOLUME */
+
+ AABB aabb;
+ for(int i=0;i<p_points.size();i++) {
+
+ if (i==0) {
+ aabb.pos=p_points[i];
+ } else {
+ aabb.expand_to(p_points[i]);
+ }
+ }
+
+
+ if (aabb.size==Vector3()) {
+ return ERR_CANT_CREATE;
+ }
+
+
+ Vector<bool> valid_points;
+ valid_points.resize(p_points.size());
+ Set<Vector3> valid_cache;
+
+ for(int i=0;i<p_points.size();i++) {
+
+ Vector3 sp = p_points[i].snapped(0.0001);
+ if (valid_cache.has(sp)) {
+ valid_points[i]=false;
+ print_line("INVALIDATED: "+itos(i));
+ }else {
+ valid_points[i]=true;
+ valid_cache.insert(sp);
+ }
+ }
+
+
+
+ /* CREATE INITIAL SIMPLEX */
+
+ int longest_axis = aabb.get_longest_axis_index();
+
+ //first two vertices are the most distant
+ int simplex[4];
+
+ {
+ real_t max,min;
+
+ for(int i=0;i<p_points.size();i++) {
+
+ if (!valid_points[i])
+ continue;
+ float d = p_points[i][longest_axis];
+ if (i==0 || d < min) {
+
+ simplex[0]=i;
+ min=d;
+ }
+
+ if (i==0 || d > max) {
+ simplex[1]=i;
+ max=d;
+ }
+
+ }
+ }
+
+ //third vertex is one most further away from the line
+
+
+ {
+ float maxd;
+ Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]];
+
+ for(int i=0;i<p_points.size();i++) {
+
+ if (!valid_points[i])
+ continue;
+
+ Vector3 n = rel12.cross(p_points[simplex[0]]-p_points[i]).cross(rel12).normalized();
+ real_t d = Math::abs(n.dot(p_points[simplex[0]])-n.dot(p_points[i]));
+
+ if (i==0 || d>maxd) {
+
+ maxd=d;
+ simplex[2]=i;
+ }
+ }
+ }
+
+ //fourth vertex is the one most further away from the plane
+
+ {
+ float maxd;
+ Plane p(p_points[simplex[0]],p_points[simplex[1]],p_points[simplex[2]]);
+
+ for(int i=0;i<p_points.size();i++) {
+
+ if (!valid_points[i])
+ continue;
+
+ real_t d = Math::abs(p.distance_to(p_points[i]));
+
+ if (i==0 || d>maxd) {
+
+ maxd=d;
+ simplex[3]=i;
+ }
+ }
+ }
+
+
+ //compute center of simplex, this is a point always warranted to be inside
+ Vector3 center;
+
+ for(int i=0;i<4;i++) {
+ center+=p_points[simplex[i]];
+ }
+
+ center/=4.0;
+
+ //add faces
+
+ List<Face> faces;
+
+ for(int i=0;i<4;i++) {
+
+ static const int face_order[4][3]={
+ {0,1,2},
+ {0,1,3},
+ {0,2,3},
+ {1,2,3}
+ };
+
+ Face f;
+ for(int j=0;j<3;j++) {
+ f.vertices[j]=simplex[face_order[i][j]];
+ }
+
+
+ Plane p(p_points[f.vertices[0]],p_points[f.vertices[1]],p_points[f.vertices[2]]);
+
+ if (p.is_point_over(center)) {
+ //flip face to clockwise if facing inwards
+ SWAP( f.vertices[0], f.vertices[1] );
+ p=-p;
+ }
+
+
+ f.plane = p;
+
+ faces.push_back(f);
+
+ }
+
+
+ /* COMPUTE AVAILABLE VERTICES */
+
+ for(int i=0;i<p_points.size();i++) {
+
+ if (i==simplex[0])
+ continue;
+ if (i==simplex[1])
+ continue;
+ if (i==simplex[2])
+ continue;
+ if (i==simplex[3])
+ continue;
+ if (!valid_points[i])
+ continue;
+
+ for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+ if (E->get().plane.distance_to(p_points[i]) > over_tolerance ) {
+
+ E->get().points_over.push_back(i);
+ break;
+ }
+ }
+
+
+
+ }
+
+ faces.sort(); // sort them, so the ones with points are in the back
+
+
+ /* BUILD HULL */
+
+
+ //poop face (while still remain)
+ //find further away point
+ //find lit faces
+ //determine horizon edges
+ //build new faces with horizon edges, them assign points side from all lit faces
+ //remove lit faces
+
+
+ uint32_t debug_stop = debug_stop_after;
+
+ while(debug_stop>0 && faces.back()->get().points_over.size()) {
+
+ debug_stop--;
+ Face& f = faces.back()->get();
+
+ //find vertex most outside
+ int next=-1;
+ real_t next_d=0;
+
+ for(int i=0;i<f.points_over.size();i++) {
+
+ real_t d = f.plane.distance_to(p_points[f.points_over[i]]);
+
+ if (d > next_d) {
+ next_d=d;
+ next=i;
+ }
+ }
+
+ ERR_FAIL_COND_V(next==-1,ERR_BUG);
+
+
+
+ Vector3 v = p_points[f.points_over[next]];
+
+ //find lit faces and lit edges
+ List< List<Face>::Element* > lit_faces; //lit face is a death sentence
+
+ Map<Edge,FaceConnect> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot
+
+ for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+ if (E->get().plane.distance_to(v) >0 ) {
+
+ lit_faces.push_back(E);
+
+ for(int i=0;i<3;i++) {
+ uint32_t a = E->get().vertices[i];
+ uint32_t b = E->get().vertices[(i+1)%3];
+ Edge e(a,b);
+
+ Map<Edge,FaceConnect>::Element *F=lit_edges.find(e);
+ if (!F) {
+ F=lit_edges.insert(e,FaceConnect());
+ }
+ if (e.vertices[0]==a) {
+ //left
+ F->get().left=E;
+ } else {
+
+ F->get().right=E;
+ }
+ }
+ }
+ }
+
+
+ //create new faces from horizon edges
+ List< List<Face>::Element* > new_faces; //new faces
+
+ for(Map<Edge,FaceConnect>::Element *E=lit_edges.front();E;E=E->next()) {
+
+ FaceConnect& fc = E->get();
+ if (fc.left && fc.right) {
+ continue; //edge is uninteresting, not on horizont
+ }
+
+ //create new face!
+
+ Face face;
+ face.vertices[0]=f.points_over[next];
+ face.vertices[1]=E->key().vertices[0];
+ face.vertices[2]=E->key().vertices[1];
+
+ Plane p(p_points[face.vertices[0]],p_points[face.vertices[1]],p_points[face.vertices[2]]);
+
+ if (p.is_point_over(center)) {
+ //flip face to clockwise if facing inwards
+ SWAP( face.vertices[0], face.vertices[1] );
+ p = -p;
+ }
+
+ face.plane = p;
+ new_faces.push_back( faces.push_back(face) );
+ }
+
+ //distribute points into new faces
+
+ for(List< List<Face>::Element* >::Element *F=lit_faces.front();F;F=F->next()) {
+
+ Face &lf = F->get()->get();
+
+ for(int i=0;i<lf.points_over.size();i++) {
+
+ if (lf.points_over[i]==f.points_over[next]) //do not add current one
+ continue;
+
+ Vector3 p = p_points[lf.points_over[i]];
+ for (List< List<Face>::Element* >::Element *E=new_faces.front();E;E=E->next()) {
+
+ Face &f2 = E->get()->get();
+ if (f2.plane.distance_to(p)>over_tolerance) {
+ f2.points_over.push_back(lf.points_over[i]);
+ break;
+ }
+ }
+
+
+ }
+ }
+
+ //erase lit faces
+
+ while(lit_faces.size()) {
+
+ faces.erase(lit_faces.front()->get());
+ lit_faces.pop_front();
+ }
+
+ //put faces that contain no points on the front
+
+ for (List< List<Face>::Element* >::Element *E=new_faces.front();E;E=E->next()) {
+
+ Face &f2 = E->get()->get();
+ if (f2.points_over.size()==0) {
+ faces.move_to_front(E->get());
+ }
+ }
+
+ //whew, done with iteration, go next
+
+
+
+ }
+
+ /* CREATE MESHDATA */
+
+
+ //make a map of edges again
+ Map<Edge,RetFaceConnect> ret_edges;
+ List<Geometry::MeshData::Face> ret_faces;
+
+
+ for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+ Geometry::MeshData::Face f;
+ f.plane = E->get().plane;
+
+
+
+ for(int i=0;i<3;i++) {
+ f.indices.push_back(E->get().vertices[i]);
+ }
+
+ List<Geometry::MeshData::Face>::Element *F = ret_faces.push_back(f);
+
+ for(int i=0;i<3;i++) {
+
+ uint32_t a = E->get().vertices[i];
+ uint32_t b = E->get().vertices[(i+1)%3];
+ Edge e(a,b);
+
+ Map<Edge,RetFaceConnect>::Element *G=ret_edges.find(e);
+ if (!G) {
+ G=ret_edges.insert(e,RetFaceConnect());
+ }
+ if (e.vertices[0]==a) {
+ //left
+ G->get().left=F;
+ } else {
+
+ G->get().right=F;
+ }
+ }
+ }
+
+ //fill faces
+
+ for (List<Geometry::MeshData::Face>::Element *E=ret_faces.front();E;E=E->next()) {
+
+ Geometry::MeshData::Face& f = E->get();
+
+ for(int i=0;i<f.indices.size();i++) {
+
+ uint32_t a = E->get().indices[i];
+ uint32_t b = E->get().indices[(i+1)%f.indices.size()];
+ Edge e(a,b);
+
+ Map<Edge,RetFaceConnect>::Element *F=ret_edges.find(e);
+
+ ERR_CONTINUE(!F);
+
+ List<Geometry::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left;
+ ERR_CONTINUE(O==E);
+
+ if (O->get().plane.is_almost_like(f.plane)) {
+ //merge and delete edge and contiguous face, while repointing edges (uuugh!)
+ int ois = O->get().indices.size();
+ int merged=0;
+
+
+ for(int j=0;j<ois;j++) {
+ //search a
+ if (O->get().indices[j]==a) {
+ //append the rest
+ for(int k=0;k<ois;k++) {
+
+ int idx = O->get().indices[(k+j)%ois];
+ int idxn = O->get().indices[(k+j+1)%ois];
+ if (idx==b && idxn==a) {//already have b!
+ break;
+ }
+ if (idx!=a) {
+ f.indices.insert(i+1,idx);
+ i++;
+ merged++;
+ }
+ Edge e2(idx,idxn);
+
+ Map<Edge,RetFaceConnect>::Element *F2=ret_edges.find(e2);
+
+ ERR_CONTINUE(!F2);
+ //change faceconnect, point to this face instead
+ if (F2->get().left == O)
+ F2->get().left=E;
+ else if (F2->get().right == O)
+ F2->get().right=E;
+
+ }
+
+ break;
+ }
+ }
+
+
+ ret_edges.erase(F); //remove the edge
+ ret_faces.erase(O); //remove the face
+
+
+ }
+
+ }
+
+ }
+
+ //fill mesh
+ r_mesh.faces.clear();
+ r_mesh.faces.resize(ret_faces.size());
+// print_line("FACECOUNT: "+itos(r_mesh.faces.size()));
+
+ int idx=0;
+ for (List<Geometry::MeshData::Face>::Element *E=ret_faces.front();E;E=E->next()) {
+ r_mesh.faces[idx++]=E->get();
+
+
+ }
+ r_mesh.edges.resize(ret_edges.size());
+ idx=0;
+ for(Map<Edge,RetFaceConnect>::Element *E=ret_edges.front();E;E=E->next()) {
+
+ Geometry::MeshData::Edge e;
+ e.a=E->key().vertices[0];
+ e.b=E->key().vertices[1];
+ r_mesh.edges[idx++]=e;
+ }
+
+ r_mesh.vertices=p_points;
+
+ //r_mesh.optimize_vertices();
+/*
+ print_line("FACES: "+itos(r_mesh.faces.size()));
+ print_line("EDGES: "+itos(r_mesh.edges.size()));
+ print_line("VERTICES: "+itos(r_mesh.vertices.size()));
+*/
+
+ return OK;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-03 15:37:48 +0000