diff --git a/modules/csg/csg.cpp b/modules/csg/csg.cpp index cc3bbed27f7..440019ed843 100644 --- a/modules/csg/csg.cpp +++ b/modules/csg/csg.cpp @@ -29,19 +29,159 @@ /*************************************************************************/ #include "csg.h" -#include "core/math/face3.h" -#include "core/math/geometry.h" -#include "core/os/os.h" -#include "core/sort_array.h" -#include "thirdparty/misc/triangulator.h" -void CSGBrush::clear() { - faces.clear(); +#include "core/math/geometry.h" +#include "core/math/math_funcs.h" +#include "core/sort_array.h" + +// Static helper functions. + +inline static bool is_snapable(const Vector3 &p_point1, const Vector3 &p_point2, real_t p_distance) { + + return (p_point1 - p_point2).length_squared() < p_distance * p_distance; +} + +inline static Vector2 interpolate_segment_uv(const Vector2 p_segement_points[2], const Vector2 p_uvs[2], const Vector2 &p_interpolation_point) { + + float segment_length = (p_segement_points[1] - p_segement_points[0]).length(); + if (segment_length < CMP_EPSILON) + return p_uvs[0]; + + float distance = (p_interpolation_point - p_segement_points[0]).length(); + float fraction = distance / segment_length; + + return p_uvs[0].linear_interpolate(p_uvs[1], fraction); +} + +inline static Vector2 interpolate_triangle_uv(const Vector2 p_vertices[3], const Vector2 p_uvs[3], const Vector2 &p_interpolation_point) { + + if (p_interpolation_point.distance_squared_to(p_vertices[0]) < CMP_EPSILON2) + return p_uvs[0]; + if (p_interpolation_point.distance_squared_to(p_vertices[1]) < CMP_EPSILON2) + return p_uvs[1]; + if (p_interpolation_point.distance_squared_to(p_vertices[2]) < CMP_EPSILON2) + return p_uvs[2]; + + Vector2 edge1 = p_vertices[1] - p_vertices[0]; + Vector2 edge2 = p_vertices[2] - p_vertices[0]; + Vector2 interpolation = p_interpolation_point - p_vertices[0]; + + float edge1_on_edge1 = edge1.dot(edge1); + float edge1_on_edge2 = edge1.dot(edge2); + float edge2_on_edge2 = edge2.dot(edge2); + float inter_on_edge1 = interpolation.dot(edge1); + float inter_on_edge2 = interpolation.dot(edge2); + float scale = (edge1_on_edge1 * edge2_on_edge2 - edge1_on_edge2 * edge1_on_edge2); + if (scale == 0) + return p_uvs[0]; + + float v = (edge2_on_edge2 * inter_on_edge1 - edge1_on_edge2 * inter_on_edge2) / scale; + float w = (edge1_on_edge1 * inter_on_edge2 - edge1_on_edge2 * inter_on_edge1) / scale; + float u = 1.0f - v - w; + + return p_uvs[0] * u + p_uvs[1] * v + p_uvs[2] * w; +} + +static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 p_vertices[3], float p_tolerance, Vector3 &r_intersection_point) { + + Vector3 edge1 = p_vertices[1] - p_vertices[0]; + Vector3 edge2 = p_vertices[2] - p_vertices[0]; + Vector3 h = p_dir.cross(edge2); + real_t a = edge1.dot(h); + // Check if ray is parrallel to triangle. + if (Math::is_zero_approx(a)) + return false; + real_t f = 1.0 / a; + + Vector3 s = p_from - p_vertices[0]; + real_t u = f * s.dot(h); + if (u < 0.0 - p_tolerance || u > 1.0 + p_tolerance) + return false; + + Vector3 q = s.cross(edge1); + real_t v = f * p_dir.dot(q); + if (v < 0.0 - p_tolerance || u + v > 1.0 + p_tolerance) + return false; + + // Ray intersects triangle. + // Calculate distance. + real_t t = f * edge2.dot(q); + // Confirm triangle is in front of ray. + if (t >= p_tolerance) { + r_intersection_point = p_from + p_dir * t; + return true; + } else + return false; +} + +inline bool is_point_in_triangle(const Vector3 &p_point, const Vector3 p_vertices[3], int p_shifted = 0) { + + real_t det = p_vertices[0].dot(p_vertices[1].cross(p_vertices[2])); + + // If determinant is, zero try shift the triangle and the point. + if (Math::is_zero_approx(det)) { + if (p_shifted > 2) { + // Triangle appears degenerate, so ignore it. + return false; + } + Vector3 shift_by; + shift_by[p_shifted] = 1; + Vector3 shifted_point = p_point + shift_by; + Vector3 shifted_vertices[3] = { p_vertices[0] + shift_by, p_vertices[1] + shift_by, p_vertices[2] + shift_by }; + return is_point_in_triangle(shifted_point, shifted_vertices, p_shifted + 1); + } + + // Find the barycentric coordinates of the point with respect to the vertices. + real_t lambda[3]; + lambda[0] = p_vertices[1].cross(p_vertices[2]).dot(p_point) / det; + lambda[1] = p_vertices[2].cross(p_vertices[0]).dot(p_point) / det; + lambda[2] = p_vertices[0].cross(p_vertices[1]).dot(p_point) / det; + + // Point is in the plane if all lambdas sum to 1. + if (!Math::is_equal_approx(lambda[0] + lambda[1] + lambda[2], 1)) return false; + + // Point is inside the triangle if all lambdas are positive. + if (lambda[0] < 0 || lambda[1] < 0 || lambda[2] < 0) return false; + + return true; +} + +inline static bool are_segements_parallel(const Vector2 p_segment1_points[2], const Vector2 p_segment2_points[2], float p_vertex_snap2) { + + Vector2 segment1 = p_segment1_points[1] - p_segment1_points[0]; + Vector2 segment2 = p_segment2_points[1] - p_segment2_points[0]; + real_t segment1_length2 = segment1.dot(segment1); + real_t segment2_length2 = segment2.dot(segment2); + real_t segment_onto_segment = segment2.dot(segment1); + + if (segment1_length2 < p_vertex_snap2 || segment2_length2 < p_vertex_snap2) + return true; + + real_t max_separation2; + if (segment1_length2 > segment2_length2) { + max_separation2 = segment2_length2 - segment_onto_segment * segment_onto_segment / segment1_length2; + } else { + max_separation2 = segment1_length2 - segment_onto_segment * segment_onto_segment / segment2_length2; + } + + return max_separation2 < p_vertex_snap2; +} + +// CSGBrush + +void CSGBrush::_regen_face_aabbs() { + + for (int i = 0; i < faces.size(); i++) { + faces.write[i].aabb = AABB(); + faces.write[i].aabb.position = faces[i].vertices[0]; + faces.write[i].aabb.expand_to(faces[i].vertices[1]); + faces.write[i].aabb.expand_to(faces[i].vertices[2]); + } } void CSGBrush::build_from_faces(const PoolVector &p_vertices, const PoolVector &p_uvs, const PoolVector &p_smooth, const PoolVector > &p_materials, const PoolVector &p_invert_faces) { - clear(); + faces.clear(); int vc = p_vertices.size(); @@ -62,37 +202,42 @@ void CSGBrush::build_from_faces(const PoolVector &p_vertices, const Poo faces.resize(p_vertices.size() / 3); for (int i = 0; i < faces.size(); i++) { + Face &f = faces.write[i]; f.vertices[0] = rv[i * 3 + 0]; f.vertices[1] = rv[i * 3 + 1]; f.vertices[2] = rv[i * 3 + 2]; + if (uvc == vc) { f.uvs[0] = ruv[i * 3 + 0]; f.uvs[1] = ruv[i * 3 + 1]; f.uvs[2] = ruv[i * 3 + 2]; } - if (sc == vc / 3) { - f.smooth = rs[i]; - } else { - f.smooth = false; - } - if (ic == vc / 3) { + if (sc == vc / 3) + f.smooth = rs[i]; + else + f.smooth = false; + + if (ic == vc / 3) f.invert = ri[i]; - } else { + else f.invert = false; - } if (mc == vc / 3) { + Ref mat = rm[i]; if (mat.is_valid()) { + const Map, int>::Element *E = material_map.find(mat); + if (E) { f.material = E->get(); } else { f.material = material_map.size(); material_map[mat] = f.material; } + } else { f.material = -1; } @@ -107,17 +252,6 @@ void CSGBrush::build_from_faces(const PoolVector &p_vertices, const Poo _regen_face_aabbs(); } -void CSGBrush::_regen_face_aabbs() { - - for (int i = 0; i < faces.size(); i++) { - - faces.write[i].aabb.position = faces[i].vertices[0]; - faces.write[i].aabb.expand_to(faces[i].vertices[1]); - faces.write[i].aabb.expand_to(faces[i].vertices[2]); - faces.write[i].aabb.grow_by(faces[i].aabb.get_longest_axis_size() * 0.001); //make it a tad bigger to avoid num precision errors - } -} - void CSGBrush::copy_from(const CSGBrush &p_brush, const Transform &p_xform) { faces = p_brush.faces; @@ -132,908 +266,218 @@ void CSGBrush::copy_from(const CSGBrush &p_brush, const Transform &p_xform) { _regen_face_aabbs(); } -//////////////////////// +// CSGBrushOperation -void CSGBrushOperation::BuildPoly::create(const CSGBrush *p_brush, int p_face, MeshMerge &mesh_merge, bool p_for_B) { +void CSGBrushOperation::merge_brushes(Operation p_operation, const CSGBrush &p_brush_a, const CSGBrush &p_brush_b, CSGBrush &r_merged_brush, float p_vertex_snap) { - //creates the initial face that will be used for clipping against the other faces - - Vector3 va[3] = { - p_brush->faces[p_face].vertices[0], - p_brush->faces[p_face].vertices[1], - p_brush->faces[p_face].vertices[2], - }; - - plane = Plane(va[0], va[1], va[2]); - - to_world.origin = va[0]; - - to_world.basis.set_axis(2, plane.normal); - to_world.basis.set_axis(0, (va[1] - va[2]).normalized()); - to_world.basis.set_axis(1, to_world.basis.get_axis(0).cross(to_world.basis.get_axis(2)).normalized()); - - to_poly = to_world.affine_inverse(); - - face_index = p_face; - - for (int i = 0; i < 3; i++) { - - Point p; - Vector3 localp = to_poly.xform(va[i]); - p.point.x = localp.x; - p.point.y = localp.y; - p.uv = p_brush->faces[p_face].uvs[i]; - - points.push_back(p); - - ///edge - - Edge e; - e.points[0] = i; - e.points[1] = (i + 1) % 3; - e.outer = true; - edges.push_back(e); - } - - smooth = p_brush->faces[p_face].smooth; - invert = p_brush->faces[p_face].invert; - - if (p_brush->faces[p_face].material != -1) { - material = p_brush->materials[p_brush->faces[p_face].material]; - } - - base_edges = 3; -} - -static Vector2 interpolate_uv(const Vector2 &p_vertex_a, const Vector2 &p_vertex_b, const Vector2 &p_vertex_c, const Vector2 &p_uv_a, const Vector2 &p_uv_c) { - - float len_a_c = (p_vertex_c - p_vertex_a).length(); - if (len_a_c < CMP_EPSILON) { - return p_uv_a; - } - - float len_a_b = (p_vertex_b - p_vertex_a).length(); - - float c = len_a_b / len_a_c; - - return p_uv_a.linear_interpolate(p_uv_c, c); -} - -static Vector2 interpolate_triangle_uv(const Vector2 &p_pos, const Vector2 *p_vtx, const Vector2 *p_uv) { - - if (p_pos.distance_squared_to(p_vtx[0]) < CMP_EPSILON2) { - return p_uv[0]; - } - if (p_pos.distance_squared_to(p_vtx[1]) < CMP_EPSILON2) { - return p_uv[1]; - } - if (p_pos.distance_squared_to(p_vtx[2]) < CMP_EPSILON2) { - return p_uv[2]; - } - - Vector2 v0 = p_vtx[1] - p_vtx[0]; - Vector2 v1 = p_vtx[2] - p_vtx[0]; - Vector2 v2 = p_pos - p_vtx[0]; - - 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 p_uv[0]; - } - float v = (d11 * d20 - d01 * d21) / denom; - float w = (d00 * d21 - d01 * d20) / denom; - float u = 1.0f - v - w; - - return p_uv[0] * u + p_uv[1] * v + p_uv[2] * w; -} - -void CSGBrushOperation::BuildPoly::_clip_segment(const CSGBrush *p_brush, int p_face, const Vector2 *segment, MeshMerge &mesh_merge, bool p_for_B) { - - //keep track of what was inserted - Vector inserted_points; - - //keep track of point indices for what was inserted, allowing reuse of points. - int segment_idx[2] = { -1, -1 }; - - //check if edge and poly share a vertex, of so, assign it to segment_idx - for (int i = 0; i < points.size(); i++) { - for (int j = 0; j < 2; j++) { - if (segment[j].is_equal_approx(points[i].point)) { - segment_idx[j] = i; - inserted_points.push_back(i); - break; + // Check for face collisions and add necessary faces. + Build2DFaceCollection build2DFaceCollection; + for (int i = 0; i < p_brush_a.faces.size(); i++) { + for (int j = 0; j < p_brush_b.faces.size(); j++) { + if (p_brush_a.faces[i].aabb.intersects_inclusive(p_brush_b.faces[j].aabb)) { + update_faces(p_brush_a, i, p_brush_b, j, build2DFaceCollection, p_vertex_snap); } } } - //check if both segment points are shared with other vertices - if (segment_idx[0] != -1 && segment_idx[1] != -1) { + // Add faces to MeshMerge. + MeshMerge mesh_merge; + mesh_merge.vertex_snap = p_vertex_snap; - if (segment_idx[0] == segment_idx[1]) { - return; //segment was too tiny, both mapped to same point + for (int i = 0; i < p_brush_a.faces.size(); i++) { + + Ref material; + if (p_brush_a.faces[i].material != -1) { + material = p_brush_a.materials[p_brush_a.faces[i].material]; } - bool found = false; - - //check if the segment already exists - for (int i = 0; i < edges.size(); i++) { - - if ( - (edges[i].points[0] == segment_idx[0] && edges[i].points[1] == segment_idx[1]) || - (edges[i].points[0] == segment_idx[1] && edges[i].points[1] == segment_idx[0])) { - found = true; - break; - } - } - - if (found) { - //it does already exist, do nothing - return; - } - - //directly add the new segment - Edge new_edge; - new_edge.points[0] = segment_idx[0]; - new_edge.points[1] = segment_idx[1]; - edges.push_back(new_edge); - return; - } - - //check edge by edge against the segment points to see if intersects - - for (int i = 0; i < base_edges; i++) { - - //if a point is shared with one of the edge points, then this edge must not be tested, as it will result in a numerical precision error. - bool edge_valid = true; - for (int j = 0; j < 2; j++) { - - if (edges[i].points[0] == segment_idx[0] || edges[i].points[1] == segment_idx[1] || edges[i].points[0] == segment_idx[1] || edges[i].points[1] == segment_idx[0]) { - edge_valid = false; //segment has this point, can't check against this - break; - } - } - - if (!edge_valid) //already hit a point in this edge, so don't test it - continue; - - //see if either points are within the edge isntead of crossing it - Vector2 res; - bool found = false; - int assign_segment_id = -1; - - for (int j = 0; j < 2; j++) { - - Vector2 edgeseg[2] = { points[edges[i].points[0]].point, points[edges[i].points[1]].point }; - Vector2 closest = Geometry::get_closest_point_to_segment_2d(segment[j], edgeseg); - - if (closest.is_equal_approx(segment[j])) { - //point rest of this edge - res = closest; - found = true; - assign_segment_id = j; - } - } - - //test if the point crosses the edge - if (!found && Geometry::segment_intersects_segment_2d(segment[0], segment[1], points[edges[i].points[0]].point, points[edges[i].points[1]].point, &res)) { - //point does cross the edge - found = true; - } - - //check whether an intersection against the segment happened - if (found) { - - //It did! so first, must slice the segment - Point new_point; - new_point.point = res; - //make sure to interpolate UV too - new_point.uv = interpolate_uv(points[edges[i].points[0]].point, new_point.point, points[edges[i].points[1]].point, points[edges[i].points[0]].uv, points[edges[i].points[1]].uv); - - int point_idx = points.size(); - points.push_back(new_point); - - //split the edge in 2 - Edge new_edge; - new_edge.points[0] = edges[i].points[0]; - new_edge.points[1] = point_idx; - new_edge.outer = edges[i].outer; - edges.write[i].points[0] = point_idx; - edges.insert(i, new_edge); - i++; //skip newly inserted edge - base_edges++; //will need an extra one in the base triangle - if (assign_segment_id >= 0) { - //point did split a segment, so make sure to remember this - segment_idx[assign_segment_id] = point_idx; - } - inserted_points.push_back(point_idx); - } - } - - //final step: after cutting the original triangle, try to see if we can still insert - //this segment - - //if already inserted two points, just use them for a segment - - if (inserted_points.size() >= 2) { //should never be >2 on non-manifold geometry, but cope with error - //two points were inserted, create the new edge - Edge new_edge; - new_edge.points[0] = inserted_points[0]; - new_edge.points[1] = inserted_points[1]; - edges.push_back(new_edge); - return; - } - - // One or no points were inserted (besides splitting), so try to see if extra points can be placed inside the triangle. - // This needs to be done here, after the previous tests were exhausted - for (int i = 0; i < 2; i++) { - - if (segment_idx[i] != -1) - continue; //already assigned to something, so skip - - //check whether one of the segment endpoints is inside the triangle. If it is, this points needs to be inserted - if (Geometry::is_point_in_triangle(segment[i], points[0].point, points[1].point, points[2].point)) { - - Point new_point; - new_point.point = segment[i]; - - Vector2 point3[3] = { points[0].point, points[1].point, points[2].point }; - Vector2 uv3[3] = { points[0].uv, points[1].uv, points[2].uv }; - - new_point.uv = interpolate_triangle_uv(new_point.point, point3, uv3); - - int point_idx = points.size(); - points.push_back(new_point); - inserted_points.push_back(point_idx); - } - } - - //check again whether two points were inserted, if so then create the new edge - if (inserted_points.size() >= 2) { //should never be >2 on non-manifold geometry, but cope with error - Edge new_edge; - new_edge.points[0] = inserted_points[0]; - new_edge.points[1] = inserted_points[1]; - edges.push_back(new_edge); - } -} - -void CSGBrushOperation::BuildPoly::clip(const CSGBrush *p_brush, int p_face, MeshMerge &mesh_merge, bool p_for_B) { - - //Clip function.. find triangle points that will be mapped to the plane and form a segment - - Vector2 segment[3]; //2D - - int src_points = 0; - - for (int i = 0; i < 3; i++) { - Vector3 p = p_brush->faces[p_face].vertices[i]; - if (plane.has_point(p)) { - Vector3 pp = plane.project(p); - pp = to_poly.xform(pp); - segment[src_points++] = Vector2(pp.x, pp.y); + if (build2DFaceCollection.build2DFacesA.has(i)) { + build2DFaceCollection.build2DFacesA[i].addFacesToMesh(mesh_merge, p_brush_a.faces[i].smooth, p_brush_a.faces[i].invert, material, false); } else { - Vector3 q = p_brush->faces[p_face].vertices[(i + 1) % 3]; - if (plane.has_point(q)) - continue; //next point is in plane, will be added eventually - if (plane.is_point_over(p) == plane.is_point_over(q)) - continue; // both on same side of the plane, don't add - - Vector3 res; - if (plane.intersects_segment(p, q, &res)) { - res = to_poly.xform(res); - segment[src_points++] = Vector2(res.x, res.y); - } - } - } - - //all above or all below, nothing to do. Should not happen though since a precheck was done before. - if (src_points == 0) - return; - - //just one point in plane is not worth doing anything - if (src_points == 1) - return; - - //transform A points to 2D - - if (segment[0].is_equal_approx(segment[1])) - return; //too small - - _clip_segment(p_brush, p_face, segment, mesh_merge, p_for_B); -} - -void CSGBrushOperation::_collision_callback(const CSGBrush *A, int p_face_a, Map &build_polys_a, const CSGBrush *B, int p_face_b, Map &build_polys_b, MeshMerge &mesh_merge) { - - //construct a frame of reference for both transforms, in order to do intersection test - Vector3 va[3] = { - A->faces[p_face_a].vertices[0], - A->faces[p_face_a].vertices[1], - A->faces[p_face_a].vertices[2], - }; - Vector3 vb[3] = { - B->faces[p_face_b].vertices[0], - B->faces[p_face_b].vertices[1], - B->faces[p_face_b].vertices[2], - }; - - { - //check if either is a degenerate - if (va[0].is_equal_approx(va[1]) || va[0].is_equal_approx(va[2]) || va[1].is_equal_approx(va[2])) - return; - - if (vb[0].is_equal_approx(vb[1]) || vb[0].is_equal_approx(vb[2]) || vb[1].is_equal_approx(vb[2])) - return; - } - - { - //check if points are the same - int equal_count = 0; - - for (int i = 0; i < 3; i++) { - + Vector3 points[3]; + Vector2 uvs[3]; for (int j = 0; j < 3; j++) { - if (va[i].distance_to(vb[j]) < mesh_merge.vertex_snap) { - equal_count++; - break; - } + points[j] = p_brush_a.faces[i].vertices[j]; + uvs[j] = p_brush_a.faces[i].uvs[j]; } - } - - //if 2 or 3 points are the same, there is no point in doing anything. They can't - //be clipped either, so add both. - if (equal_count == 2 || equal_count == 3) { - return; + mesh_merge.add_face(points, uvs, p_brush_a.faces[i].smooth, p_brush_a.faces[i].invert, material, false); } } - // do a quick pre-check for no-intersection using the SAT theorem + for (int i = 0; i < p_brush_b.faces.size(); i++) { - { - - //b under or over a plane - int over_count = 0, in_plane_count = 0, under_count = 0; - Plane plane_a(va[0], va[1], va[2]); - if (plane_a.normal == Vector3()) { - return; //degenerate + Ref material; + if (p_brush_b.faces[i].material != -1) { + material = p_brush_b.materials[p_brush_b.faces[i].material]; } - for (int i = 0; i < 3; i++) { - if (plane_a.has_point(vb[i])) - in_plane_count++; - else if (plane_a.is_point_over(vb[i])) - over_count++; - else - under_count++; - } - - if (over_count == 0 || under_count == 0) - return; //no intersection, something needs to be under AND over - - //a under or over b plane - over_count = 0; - under_count = 0; - in_plane_count = 0; - - Plane plane_b(vb[0], vb[1], vb[2]); - if (plane_b.normal == Vector3()) - return; //degenerate - - for (int i = 0; i < 3; i++) { - if (plane_b.has_point(va[i])) - in_plane_count++; - else if (plane_b.is_point_over(va[i])) - over_count++; - else - under_count++; - } - - if (over_count == 0 || under_count == 0) - return; //no intersection, something needs to be under AND over - - //edge pairs (cross product combinations), see SAT theorem - - for (int i = 0; i < 3; i++) { - - Vector3 axis_a = (va[i] - va[(i + 1) % 3]).normalized(); - + if (build2DFaceCollection.build2DFacesB.has(i)) { + build2DFaceCollection.build2DFacesB[i].addFacesToMesh(mesh_merge, p_brush_b.faces[i].smooth, p_brush_b.faces[i].invert, material, true); + } else { + Vector3 points[3]; + Vector2 uvs[3]; for (int j = 0; j < 3; j++) { + points[j] = p_brush_b.faces[i].vertices[j]; + uvs[j] = p_brush_b.faces[i].uvs[j]; + } + mesh_merge.add_face(points, uvs, p_brush_b.faces[i].smooth, p_brush_b.faces[i].invert, material, true); + } + } - Vector3 axis_b = (vb[j] - vb[(j + 1) % 3]).normalized(); + // Mark faces that ended up inside the intersection. + mesh_merge.mark_inside_faces(); - Vector3 sep_axis = axis_a.cross(axis_b); - if (sep_axis == Vector3()) - continue; //colineal - sep_axis.normalize(); + // Create new brush and fill with new faces. + r_merged_brush.faces.clear(); - real_t min_a = 1e20, max_a = -1e20; - real_t min_b = 1e20, max_b = -1e20; + switch (p_operation) { - for (int k = 0; k < 3; k++) { - real_t d = sep_axis.dot(va[k]); - min_a = MIN(min_a, d); - max_a = MAX(max_a, d); - d = sep_axis.dot(vb[k]); - min_b = MIN(min_b, d); - max_b = MAX(max_b, d); + case OPERATION_UNION: { + + int outside_count = 0; + + for (int i = 0; i < mesh_merge.faces.size(); i++) { + if (mesh_merge.faces[i].inside) + continue; + outside_count++; + } + + r_merged_brush.faces.resize(outside_count); + + outside_count = 0; + + for (int i = 0; i < mesh_merge.faces.size(); i++) { + + if (mesh_merge.faces[i].inside) + continue; + + for (int j = 0; j < 3; j++) { + r_merged_brush.faces.write[outside_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; + r_merged_brush.faces.write[outside_count].uvs[j] = mesh_merge.faces[i].uvs[j]; } - min_b -= (max_a - min_a) * 0.5; - max_b += (max_a - min_a) * 0.5; + r_merged_brush.faces.write[outside_count].smooth = mesh_merge.faces[i].smooth; + r_merged_brush.faces.write[outside_count].invert = mesh_merge.faces[i].invert; + r_merged_brush.faces.write[outside_count].material = mesh_merge.faces[i].material_idx; + outside_count++; + } - real_t dmin = min_b - (min_a + max_a) * 0.5; - real_t dmax = max_b - (min_a + max_a) * 0.5; + r_merged_brush._regen_face_aabbs(); - if (dmin > CMP_EPSILON || dmax < -CMP_EPSILON) { - return; //does not contain zero, so they don't overlap + } break; + + case OPERATION_INTERSECTION: { + + int inside_count = 0; + + for (int i = 0; i < mesh_merge.faces.size(); i++) { + if (!mesh_merge.faces[i].inside) + continue; + inside_count++; + } + + r_merged_brush.faces.resize(inside_count); + + inside_count = 0; + + for (int i = 0; i < mesh_merge.faces.size(); i++) { + + if (!mesh_merge.faces[i].inside) + continue; + + for (int j = 0; j < 3; j++) { + r_merged_brush.faces.write[inside_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; + r_merged_brush.faces.write[inside_count].uvs[j] = mesh_merge.faces[i].uvs[j]; } - } - } - } - //if we are still here, it means they most likely intersect, so create BuildPolys if they don't exist - - BuildPoly *poly_a = NULL; - - if (!build_polys_a.has(p_face_a)) { - - BuildPoly bp; - bp.create(A, p_face_a, mesh_merge, false); - build_polys_a[p_face_a] = bp; - } - - poly_a = &build_polys_a[p_face_a]; - - BuildPoly *poly_b = NULL; - - if (!build_polys_b.has(p_face_b)) { - - BuildPoly bp; - bp.create(B, p_face_b, mesh_merge, true); - build_polys_b[p_face_b] = bp; - } - - poly_b = &build_polys_b[p_face_b]; - - //clip each other, this could be improved by using vertex unique IDs (more vertices may be shared instead of using snap) - poly_a->clip(B, p_face_b, mesh_merge, false); - poly_b->clip(A, p_face_a, mesh_merge, true); -} - -void CSGBrushOperation::_add_poly_points(const BuildPoly &p_poly, int p_edge, int p_from_point, int p_to_point, const Vector > &vertex_process, Vector &edge_process, Vector &r_poly) { - - //this function follows the polygon points counter clockwise and adds them. It creates lists of unique polygons - //every time an unused edge is found, it's pushed to a stack and continues from there. - - List edge_stack; - - { - EdgeSort es; - es.angle = 0; //won't be checked here - es.edge = p_edge; - es.prev_point = p_from_point; - es.edge_point = p_to_point; - - edge_stack.push_back(es); - } - - //attempt to empty the stack. - while (edge_stack.size()) { - - EdgeSort e = edge_stack.front()->get(); - edge_stack.pop_front(); - - if (edge_process[e.edge]) { - //nothing to do here - continue; - } - - Vector points; - points.push_back(e.prev_point); - - int prev_point = e.prev_point; - int to_point = e.edge_point; - int current_edge = e.edge; - - edge_process.write[e.edge] = true; //mark as processed - - int limit = p_poly.points.size() * 4; //avoid infinite recursion - - while (to_point != e.prev_point && limit) { - - Vector2 segment[2] = { p_poly.points[prev_point].point, p_poly.points[to_point].point }; - - //construct a basis transform from the segment, which will be used to check the angle - Transform2D t2d; - t2d[0] = (segment[1] - segment[0]).normalized(); //use as Y - t2d[1] = Vector2(-t2d[0].y, t2d[0].x); // use as tangent - t2d[2] = segment[1]; //origin - - if (t2d.basis_determinant() == 0) - break; //abort poly - - t2d.affine_invert(); - - //push all edges found here, they will be sorted by minimum angle later. - Vector next_edges; - - for (int i = 0; i < vertex_process[to_point].size(); i++) { - - int edge = vertex_process[to_point][i]; - int opposite_point = p_poly.edges[edge].points[0] == to_point ? p_poly.edges[edge].points[1] : p_poly.edges[edge].points[0]; - if (opposite_point == prev_point) - continue; //not going back - - EdgeSort e2; - Vector2 local_vec = t2d.xform(p_poly.points[opposite_point].point); - e2.angle = -local_vec.angle(); //negate so we can sort by minimum angle - e2.edge = edge; - e2.edge_point = opposite_point; - e2.prev_point = to_point; - - next_edges.push_back(e2); + r_merged_brush.faces.write[inside_count].smooth = mesh_merge.faces[i].smooth; + r_merged_brush.faces.write[inside_count].invert = mesh_merge.faces[i].invert; + r_merged_brush.faces.write[inside_count].material = mesh_merge.faces[i].material_idx; + inside_count++; } - //finally, sort by minimum angle - next_edges.sort(); + r_merged_brush._regen_face_aabbs(); - int next_point = -1; - int next_edge = -1; + } break; - for (int i = 0; i < next_edges.size(); i++) { + case OPERATION_SUBSTRACTION: { - if (i == 0) { - //minimum angle found is the next point - next_point = next_edges[i].edge_point; - next_edge = next_edges[i].edge; + int face_count = 0; - } else { - //the rest are pushed to the stack IF they were not processed yet. - if (!edge_process[next_edges[i].edge]) { - edge_stack.push_back(next_edges[i]); - } + for (int i = 0; i < mesh_merge.faces.size(); i++) { + if (mesh_merge.faces[i].from_b && !mesh_merge.faces[i].inside) + continue; + if (!mesh_merge.faces[i].from_b && mesh_merge.faces[i].inside) + continue; + face_count++; + } + + r_merged_brush.faces.resize(face_count); + + face_count = 0; + + for (int i = 0; i < mesh_merge.faces.size(); i++) { + + if (mesh_merge.faces[i].from_b && !mesh_merge.faces[i].inside) + continue; + if (!mesh_merge.faces[i].from_b && mesh_merge.faces[i].inside) + continue; + + for (int j = 0; j < 3; j++) { + r_merged_brush.faces.write[face_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; + r_merged_brush.faces.write[face_count].uvs[j] = mesh_merge.faces[i].uvs[j]; } + + if (mesh_merge.faces[i].from_b) { + //invert facing of insides of B + SWAP(r_merged_brush.faces.write[face_count].vertices[1], r_merged_brush.faces.write[face_count].vertices[2]); + SWAP(r_merged_brush.faces.write[face_count].uvs[1], r_merged_brush.faces.write[face_count].uvs[2]); + } + + r_merged_brush.faces.write[face_count].smooth = mesh_merge.faces[i].smooth; + r_merged_brush.faces.write[face_count].invert = mesh_merge.faces[i].invert; + r_merged_brush.faces.write[face_count].material = mesh_merge.faces[i].material_idx; + face_count++; } - if (next_edge == -1) { - //did not find anything, may be a dead-end edge (this should normally not happen) - //just flip the direction and go back - next_point = prev_point; - next_edge = current_edge; - } + r_merged_brush._regen_face_aabbs(); - points.push_back(to_point); + } break; + } - prev_point = to_point; - to_point = next_point; - edge_process.write[next_edge] = true; //mark this edge as processed - current_edge = next_edge; - - limit--; - } - - //if more than 2 points were added to the polygon, add it to the list of polygons. - if (points.size() > 2) { - PolyPoints pp; - pp.points = points; - r_poly.push_back(pp); - } + // Update the list of materials. + r_merged_brush.materials.resize(mesh_merge.materials.size()); + for (const Map, int>::Element *E = mesh_merge.materials.front(); E; E = E->next()) { + r_merged_brush.materials.write[E->get()] = E->key(); } } -void CSGBrushOperation::_add_poly_outline(const BuildPoly &p_poly, int p_from_point, int p_to_point, const Vector > &vertex_process, Vector &r_outline) { +// CSGBrushOperation::MeshMerge - //this is the opposite of the function above. It adds polygon outlines instead. - //this is used for triangulating holes. - //no stack is used here because only the bigger outline is interesting. - - r_outline.push_back(p_from_point); - - int prev_point = p_from_point; - int to_point = p_to_point; - - int limit = p_poly.points.size() * 4; //avoid infinite recursion - - while (to_point != p_from_point && limit) { - - Vector2 segment[2] = { p_poly.points[prev_point].point, p_poly.points[to_point].point }; - //again create a transform to compute the angle. - Transform2D t2d; - t2d[0] = (segment[1] - segment[0]).normalized(); //use as Y - t2d[1] = Vector2(-t2d[0].y, t2d[0].x); // use as tangent - t2d[2] = segment[1]; //origin - - if (t2d.basis_determinant() == 0) - break; //abort poly - - t2d.affine_invert(); - - float max_angle = 0; - int next_point_angle = -1; - - for (int i = 0; i < vertex_process[to_point].size(); i++) { - - int edge = vertex_process[to_point][i]; - int opposite_point = p_poly.edges[edge].points[0] == to_point ? p_poly.edges[edge].points[1] : p_poly.edges[edge].points[0]; - if (opposite_point == prev_point) - continue; //not going back - - float angle = -t2d.xform(p_poly.points[opposite_point].point).angle(); - if (next_point_angle == -1 || angle > max_angle) { //same as before but use greater to check. - max_angle = angle; - next_point_angle = opposite_point; - } - } - - if (next_point_angle == -1) { - //go back because no route found - next_point_angle = prev_point; - } - - r_outline.push_back(to_point); - prev_point = to_point; - to_point = next_point_angle; - - limit--; - } -} - -void CSGBrushOperation::_merge_poly(MeshMerge &mesh, int p_face_idx, const BuildPoly &p_poly, bool p_from_b) { - - //finally, merge the 2D polygon back to 3D - - Vector > vertex_process; - Vector edge_process; - - vertex_process.resize(p_poly.points.size()); - edge_process.resize(p_poly.edges.size()); - - //none processed by default - for (int i = 0; i < edge_process.size(); i++) { - edge_process.write[i] = false; - } - - //put edges in points, so points can go through them - for (int i = 0; i < p_poly.edges.size(); i++) { - vertex_process.write[p_poly.edges[i].points[0]].push_back(i); - vertex_process.write[p_poly.edges[i].points[1]].push_back(i); - } - - Vector polys; - - //process points that were not processed - for (int i = 0; i < edge_process.size(); i++) { - if (edge_process[i]) - continue; //already processed - - int intersect_poly = -1; - - if (i > 0) { - //this is disconnected, so it's clearly a hole. lets find where it belongs - Vector2 ref_point = p_poly.points[p_poly.edges[i].points[0]].point; - - for (int j = 0; j < polys.size(); j++) { - - //find a point outside poly - Vector2 out_point(-1e20, -1e20); - - const PolyPoints &pp = polys[j]; - - for (int k = 0; k < pp.points.size(); k++) { - Vector2 p = p_poly.points[pp.points[k]].point; - out_point.x = MAX(out_point.x, p.x); - out_point.y = MAX(out_point.y, p.y); - } - - out_point += Vector2(0.12341234, 0.4123412); // move to a random place to avoid direct edge-point chances - - int intersections = 0; - - for (int k = 0; k < pp.points.size(); k++) { - Vector2 p1 = p_poly.points[pp.points[k]].point; - Vector2 p2 = p_poly.points[pp.points[(k + 1) % pp.points.size()]].point; - - if (Geometry::segment_intersects_segment_2d(ref_point, out_point, p1, p2, NULL)) { - intersections++; - } - } - - if (intersections % 2 == 1) { - //hole is inside this poly - intersect_poly = j; - break; - } - } - } - - if (intersect_poly != -1) { - //must add this as a hole - Vector outline; - _add_poly_outline(p_poly, p_poly.edges[i].points[0], p_poly.edges[i].points[1], vertex_process, outline); - - if (outline.size() > 1) { - polys.write[intersect_poly].holes.push_back(outline); - } - } - _add_poly_points(p_poly, i, p_poly.edges[i].points[0], p_poly.edges[i].points[1], vertex_process, edge_process, polys); - } - - //get rid of holes, not the most optiomal way, but also not a common case at all to be inoptimal - for (int i = 0; i < polys.size(); i++) { - - if (!polys[i].holes.size()) - continue; - - //repeat until no more holes are left to be merged - while (polys[i].holes.size()) { - - //try to merge a hole with the outline - bool added_hole = false; - - for (int j = 0; j < polys[i].holes.size(); j++) { - - //try hole vertices - int with_outline_vertex = -1; - int from_hole_vertex = -1; - - bool found = false; - - for (int k = 0; k < polys[i].holes[j].size(); k++) { - - int from_idx = polys[i].holes[j][k]; - Vector2 from = p_poly.points[from_idx].point; - - //try a segment from hole vertex to outline vertices - from_hole_vertex = k; - - bool valid = true; - - for (int l = 0; l < polys[i].points.size(); l++) { - - int to_idx = polys[i].points[l]; - Vector2 to = p_poly.points[to_idx].point; - with_outline_vertex = l; - - //try against outline (other points) first - - valid = true; - - for (int m = 0; m < polys[i].points.size(); m++) { - - int m_next = (m + 1) % polys[i].points.size(); - if (m == with_outline_vertex || m_next == with_outline_vertex) //do not test with edges that share this point - continue; - - if (Geometry::segment_intersects_segment_2d(from, to, p_poly.points[polys[i].points[m]].point, p_poly.points[polys[i].points[m_next]].point, NULL)) { - valid = false; - break; - } - } - - if (!valid) - continue; - - //try against all holes including self - - for (int m = 0; m < polys[i].holes.size(); m++) { - - for (int n = 0; n < polys[i].holes[m].size(); n++) { - - int n_next = (n + 1) % polys[i].holes[m].size(); - if (m == j && (n == from_hole_vertex || n_next == from_hole_vertex)) //contains vertex being tested from current hole, skip - continue; - - if (Geometry::segment_intersects_segment_2d(from, to, p_poly.points[polys[i].holes[m][n]].point, p_poly.points[polys[i].holes[m][n_next]].point, NULL)) { - valid = false; - break; - } - } - - if (!valid) - break; - } - - if (valid) //all passed! exit loop - break; - else - continue; //something went wrong, go on. - } - - if (valid) { - found = true; //if in the end this was valid, use it - break; - } - } - - if (found) { - - //hook this hole with outline, and remove from list of holes - - //duplicate point - int insert_at = with_outline_vertex; - int point = polys[i].points[insert_at]; - polys.write[i].points.insert(insert_at, point); - insert_at++; - //insert all others, outline should be backwards (must check) - int holesize = polys[i].holes[j].size(); - for (int k = 0; k <= holesize; k++) { - int idx = (from_hole_vertex + k) % holesize; - int point2 = polys[i].holes[j][idx]; - polys.write[i].points.insert(insert_at, point2); - insert_at++; - } - - added_hole = true; - polys.write[i].holes.remove(j); - break; //got rid of hole, break and continue - } - } - - ERR_BREAK(!added_hole); - } - } - - //triangulate polygons - - for (int i = 0; i < polys.size(); i++) { - - Vector vertices; - vertices.resize(polys[i].points.size()); - for (int j = 0; j < vertices.size(); j++) { - vertices.write[j] = p_poly.points[polys[i].points[j]].point; - } - - Vector indices = Geometry::triangulate_polygon(vertices); - - for (int j = 0; j < indices.size(); j += 3) { - - //obtain the vertex - - Vector3 face[3]; - Vector2 uv[3]; - float cp = Geometry::vec2_cross(p_poly.points[polys[i].points[indices[j + 0]]].point, p_poly.points[polys[i].points[indices[j + 1]]].point, p_poly.points[polys[i].points[indices[j + 2]]].point); - if (Math::abs(cp) < CMP_EPSILON) - continue; - - for (int k = 0; k < 3; k++) { - - Vector2 p = p_poly.points[polys[i].points[indices[j + k]]].point; - face[k] = p_poly.to_world.xform(Vector3(p.x, p.y, 0)); - uv[k] = p_poly.points[polys[i].points[indices[j + k]]].uv; - } - - mesh.add_face(face[0], face[1], face[2], uv[0], uv[1], uv[2], p_poly.smooth, p_poly.invert, p_poly.material, p_from_b); - } - } -} - -//use a limit to speed up bvh and limit the depth +// Use a limit to speed up bvh and limit the depth. #define BVH_LIMIT 8 -int CSGBrushOperation::MeshMerge::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) { +int CSGBrushOperation::MeshMerge::_create_bvh(FaceBVH *facebvhptr, FaceBVH **facebvhptrptr, int p_from, int p_size, int p_depth, int &r_max_depth, int &r_max_alloc) { - if (p_depth > max_depth) { - max_depth = p_depth; + if (p_depth > r_max_depth) { + r_max_depth = p_depth; } if (p_size == 0) { - return -1; - } else if (p_size <= BVH_LIMIT) { + } + if (p_size <= BVH_LIMIT) { for (int i = 0; i < p_size - 1; i++) { - p_bb[p_from + i]->next = p_bb[p_from + i + 1] - p_bvh; + facebvhptrptr[p_from + i]->next = facebvhptrptr[p_from + i + 1] - facebvhptr; } - return p_bb[p_from] - p_bvh; + return facebvhptrptr[p_from] - facebvhptr; } AABB aabb; - aabb = p_bb[p_from]->aabb; + aabb = facebvhptrptr[p_from]->aabb; for (int i = 1; i < p_size; i++) { - - aabb.merge_with(p_bb[p_from + i]->aabb); + aabb.merge_with(facebvhptrptr[p_from + i]->aabb); } int li = aabb.get_longest_axis_index(); @@ -1041,28 +485,29 @@ int CSGBrushOperation::MeshMerge::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from switch (li) { case Vector3::AXIS_X: { - SortArray sort_x; - sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); + SortArray sort_x; + sort_x.nth_element(0, p_size, p_size / 2, &facebvhptrptr[p_from]); //sort_x.sort(&p_bb[p_from],p_size); } break; + case Vector3::AXIS_Y: { - SortArray sort_y; - sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); + SortArray sort_y; + sort_y.nth_element(0, p_size, p_size / 2, &facebvhptrptr[p_from]); //sort_y.sort(&p_bb[p_from],p_size); } break; - case Vector3::AXIS_Z: { - SortArray sort_z; - sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); - //sort_z.sort(&p_bb[p_from],p_size); + case Vector3::AXIS_Z: { + SortArray sort_z; + sort_z.nth_element(0, p_size, p_size / 2, &facebvhptrptr[p_from]); + //sort_z.sort(&p_bb[p_from],p_size); } break; } - int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, max_depth, max_alloc); - int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, max_depth, max_alloc); + int left = _create_bvh(facebvhptr, facebvhptrptr, p_from, p_size / 2, p_depth + 1, r_max_depth, r_max_alloc); + int right = _create_bvh(facebvhptr, facebvhptrptr, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, r_max_depth, r_max_alloc); - int index = max_alloc++; - BVH *_new = &p_bvh[index]; + int index = r_max_alloc++; + FaceBVH *_new = &facebvhptr[index]; _new->aabb = aabb; _new->center = aabb.position + aabb.size * 0.5; _new->face = -1; @@ -1073,7 +518,27 @@ int CSGBrushOperation::MeshMerge::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from return index; } -int CSGBrushOperation::MeshMerge::_bvh_count_intersections(BVH *bvhptr, int p_max_depth, int p_bvh_first, const Vector3 &p_begin, const Vector3 &p_end, int p_exclude) const { +void CSGBrushOperation::MeshMerge::_add_distance(List &r_intersectionsA, List &r_intersectionsB, bool p_from_B, real_t p_distance) const { + + List &intersections = p_from_B ? r_intersectionsB : r_intersectionsA; + + // Check if distance exists. + for (const List::Element *E = intersections.front(); E; E = E->next()) + if (Math::abs(**E - p_distance) < vertex_snap) return; + + intersections.push_back(p_distance); +} + +bool CSGBrushOperation::MeshMerge::_bvh_inside(FaceBVH *facebvhptr, int p_max_depth, int p_bvh_first, int p_face_idx) const { + + Face face = faces[p_face_idx]; + Vector3 face_points[3] = { + points[face.points[0]], + points[face.points[1]], + points[face.points[2]] + }; + Vector3 face_center = (face_points[0] + face_points[1] + face_points[2]) / 3.0; + Vector3 face_normal = Plane(face_points[0], face_points[1], face_points[2]).normal; uint32_t *stack = (uint32_t *)alloca(sizeof(int) * p_max_depth); @@ -1084,54 +549,58 @@ int CSGBrushOperation::MeshMerge::_bvh_count_intersections(BVH *bvhptr, int p_ma VISIT_DONE_BIT = 3, VISITED_BIT_SHIFT = 29, NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, - VISITED_BIT_MASK = ~NODE_IDX_MASK, - + VISITED_BIT_MASK = ~NODE_IDX_MASK }; - int intersections = 0; + List intersectionsA; + List intersectionsB; int level = 0; - - const Vector3 *vertexptr = points.ptr(); - const Face *facesptr = faces.ptr(); - AABB segment_aabb; - segment_aabb.position = p_begin; - segment_aabb.expand_to(p_end); - int pos = p_bvh_first; - stack[0] = pos; + while (true) { uint32_t node = stack[level] & NODE_IDX_MASK; - const BVH &b = bvhptr[node]; + const FaceBVH *current_facebvhptr = &(facebvhptr[node]); bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { - if (b.face >= 0) { + if (current_facebvhptr->face >= 0) { - const BVH *bp = &b; + while (current_facebvhptr) { - while (bp) { + if (p_face_idx != current_facebvhptr->face && + current_facebvhptr->aabb.intersects_ray(face_center, face_normal)) { - bool valid = segment_aabb.intersects(bp->aabb) && bp->aabb.intersects_segment(p_begin, p_end); + const Face ¤t_face = faces[current_facebvhptr->face]; + Vector3 current_points[3] = { + points[current_face.points[0]], + points[current_face.points[1]], + points[current_face.points[2]] + }; + Vector3 current_normal = Plane(current_points[0], current_points[1], current_points[2]).normal; + Vector3 intersection_point; - if (valid && p_exclude != bp->face) { - const Face &s = facesptr[bp->face]; - Face3 f3(vertexptr[s.points[0]], vertexptr[s.points[1]], vertexptr[s.points[2]]); - - Vector3 res; - - if (f3.intersects_segment(p_begin, p_end, &res)) { - intersections++; + // Check if faces are co-planar. + if ((current_normal - face_normal).length_squared() < CMP_EPSILON2 && + is_point_in_triangle(face_center, current_points)) { + // Only add an intersection if checking a B face. + if (face.from_b) + _add_distance(intersectionsA, intersectionsB, current_face.from_b, 0); + } else if (ray_intersects_triangle(face_center, face_normal, current_points, CMP_EPSILON, intersection_point)) { + real_t distance = (intersection_point - face_center).length(); + _add_distance(intersectionsA, intersectionsB, current_face.from_b, distance); } } - if (bp->next != -1) { - bp = &bvhptr[bp->next]; + + if (current_facebvhptr->next != -1) { + current_facebvhptr = &facebvhptr[current_facebvhptr->next]; } else { - bp = NULL; + current_facebvhptr = nullptr; } } @@ -1139,32 +608,33 @@ int CSGBrushOperation::MeshMerge::_bvh_count_intersections(BVH *bvhptr, int p_ma } else { - bool valid = segment_aabb.intersects(b.aabb) && b.aabb.intersects_segment(p_begin, p_end); + bool valid = current_facebvhptr->aabb.intersects_ray(face_center, face_normal); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - } else { stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } + case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.left | TEST_AABB_BIT; + stack[level + 1] = current_facebvhptr->left | TEST_AABB_BIT; level++; continue; } + case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = b.right | TEST_AABB_BIT; + stack[level + 1] = current_facebvhptr->right | TEST_AABB_BIT; level++; continue; } + case VISIT_DONE_BIT: { if (level == 0) { @@ -1180,130 +650,112 @@ int CSGBrushOperation::MeshMerge::_bvh_count_intersections(BVH *bvhptr, int p_ma break; } - return intersections; + // Inside if face normal intersects other faces an odd number of times. + return (intersectionsA.size() + intersectionsB.size()) & 1; } void CSGBrushOperation::MeshMerge::mark_inside_faces() { - // mark faces that are inside. This helps later do the boolean ops when merging. - // this approach is very brute force (with a bunch of optimizatios, such as BVH and pre AABB intersection test) + // Mark faces that are inside. This helps later do the boolean ops when merging. + // This approach is very brute force with a bunch of optimizations, + // such as BVH and pre AABB intersection test. - AABB aabb; + Vector bvhvec; + bvhvec.resize(faces.size() * 3); // Will never be larger than this (TODO: Make better) + FaceBVH *facebvh = bvhvec.ptrw(); - for (int i = 0; i < points.size(); i++) { - if (i == 0) { - aabb.position = points[i]; - } else { - aabb.expand_to(points[i]); - } - } - - float max_distance = aabb.size.length() * 1.2; - - Vector bvhvec; - bvhvec.resize(faces.size() * 3); //will never be larger than this (todo make better) - BVH *bvh = bvhvec.ptrw(); - - AABB faces_a; - AABB faces_b; + AABB aabb_a; + AABB aabb_b; bool first_a = true; bool first_b = true; for (int i = 0; i < faces.size(); i++) { - bvh[i].left = -1; - bvh[i].right = -1; - bvh[i].face = i; - bvh[i].aabb.position = points[faces[i].points[0]]; - bvh[i].aabb.expand_to(points[faces[i].points[1]]); - bvh[i].aabb.expand_to(points[faces[i].points[2]]); - bvh[i].center = bvh[i].aabb.position + bvh[i].aabb.size * 0.5; - bvh[i].next = -1; + facebvh[i].left = -1; + facebvh[i].right = -1; + facebvh[i].face = i; + facebvh[i].aabb.position = points[faces[i].points[0]]; + facebvh[i].aabb.expand_to(points[faces[i].points[1]]); + facebvh[i].aabb.expand_to(points[faces[i].points[2]]); + facebvh[i].center = facebvh[i].aabb.position + facebvh[i].aabb.size * 0.5; + facebvh[i].aabb.grow_by(vertex_snap); + facebvh[i].next = -1; + if (faces[i].from_b) { if (first_b) { - faces_b = bvh[i].aabb; + aabb_b = facebvh[i].aabb; first_b = false; } else { - faces_b.merge_with(bvh[i].aabb); + aabb_b.merge_with(facebvh[i].aabb); } } else { if (first_a) { - faces_a = bvh[i].aabb; + aabb_a = facebvh[i].aabb; first_a = false; } else { - faces_a.merge_with(bvh[i].aabb); + aabb_a.merge_with(facebvh[i].aabb); } } } - AABB intersection_aabb = faces_a.intersection(faces_b); - intersection_aabb.grow_by(intersection_aabb.get_longest_axis_size() * 0.01); //grow a little, avoid numerical error + AABB intersection_aabb = aabb_a.intersection(aabb_b); - if (intersection_aabb.size == Vector3()) //AABB do not intersect, so neither do shapes. + // Check if shape AABBs intersect. + if (intersection_aabb.size == Vector3()) return; - Vector bvhtrvec; + Vector bvhtrvec; bvhtrvec.resize(faces.size()); - BVH **bvhptr = bvhtrvec.ptrw(); + FaceBVH **bvhptr = bvhtrvec.ptrw(); for (int i = 0; i < faces.size(); i++) { - - bvhptr[i] = &bvh[i]; + bvhptr[i] = &facebvh[i]; } int max_depth = 0; int max_alloc = faces.size(); - _create_bvh(bvh, bvhptr, 0, faces.size(), 1, max_depth, max_alloc); + _create_bvh(facebvh, bvhptr, 0, faces.size(), 1, max_depth, max_alloc); for (int i = 0; i < faces.size(); i++) { - if (!intersection_aabb.intersects(bvh[i].aabb)) - continue; //not in AABB intersection, so not in face intersection - Vector3 center = points[faces[i].points[0]]; - center += points[faces[i].points[1]]; - center += points[faces[i].points[2]]; - center /= 3.0; + // Check if face AABB intersects the intersection AABB. + if (!intersection_aabb.intersects_inclusive(facebvh[i].aabb)) + continue; - Plane plane(points[faces[i].points[0]], points[faces[i].points[1]], points[faces[i].points[2]]); - Vector3 target = center + plane.normal * max_distance + Vector3(0.0001234, 0.000512, 0.00013423); //reduce chance of edge hits by doing a small increment - - int intersections = _bvh_count_intersections(bvh, max_depth, max_alloc - 1, center, target, i); - - if (intersections & 1) { + if (_bvh_inside(facebvh, max_depth, max_alloc - 1, i)) faces.write[i].inside = true; - } } } -void CSGBrushOperation::MeshMerge::add_face(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector2 &p_uv_a, const Vector2 &p_uv_b, const Vector2 &p_uv_c, bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b) { +void CSGBrushOperation::MeshMerge::add_face(const Vector3 p_points[], const Vector2 p_uvs[], bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b) { - Vector3 src_points[3] = { p_a, p_b, p_c }; - Vector2 src_uvs[3] = { p_uv_a, p_uv_b, p_uv_c }; int indices[3]; for (int i = 0; i < 3; i++) { VertexKey vk; - vk.x = int((double(src_points[i].x) + double(vertex_snap) * 0.31234) / double(vertex_snap)); - vk.y = int((double(src_points[i].y) + double(vertex_snap) * 0.31234) / double(vertex_snap)); - vk.z = int((double(src_points[i].z) + double(vertex_snap) * 0.31234) / double(vertex_snap)); + vk.x = int((double(p_points[i].x) + double(vertex_snap) * 0.31234) / double(vertex_snap)); + vk.y = int((double(p_points[i].y) + double(vertex_snap) * 0.31234) / double(vertex_snap)); + vk.z = int((double(p_points[i].z) + double(vertex_snap) * 0.31234) / double(vertex_snap)); int res; if (snap_cache.lookup(vk, res)) { indices[i] = res; } else { indices[i] = points.size(); - points.push_back(src_points[i]); + points.push_back(p_points[i]); snap_cache.set(vk, indices[i]); } } + // Don't add degenerate faces. if (indices[0] == indices[2] || indices[0] == indices[1] || indices[1] == indices[2]) - return; //not adding degenerate + return; MeshMerge::Face face; face.from_b = p_from_b; face.inside = false; face.smooth = p_smooth; face.invert = p_invert; + if (p_material.is_valid()) { if (!materials.has(p_material)) { face.material_idx = materials.size(); @@ -1316,205 +768,708 @@ void CSGBrushOperation::MeshMerge::add_face(const Vector3 &p_a, const Vector3 &p } for (int k = 0; k < 3; k++) { - face.points[k] = indices[k]; - face.uvs[k] = src_uvs[k]; - ; + face.uvs[k] = p_uvs[k]; } faces.push_back(face); } -void CSGBrushOperation::merge_brushes(Operation p_operation, const CSGBrush &p_A, const CSGBrush &p_B, CSGBrush &result, float p_snap) { +// CSGBrushOperation::Build2DFaces - CallbackData cd; - cd.self = this; - cd.A = &p_A; - cd.B = &p_B; +int CSGBrushOperation::Build2DFaces::_get_point_idx(const Vector2 &p_point) { - MeshMerge mesh_merge; - mesh_merge.vertex_snap = p_snap; + for (int vertex_idx = 0; vertex_idx < vertices.size(); ++vertex_idx) { + if ((p_point - vertices[vertex_idx].point).length_squared() < vertex_snap2) + return vertex_idx; + } + return -1; +} - //check intersections between faces. Use AABB to speed up precheck - //this generates list of buildpolys and clips them. - //this was originally BVH optimized, but its not really worth it. - for (int i = 0; i < p_A.faces.size(); i++) { - cd.face_a = i; - for (int j = 0; j < p_B.faces.size(); j++) { - if (p_A.faces[i].aabb.intersects(p_B.faces[j].aabb)) { - _collision_callback(&p_A, i, cd.build_polys_A, &p_B, j, cd.build_polys_B, mesh_merge); +int CSGBrushOperation::Build2DFaces::_add_vertex(const Vertex2D &p_vertex) { + + // Check if vertex exists. + int vertex_id = _get_point_idx(p_vertex.point); + if (vertex_id != -1) return vertex_id; + + vertices.push_back(p_vertex); + return vertices.size() - 1; +} + +void CSGBrushOperation::Build2DFaces::_add_vertex_idx_sorted(Vector &r_vertex_indices, int p_new_vertex_index) { + + if (p_new_vertex_index >= 0 && r_vertex_indices.find(p_new_vertex_index) == -1) { + ERR_FAIL_COND_MSG(p_new_vertex_index >= vertices.size(), "Invalid vertex index."); + + // The first vertex. + if (r_vertex_indices.size() == 0) { + // Simply add it. + r_vertex_indices.push_back(p_new_vertex_index); + return; + } + + // The second vertex. + if (r_vertex_indices.size() == 1) { + + Vector2 first_point = vertices[r_vertex_indices[0]].point; + Vector2 new_point = vertices[p_new_vertex_index].point; + + // Sort along the axis with the greatest difference. + int axis = 0; + if (Math::abs(new_point.x - first_point.x) < Math::abs(new_point.y - first_point.y)) axis = 1; + + // Add it to the beginnig or the end appropriately. + if (new_point[axis] < first_point[axis]) + r_vertex_indices.insert(0, p_new_vertex_index); + else + r_vertex_indices.push_back(p_new_vertex_index); + + return; + } + + // Third or later vertices. + Vector2 first_point = vertices[r_vertex_indices[0]].point; + Vector2 last_point = vertices[r_vertex_indices[r_vertex_indices.size() - 1]].point; + Vector2 new_point = vertices[p_new_vertex_index].point; + + // Determine axis being sorted against i.e. the axis with the greatest difference. + int axis = 0; + if (Math::abs(last_point.x - first_point.x) < Math::abs(last_point.y - first_point.y)) axis = 1; + + // Insert the point at the appropriate index. + for (int insert_idx = 0; insert_idx < r_vertex_indices.size(); ++insert_idx) { + Vector2 insert_point = vertices[r_vertex_indices[insert_idx]].point; + if (new_point[axis] < insert_point[axis]) { + r_vertex_indices.insert(insert_idx, p_new_vertex_index); + return; } } - } - //merge the already cliped polys back to 3D - for (Map::Element *E = cd.build_polys_A.front(); E; E = E->next()) { - _merge_poly(mesh_merge, E->key(), E->get(), false); - } - - for (Map::Element *E = cd.build_polys_B.front(); E; E = E->next()) { - _merge_poly(mesh_merge, E->key(), E->get(), true); - } - - //merge the non clipped faces back - - for (int i = 0; i < p_A.faces.size(); i++) { - - if (cd.build_polys_A.has(i)) - continue; //made from buildpoly, skipping - - Vector3 points[3]; - Vector2 uvs[3]; - for (int j = 0; j < 3; j++) { - points[j] = p_A.faces[i].vertices[j]; - uvs[j] = p_A.faces[i].uvs[j]; - } - Ref material; - if (p_A.faces[i].material != -1) { - material = p_A.materials[p_A.faces[i].material]; - } - mesh_merge.add_face(points[0], points[1], points[2], uvs[0], uvs[1], uvs[2], p_A.faces[i].smooth, p_A.faces[i].invert, material, false); - } - - for (int i = 0; i < p_B.faces.size(); i++) { - - if (cd.build_polys_B.has(i)) - continue; //made from buildpoly, skipping - - Vector3 points[3]; - Vector2 uvs[3]; - for (int j = 0; j < 3; j++) { - points[j] = p_B.faces[i].vertices[j]; - uvs[j] = p_B.faces[i].uvs[j]; - } - Ref material; - if (p_B.faces[i].material != -1) { - material = p_B.materials[p_B.faces[i].material]; - } - mesh_merge.add_face(points[0], points[1], points[2], uvs[0], uvs[1], uvs[2], p_B.faces[i].smooth, p_B.faces[i].invert, material, true); - } - - //mark faces that ended up inside the intersection - mesh_merge.mark_inside_faces(); - - //regen new brush to start filling it again - result.clear(); - - switch (p_operation) { - - case OPERATION_UNION: { - - int outside_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - if (mesh_merge.faces[i].inside) - continue; - - outside_count++; - } - - result.faces.resize(outside_count); - - outside_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - if (mesh_merge.faces[i].inside) - continue; - for (int j = 0; j < 3; j++) { - result.faces.write[outside_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; - result.faces.write[outside_count].uvs[j] = mesh_merge.faces[i].uvs[j]; - } - - result.faces.write[outside_count].smooth = mesh_merge.faces[i].smooth; - result.faces.write[outside_count].invert = mesh_merge.faces[i].invert; - result.faces.write[outside_count].material = mesh_merge.faces[i].material_idx; - outside_count++; - } - - result._regen_face_aabbs(); - - } break; - case OPERATION_INTERSECTION: { - - int inside_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - if (!mesh_merge.faces[i].inside) - continue; - - inside_count++; - } - - result.faces.resize(inside_count); - - inside_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - if (!mesh_merge.faces[i].inside) - continue; - for (int j = 0; j < 3; j++) { - result.faces.write[inside_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; - result.faces.write[inside_count].uvs[j] = mesh_merge.faces[i].uvs[j]; - } - - result.faces.write[inside_count].smooth = mesh_merge.faces[i].smooth; - result.faces.write[inside_count].invert = mesh_merge.faces[i].invert; - result.faces.write[inside_count].material = mesh_merge.faces[i].material_idx; - inside_count++; - } - - result._regen_face_aabbs(); - - } break; - case OPERATION_SUBSTRACTION: { - - int face_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - if (mesh_merge.faces[i].from_b && !mesh_merge.faces[i].inside) - continue; - if (!mesh_merge.faces[i].from_b && mesh_merge.faces[i].inside) - continue; - - face_count++; - } - - result.faces.resize(face_count); - - face_count = 0; - - for (int i = 0; i < mesh_merge.faces.size(); i++) { - - if (mesh_merge.faces[i].from_b && !mesh_merge.faces[i].inside) - continue; - if (!mesh_merge.faces[i].from_b && mesh_merge.faces[i].inside) - continue; - - for (int j = 0; j < 3; j++) { - result.faces.write[face_count].vertices[j] = mesh_merge.points[mesh_merge.faces[i].points[j]]; - result.faces.write[face_count].uvs[j] = mesh_merge.faces[i].uvs[j]; - } - - if (mesh_merge.faces[i].from_b) { - //invert facing of insides of B - SWAP(result.faces.write[face_count].vertices[1], result.faces.write[face_count].vertices[2]); - SWAP(result.faces.write[face_count].uvs[1], result.faces.write[face_count].uvs[2]); - } - - result.faces.write[face_count].smooth = mesh_merge.faces[i].smooth; - result.faces.write[face_count].invert = mesh_merge.faces[i].invert; - result.faces.write[face_count].material = mesh_merge.faces[i].material_idx; - face_count++; - } - - result._regen_face_aabbs(); - - } break; - } - - //updatelist of materials - result.materials.resize(mesh_merge.materials.size()); - for (const Map, int>::Element *E = mesh_merge.materials.front(); E; E = E->next()) { - result.materials.write[E->get()] = E->key(); + // New largest, add it to the end. + r_vertex_indices.push_back(p_new_vertex_index); } } + +void CSGBrushOperation::Build2DFaces::_merge_faces(const Vector &p_segment_indices) { + + int segments = p_segment_indices.size() - 1; + if (segments < 2) return; + + // Faces around an inner vertex are merged by moving the inner vertex to the first vertex. + for (int sorted_idx = 1; sorted_idx < segments; ++sorted_idx) { + + int closest_idx = 0; + int inner_idx = p_segment_indices[sorted_idx]; + + if (sorted_idx > segments / 2) { + // Merge to other segment end. + closest_idx = segments; + // Reverse the merge order. + inner_idx = p_segment_indices[segments + segments / 2 - sorted_idx]; + } + + // Find the mergable faces. + Vector merge_faces_idx; + Vector merge_faces; + Vector merge_faces_inner_vertex_idx; + for (int face_idx = 0; face_idx < faces.size(); ++face_idx) { + for (int face_vertex_idx = 0; face_vertex_idx < 3; ++face_vertex_idx) { + if (faces[face_idx].vertex_idx[face_vertex_idx] == inner_idx) { + merge_faces_idx.push_back(face_idx); + merge_faces.push_back(faces[face_idx]); + merge_faces_inner_vertex_idx.push_back(face_vertex_idx); + } + } + } + + Vector degenerate_points; + + // Create the new faces. + for (int merge_idx = 0; merge_idx < merge_faces.size(); ++merge_idx) { + + int outer_edge_idx[2]; + outer_edge_idx[0] = merge_faces[merge_idx].vertex_idx[(merge_faces_inner_vertex_idx[merge_idx] + 1) % 3]; + outer_edge_idx[1] = merge_faces[merge_idx].vertex_idx[(merge_faces_inner_vertex_idx[merge_idx] + 2) % 3]; + + // Skip flattened faces. + if (outer_edge_idx[0] == p_segment_indices[closest_idx] || + outer_edge_idx[1] == p_segment_indices[closest_idx]) continue; + + //Don't create degenerate triangles. + Vector2 edge1[2] = { + vertices[outer_edge_idx[0]].point, + vertices[p_segment_indices[closest_idx]].point + }; + Vector2 edge2[2] = { + vertices[outer_edge_idx[1]].point, + vertices[p_segment_indices[closest_idx]].point + }; + if (are_segements_parallel(edge1, edge2, vertex_snap2)) { + degenerate_points.push_back(outer_edge_idx[0]); + degenerate_points.push_back(outer_edge_idx[1]); + continue; + } + + // Create new faces. + Face2D new_face; + new_face.vertex_idx[0] = p_segment_indices[closest_idx]; + new_face.vertex_idx[1] = outer_edge_idx[0]; + new_face.vertex_idx[2] = outer_edge_idx[1]; + faces.push_back(new_face); + } + + // Delete the old faces in reverse index order. + merge_faces_idx.sort(); + merge_faces_idx.invert(); + for (int i = 0; i < merge_faces_idx.size(); ++i) + faces.remove(merge_faces_idx[i]); + + if (degenerate_points.size() == 0) continue; + + // Split faces using degenerate points. + for (int face_idx = 0; face_idx < faces.size(); ++face_idx) { + + Face2D face = faces[face_idx]; + Vertex2D face_vertices[3] = { + vertices[face.vertex_idx[0]], + vertices[face.vertex_idx[1]], + vertices[face.vertex_idx[2]] + }; + Vector2 face_points[3] = { + face_vertices[0].point, + face_vertices[1].point, + face_vertices[2].point + }; + + for (int point_idx = 0; point_idx < degenerate_points.size(); ++point_idx) { + + int degenerate_idx = degenerate_points[point_idx]; + Vector2 point_2D = vertices[degenerate_idx].point; + + // Check if point is existing face vertex. + bool existing = false; + for (int i = 0; i < 3; ++i) { + if ((point_2D - face_vertices[i].point).length_squared() < vertex_snap2) { + existing = true; + break; + } + } + if (existing) continue; + + // Check if point is on an each edge. + for (int face_edge_idx = 0; face_edge_idx < 3; ++face_edge_idx) { + + Vector2 edge_points[2] = { + face_points[face_edge_idx], + face_points[(face_edge_idx + 1) % 3] + }; + Vector2 closest_point = Geometry::get_closest_point_to_segment_2d(point_2D, edge_points); + + if ((closest_point - point_2D).length_squared() < vertex_snap2) { + + int opposite_vertex_idx = face.vertex_idx[(face_edge_idx + 2) % 3]; + + // If new vertex snaps to degenerate vertex, just delete this face. + if (degenerate_idx == opposite_vertex_idx) { + faces.remove(face_idx); + // Update index. + --face_idx; + break; + } + + // Create two new faces around the new edge and remove this face. + // The new edge is the last edge. + Face2D left_face; + left_face.vertex_idx[0] = degenerate_idx; + left_face.vertex_idx[1] = face.vertex_idx[(face_edge_idx + 1) % 3]; + left_face.vertex_idx[2] = opposite_vertex_idx; + Face2D right_face; + right_face.vertex_idx[0] = opposite_vertex_idx; + right_face.vertex_idx[1] = face.vertex_idx[face_edge_idx]; + right_face.vertex_idx[2] = degenerate_idx; + faces.remove(face_idx); + faces.insert(face_idx, right_face); + faces.insert(face_idx, left_face); + + // Don't check against the new faces. + ++face_idx; + + // No need to check other edges. + break; + } + } + } + } + } +} + +void CSGBrushOperation::Build2DFaces::_find_edge_intersections(const Vector2 p_segment_points[2], Vector &r_segment_indices) { + + // For each face. + for (int face_idx = 0; face_idx < faces.size(); ++face_idx) { + + Face2D face = faces[face_idx]; + Vertex2D face_vertices[3] = { + vertices[face.vertex_idx[0]], + vertices[face.vertex_idx[1]], + vertices[face.vertex_idx[2]] + }; + + // Check each edge. + for (int face_edge_idx = 0; face_edge_idx < 3; ++face_edge_idx) { + + Vector2 edge_points[2] = { + face_vertices[face_edge_idx].point, + face_vertices[(face_edge_idx + 1) % 3].point + }; + Vector2 edge_uvs[2] = { + face_vertices[face_edge_idx].uv, + face_vertices[(face_edge_idx + 1) % 3].uv + }; + Vector2 intersection_point; + + // First check if the ends of the segment are on the edge. + bool on_edge = false; + for (int edge_point_idx = 0; edge_point_idx < 2; ++edge_point_idx) { + intersection_point = Geometry::get_closest_point_to_segment_2d(p_segment_points[edge_point_idx], edge_points); + if ((intersection_point - p_segment_points[edge_point_idx]).length_squared() < vertex_snap2) { + on_edge = true; + break; + } + } + + // Else check if the segment intersects the edge. + if (on_edge || Geometry::segment_intersects_segment_2d(p_segment_points[0], p_segment_points[1], edge_points[0], edge_points[1], &intersection_point)) { + + // Check if intersection point is an edge point. + if ((intersection_point - edge_points[0]).length_squared() < vertex_snap2 || + (intersection_point - edge_points[1]).length_squared() < vertex_snap2) continue; + + // Check if edge exists, by checking if the intersecting segment is parallel to the edge. + if (are_segements_parallel(p_segment_points, edge_points, vertex_snap2)) continue; + + // Add the intersection point as a new vertex. + Vertex2D new_vertex; + new_vertex.point = intersection_point; + new_vertex.uv = interpolate_segment_uv(edge_points, edge_uvs, intersection_point); + int new_vertex_idx = _add_vertex(new_vertex); + int opposite_vertex_idx = face.vertex_idx[(face_edge_idx + 2) % 3]; + _add_vertex_idx_sorted(r_segment_indices, new_vertex_idx); + + // If new vertex snaps to opposite vertex, just delete this face. + if (new_vertex_idx == opposite_vertex_idx) { + faces.remove(face_idx); + // Update index. + --face_idx; + break; + } + + // Don't create degenerate triangles. + Vector2 split_edge1[2] = { vertices[new_vertex_idx].point, edge_points[0] }; + Vector2 split_edge2[2] = { vertices[new_vertex_idx].point, edge_points[1] }; + Vector2 new_edge[2] = { vertices[new_vertex_idx].point, vertices[opposite_vertex_idx].point }; + if (are_segements_parallel(split_edge1, new_edge, vertex_snap2) && + are_segements_parallel(split_edge2, new_edge, vertex_snap2)) { + break; + } + + // If opposite point is on the segemnt, add its index to segment indices too. + Vector2 closest_point = Geometry::get_closest_point_to_segment_2d(vertices[opposite_vertex_idx].point, p_segment_points); + if ((closest_point - vertices[opposite_vertex_idx].point).length_squared() < vertex_snap2) + _add_vertex_idx_sorted(r_segment_indices, opposite_vertex_idx); + + // Create two new faces around the new edge and remove this face. + // The new edge is the last edge. + Face2D left_face; + left_face.vertex_idx[0] = new_vertex_idx; + left_face.vertex_idx[1] = face.vertex_idx[(face_edge_idx + 1) % 3]; + left_face.vertex_idx[2] = opposite_vertex_idx; + Face2D right_face; + right_face.vertex_idx[0] = opposite_vertex_idx; + right_face.vertex_idx[1] = face.vertex_idx[face_edge_idx]; + right_face.vertex_idx[2] = new_vertex_idx; + faces.remove(face_idx); + faces.insert(face_idx, right_face); + faces.insert(face_idx, left_face); + + // Check against the new faces. + --face_idx; + break; + } + } + } +} + +int CSGBrushOperation::Build2DFaces::_insert_point(const Vector2 &p_point) { + + int new_vertex_idx = -1; + + for (int face_idx = 0; face_idx < faces.size(); ++face_idx) { + + Face2D face = faces[face_idx]; + Vertex2D face_vertices[3] = { + vertices[face.vertex_idx[0]], + vertices[face.vertex_idx[1]], + vertices[face.vertex_idx[2]] + }; + Vector2 points[3] = { + face_vertices[0].point, + face_vertices[1].point, + face_vertices[2].point + }; + Vector2 uvs[3] = { + face_vertices[0].uv, + face_vertices[1].uv, + face_vertices[2].uv + }; + + // Check if point is existing face vertex. + for (int i = 0; i < 3; ++i) { + if ((p_point - face_vertices[i].point).length_squared() < vertex_snap2) + return face.vertex_idx[i]; + } + + // Check if point is on an each edge. + bool on_edge = false; + for (int face_edge_idx = 0; face_edge_idx < 3; ++face_edge_idx) { + + Vector2 edge_points[2] = { + points[face_edge_idx], + points[(face_edge_idx + 1) % 3] + }; + Vector2 edge_uvs[2] = { + uvs[face_edge_idx], + uvs[(face_edge_idx + 1) % 3] + }; + + Vector2 closest_point = Geometry::get_closest_point_to_segment_2d(p_point, edge_points); + if ((closest_point - p_point).length_squared() < vertex_snap2) { + on_edge = true; + + // Add the point as a new vertex. + Vertex2D new_vertex; + new_vertex.point = p_point; + new_vertex.uv = interpolate_segment_uv(edge_points, edge_uvs, p_point); + new_vertex_idx = _add_vertex(new_vertex); + int opposite_vertex_idx = face.vertex_idx[(face_edge_idx + 2) % 3]; + + // If new vertex snaps to opposite vertex, just delete this face. + if (new_vertex_idx == opposite_vertex_idx) { + faces.remove(face_idx); + // Update index. + --face_idx; + break; + } + + // Don't create degenerate triangles. + Vector2 split_edge1[2] = { vertices[new_vertex_idx].point, edge_points[0] }; + Vector2 split_edge2[2] = { vertices[new_vertex_idx].point, edge_points[1] }; + Vector2 new_edge[2] = { vertices[new_vertex_idx].point, vertices[opposite_vertex_idx].point }; + if (are_segements_parallel(split_edge1, new_edge, vertex_snap2) && + are_segements_parallel(split_edge2, new_edge, vertex_snap2)) { + break; + } + + // Create two new faces around the new edge and remove this face. + // The new edge is the last edge. + Face2D left_face; + left_face.vertex_idx[0] = new_vertex_idx; + left_face.vertex_idx[1] = face.vertex_idx[(face_edge_idx + 1) % 3]; + left_face.vertex_idx[2] = opposite_vertex_idx; + Face2D right_face; + right_face.vertex_idx[0] = opposite_vertex_idx; + right_face.vertex_idx[1] = face.vertex_idx[face_edge_idx]; + right_face.vertex_idx[2] = new_vertex_idx; + faces.remove(face_idx); + faces.insert(face_idx, right_face); + faces.insert(face_idx, left_face); + + // Don't check against the new faces. + ++face_idx; + + // No need to check other edges. + break; + } + } + + // If not on an edge, check if the point is inside the face. + if (!on_edge && Geometry::is_point_in_triangle(p_point, face_vertices[0].point, face_vertices[1].point, face_vertices[2].point)) { + + // Add the point as a new vertex. + Vertex2D new_vertex; + new_vertex.point = p_point; + new_vertex.uv = interpolate_triangle_uv(points, uvs, p_point); + new_vertex_idx = _add_vertex(new_vertex); + + // Create three new faces around this point and remove this face. + // The new vertex is the last vertex. + for (int i = 0; i < 3; ++i) { + + // Don't create degenerate triangles. + Vector2 edge[2] = { points[i], points[(i + 1) % 3] }; + Vector2 new_edge1[2] = { vertices[new_vertex_idx].point, points[i] }; + Vector2 new_edge2[2] = { vertices[new_vertex_idx].point, points[(i + 1) % 3] }; + if (are_segements_parallel(edge, new_edge1, vertex_snap2) && + are_segements_parallel(edge, new_edge2, vertex_snap2)) { + continue; + } + + Face2D new_face; + new_face.vertex_idx[0] = face.vertex_idx[i]; + new_face.vertex_idx[1] = face.vertex_idx[(i + 1) % 3]; + new_face.vertex_idx[2] = new_vertex_idx; + faces.push_back(new_face); + } + faces.remove(face_idx); + + // No need to check other faces. + break; + } + } + + return new_vertex_idx; +} + +void CSGBrushOperation::Build2DFaces::insert(const CSGBrush &p_brush, int p_face_idx) { + + // Find edge points that cross the plane and face points that are in the plane. + // Map those points to 2D. + // Create new faces from those points. + + Vector2 points_2D[3]; + int points_count = 0; + + for (int i = 0; i < 3; i++) { + + Vector3 point_3D = p_brush.faces[p_face_idx].vertices[i]; + + if (plane.has_point(point_3D)) { + // Point is in the plane, add it. + Vector3 point_2D = plane.project(point_3D); + point_2D = to_2D.xform(point_2D); + points_2D[points_count++] = Vector2(point_2D.x, point_2D.y); + + } else { + + Vector3 next_point_3D = p_brush.faces[p_face_idx].vertices[(i + 1) % 3]; + + if (plane.has_point(next_point_3D)) + continue; // Next point is in plane, it will be added separately. + if (plane.is_point_over(point_3D) == plane.is_point_over(next_point_3D)) + continue; // Both points on the same side of the plane, ignore. + + // Edge crosses the plane, find and add the intersection point. + Vector3 point_2D; + if (plane.intersects_segment(point_3D, next_point_3D, &point_2D)) { + point_2D = to_2D.xform(point_2D); + points_2D[points_count++] = Vector2(point_2D.x, point_2D.y); + } + } + } + + Vector segment_indices; + Vector2 segment[2]; + int inserted_index[3] = { -1, -1, -1 }; + + // Insert points. + for (int i = 0; i < points_count; ++i) { + inserted_index[i] = _insert_point(points_2D[i]); + } + + if (points_count == 2) { + // Insert a single segment. + segment[0] = points_2D[0]; + segment[1] = points_2D[1]; + _find_edge_intersections(segment, segment_indices); + for (int i = 0; i < 2; ++i) { + _add_vertex_idx_sorted(segment_indices, inserted_index[i]); + } + _merge_faces(segment_indices); + } + + if (points_count == 3) { + // Insert three segments. + for (int edge_idx = 0; edge_idx < 3; ++edge_idx) { + segment[0] = points_2D[edge_idx]; + segment[1] = points_2D[(edge_idx + 1) % 3]; + _find_edge_intersections(segment, segment_indices); + for (int i = 0; i < 2; ++i) { + _add_vertex_idx_sorted(segment_indices, inserted_index[(edge_idx + i) % 3]); + } + _merge_faces(segment_indices); + segment_indices.clear(); + } + } +} + +void CSGBrushOperation::Build2DFaces::addFacesToMesh(MeshMerge &r_mesh_merge, bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b) { + + for (int face_idx = 0; face_idx < faces.size(); ++face_idx) { + Face2D face = faces[face_idx]; + Vertex2D fv[3] = { + vertices[face.vertex_idx[0]], + vertices[face.vertex_idx[1]], + vertices[face.vertex_idx[2]] + }; + + // Convert 2D vertex points to 3D. + Vector3 points_3D[3]; + Vector2 uvs[3]; + for (int i = 0; i < 3; ++i) { + Vector3 point_2D(fv[i].point.x, fv[i].point.y, 0); + points_3D[i] = to_3D.xform(point_2D); + uvs[i] = fv[i].uv; + } + + r_mesh_merge.add_face(points_3D, uvs, p_smooth, p_invert, p_material, p_from_b); + } +} + +CSGBrushOperation::Build2DFaces::Build2DFaces(const CSGBrush &p_brush, int p_face_idx, float p_vertex_snap2) : + vertex_snap2(p_vertex_snap2 * p_vertex_snap2) { + + // Convert 3D vertex points to 2D. + Vector3 points_3D[3] = { + p_brush.faces[p_face_idx].vertices[0], + p_brush.faces[p_face_idx].vertices[1], + p_brush.faces[p_face_idx].vertices[2], + }; + + plane = Plane(points_3D[0], points_3D[1], points_3D[2]); + to_3D.origin = points_3D[0]; + to_3D.basis.set_axis(2, plane.normal); + to_3D.basis.set_axis(0, (points_3D[1] - points_3D[2]).normalized()); + to_3D.basis.set_axis(1, to_3D.basis.get_axis(0).cross(to_3D.basis.get_axis(2)).normalized()); + to_2D = to_3D.affine_inverse(); + + Face2D face; + for (int i = 0; i < 3; i++) { + Vertex2D vertex; + Vector3 point_2D = to_2D.xform(points_3D[i]); + vertex.point.x = point_2D.x; + vertex.point.y = point_2D.y; + vertex.uv = p_brush.faces[p_face_idx].uvs[i]; + vertices.push_back(vertex); + face.vertex_idx[i] = i; + } + faces.push_back(face); +} + +void CSGBrushOperation::update_faces(const CSGBrush &p_brush_a, const int p_face_idx_a, const CSGBrush &p_brush_b, const int p_face_idx_b, Build2DFaceCollection &p_collection, float p_vertex_snap) { + + Vector3 vertices_a[3] = { + p_brush_a.faces[p_face_idx_a].vertices[0], + p_brush_a.faces[p_face_idx_a].vertices[1], + p_brush_a.faces[p_face_idx_a].vertices[2], + }; + + Vector3 vertices_b[3] = { + p_brush_b.faces[p_face_idx_b].vertices[0], + p_brush_b.faces[p_face_idx_b].vertices[1], + p_brush_b.faces[p_face_idx_b].vertices[2], + }; + + // Don't use degenerate faces. + bool has_degenerate = false; + if (is_snapable(vertices_a[0], vertices_a[1], p_vertex_snap) || + is_snapable(vertices_a[0], vertices_a[2], p_vertex_snap) || + is_snapable(vertices_a[1], vertices_a[2], p_vertex_snap)) { + p_collection.build2DFacesA[p_face_idx_a] = Build2DFaces(); + has_degenerate = true; + } + + if (is_snapable(vertices_b[0], vertices_b[1], p_vertex_snap) || + is_snapable(vertices_b[0], vertices_b[2], p_vertex_snap) || + is_snapable(vertices_b[1], vertices_b[2], p_vertex_snap)) { + p_collection.build2DFacesB[p_face_idx_b] = Build2DFaces(); + has_degenerate = true; + } + if (has_degenerate) return; + + // Ensure B has points either side of or in the plane of A. + int in_plane_count = 0, over_count = 0, under_count = 0; + Plane plane_a(vertices_a[0], vertices_a[1], vertices_a[2]); + ERR_FAIL_COND_MSG(plane_a.normal == Vector3(), "Couldn't form plane from Brush A face."); + + for (int i = 0; i < 3; i++) { + if (plane_a.has_point(vertices_b[i])) + in_plane_count++; + else if (plane_a.is_point_over(vertices_b[i])) + over_count++; + else + under_count++; + } + // If all points under or over the plane, there is no intesection. + if (over_count == 3 || under_count == 3) return; + + // Ensure A has points either side of or in the plane of B. + in_plane_count = 0; + over_count = 0; + under_count = 0; + Plane plane_b(vertices_b[0], vertices_b[1], vertices_b[2]); + ERR_FAIL_COND_MSG(plane_b.normal == Vector3(), "Couldn't form plane from Brush B face."); + + for (int i = 0; i < 3; i++) { + if (plane_b.has_point(vertices_a[i])) + in_plane_count++; + else if (plane_b.is_point_over(vertices_a[i])) + over_count++; + else + under_count++; + } + // If all points under or over the plane, there is no intesection. + if (over_count == 3 || under_count == 3) return; + + // Check for intersection using the SAT theorem. + { + + // Edge pair cross product combinations. + for (int i = 0; i < 3; i++) { + + Vector3 axis_a = (vertices_a[i] - vertices_a[(i + 1) % 3]).normalized(); + + for (int j = 0; j < 3; j++) { + + Vector3 axis_b = (vertices_b[j] - vertices_b[(j + 1) % 3]).normalized(); + + Vector3 sep_axis = axis_a.cross(axis_b); + if (sep_axis == Vector3()) + continue; //colineal + sep_axis.normalize(); + + real_t min_a = 1e20, max_a = -1e20; + real_t min_b = 1e20, max_b = -1e20; + + for (int k = 0; k < 3; k++) { + real_t d = sep_axis.dot(vertices_a[k]); + min_a = MIN(min_a, d); + max_a = MAX(max_a, d); + d = sep_axis.dot(vertices_b[k]); + min_b = MIN(min_b, d); + max_b = MAX(max_b, d); + } + + min_b -= (max_a - min_a) * 0.5; + max_b += (max_a - min_a) * 0.5; + + real_t dmin = min_b - (min_a + max_a) * 0.5; + real_t dmax = max_b - (min_a + max_a) * 0.5; + + if (dmin > CMP_EPSILON || dmax < -CMP_EPSILON) { + return; // Does not contain zero, so they don't overlap. + } + } + } + } + + // If we're still here, the faces probably intersect, so add new faces. + if (!p_collection.build2DFacesA.has(p_face_idx_a)) { + p_collection.build2DFacesA[p_face_idx_a] = Build2DFaces(p_brush_a, p_face_idx_a, p_vertex_snap); + } + p_collection.build2DFacesA[p_face_idx_a].insert(p_brush_b, p_face_idx_b); + + if (!p_collection.build2DFacesB.has(p_face_idx_b)) { + p_collection.build2DFacesB[p_face_idx_b] = Build2DFaces(p_brush_b, p_face_idx_b, p_vertex_snap); + } + p_collection.build2DFacesB[p_face_idx_b].insert(p_brush_a, p_face_idx_a); +} diff --git a/modules/csg/csg.h b/modules/csg/csg.h index a11e55c72a3..3f63630cae9 100644 --- a/modules/csg/csg.h +++ b/modules/csg/csg.h @@ -31,20 +31,22 @@ #ifndef CSG_H #define CSG_H +#include "core/list.h" #include "core/map.h" #include "core/math/aabb.h" #include "core/math/plane.h" -#include "core/math/rect2.h" #include "core/math/transform.h" +#include "core/math/vector2.h" #include "core/math/vector3.h" #include "core/oa_hash_map.h" #include "core/pool_vector.h" +#include "core/reference.h" +#include "core/vector.h" #include "scene/resources/material.h" struct CSGBrush { struct Face { - Vector3 vertices[3]; Vector2 uvs[3]; AABB aabb; @@ -56,12 +58,11 @@ struct CSGBrush { Vector faces; Vector > materials; - void _regen_face_aabbs(); - //create a brush from faces + inline void _regen_face_aabbs(); + + // Create a brush from faces. void build_from_faces(const PoolVector &p_vertices, const PoolVector &p_uvs, const PoolVector &p_smooth, const PoolVector > &p_materials, const PoolVector &p_invert_faces); void copy_from(const CSGBrush &p_brush, const Transform &p_xform); - - void clear(); }; struct CSGBrushOperation { @@ -70,12 +71,23 @@ struct CSGBrushOperation { OPERATION_UNION, OPERATION_INTERSECTION, OPERATION_SUBSTRACTION, - }; + void merge_brushes(Operation p_operation, const CSGBrush &p_brush_a, const CSGBrush &p_brush_b, CSGBrush &r_merged_brush, float p_vertex_snap); + struct MeshMerge { - struct BVH { + struct Face { + bool from_b; + bool inside; + int points[3]; + Vector2 uvs[3]; + bool smooth; + bool invert; + int material_idx; + }; + + struct FaceBVH { int face; int left; int right; @@ -84,32 +96,23 @@ struct CSGBrushOperation { AABB aabb; }; - struct BVHCmpX { - - bool operator()(const BVH *p_left, const BVH *p_right) const { - + struct FaceBVHCmpX { + _FORCE_INLINE_ bool operator()(const FaceBVH *p_left, const FaceBVH *p_right) const { return p_left->center.x < p_right->center.x; } }; - struct BVHCmpY { - - bool operator()(const BVH *p_left, const BVH *p_right) const { - + struct FaceBVHCmpY { + _FORCE_INLINE_ bool operator()(const FaceBVH *p_left, const FaceBVH *p_right) const { return p_left->center.y < p_right->center.y; } }; - struct BVHCmpZ { - - bool operator()(const BVH *p_left, const BVH *p_right) const { - + struct FaceBVHCmpZ { + _FORCE_INLINE_ bool operator()(const FaceBVH *p_left, const FaceBVH *p_right) const { return p_left->center.z < p_right->center.z; } }; - int _bvh_count_intersections(BVH *bvhptr, int p_max_depth, int p_bvh_first, const Vector3 &p_begin, const Vector3 &p_end, int p_exclude) const; - int _create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc); - struct VertexKey { int32_t x, y, z; _FORCE_INLINE_ bool operator<(const VertexKey &p_key) const { @@ -138,99 +141,59 @@ struct CSGBrushOperation { } }; - OAHashMap snap_cache; - Vector points; - - struct Face { - bool from_b; - bool inside; - int points[3]; - Vector2 uvs[3]; - bool smooth; - bool invert; - int material_idx; - }; - Vector faces; - Map, int> materials; - Map vertex_map; - void add_face(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector2 &p_uv_a, const Vector2 &p_uv_b, const Vector2 &p_uv_c, bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b); - // void add_face(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, bool p_from_b); - + OAHashMap snap_cache; float vertex_snap; + + inline void _add_distance(List &r_intersectionsA, List &r_intersectionsB, bool p_from_B, real_t p_distance) const; + inline bool _bvh_inside(FaceBVH *facebvhptr, int p_max_depth, int p_bvh_first, int p_face_idx) const; + inline int _create_bvh(FaceBVH *facebvhptr, FaceBVH **facebvhptrptr, int p_from, int p_size, int p_depth, int &r_max_depth, int &r_max_alloc); + + void add_face(const Vector3 p_points[3], const Vector2 p_uvs[3], bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b); void mark_inside_faces(); }; - struct BuildPoly { + struct Build2DFaces { - Plane plane; - Transform to_poly; - Transform to_world; - int face_index; - - struct Point { + struct Vertex2D { Vector2 point; Vector2 uv; }; - Vector points; - - struct Edge { - bool outer; - int points[2]; - Edge() { - outer = false; - } + struct Face2D { + int vertex_idx[3]; }; - Vector edges; - Ref material; - bool smooth; - bool invert; + Vector vertices; + Vector faces; + Plane plane; + Transform to_2D; + Transform to_3D; + float vertex_snap2; - int base_edges; //edges from original triangle, even if split + inline int _get_point_idx(const Vector2 &p_point); + inline int _add_vertex(const Vertex2D &p_vertex); + inline void _add_vertex_idx_sorted(Vector &r_vertex_indices, int p_new_vertex_index); + inline void _merge_faces(const Vector &p_segment_indices); + inline void _find_edge_intersections(const Vector2 p_segment_points[2], Vector &r_segment_indices); + inline int _insert_point(const Vector2 &p_point); - void _clip_segment(const CSGBrush *p_brush, int p_face, const Vector2 *segment, MeshMerge &mesh_merge, bool p_for_B); + void insert(const CSGBrush &p_brush, int p_brush_face); + void addFacesToMesh(MeshMerge &r_mesh_merge, bool p_smooth, bool p_invert, const Ref &p_material, bool p_from_b); - void create(const CSGBrush *p_brush, int p_face, MeshMerge &mesh_merge, bool p_for_B); - void clip(const CSGBrush *p_brush, int p_face, MeshMerge &mesh_merge, bool p_for_B); + Build2DFaces() {} + Build2DFaces(const CSGBrush &p_brush, int p_brush_face, float p_vertex_snap2); }; - struct PolyPoints { - - Vector points; - - Vector > holes; + struct Build2DFaceCollection { + Map build2DFacesA; + Map build2DFacesB; }; - struct EdgeSort { - int edge; - int prev_point; - int edge_point; - float angle; - bool operator<(const EdgeSort &p_edge) const { return angle < p_edge.angle; } - }; - - struct CallbackData { - const CSGBrush *A; - const CSGBrush *B; - int face_a; - CSGBrushOperation *self; - Map build_polys_A; - Map build_polys_B; - }; - - void _add_poly_points(const BuildPoly &p_poly, int p_edge, int p_from_point, int p_to_point, const Vector > &vertex_process, Vector &edge_process, Vector &r_poly); - void _add_poly_outline(const BuildPoly &p_poly, int p_from_point, int p_to_point, const Vector > &vertex_process, Vector &r_outline); - void _merge_poly(MeshMerge &mesh, int p_face_idx, const BuildPoly &p_poly, bool p_from_b); - - void _collision_callback(const CSGBrush *A, int p_face_a, Map &build_polys_a, const CSGBrush *B, int p_face_b, Map &build_polys_b, MeshMerge &mesh_merge); - - static void _collision_callbacks(void *ud, int p_face_b); - void merge_brushes(Operation p_operation, const CSGBrush &p_A, const CSGBrush &p_B, CSGBrush &result, float p_snap = 0.001); + void update_faces(const CSGBrush &p_brush_a, const int p_face_idx_a, const CSGBrush &p_brush_b, const int p_face_idx_b, Build2DFaceCollection &p_collection, float p_vertex_snap); }; #endif // CSG_H