Fix multiple issues with CSG module.

- Replaces BuildPoly with Build2DFaces, which creates faces as each
  pair of face intersections are processed, instead of trying to create
  them after all the intersections are processed. Ensures that faces are
  merged when possible, and removes degenerate triangles.

- Treats the child as inside the parent when faces are coplanar.

- General clean up of csg.h and csg.cpp.

1 files changed, 1036 insertions, 1081 deletions

diff --git a/modules/csg/csg.cpp b/modules/csg/csg.cpp
index 36055ce840..e9ca1d3e5b 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/math/math_funcs.h"
 #include "core/sort_array.h"
-#include "thirdparty/misc/triangulator.h"
 
-void CSGBrush::clear() {
-	faces.clear();
+// 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 Vector<Vector3> &p_vertices, const Vector<Vector2> &p_uvs, const Vector<bool> &p_smooth, const Vector<Ref<Material> > &p_materials, const Vector<bool> &p_invert_faces) {
 
-	clear();
+	faces.clear();
 
 	int vc = p_vertices.size();
 
@@ -62,37 +202,42 @@ void CSGBrush::build_from_faces(const Vector<Vector3> &p_vertices, const Vector<
 	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) {
+
+		if (sc == vc / 3)
 			f.smooth = rs[i];
-		} else {
+		else
 			f.smooth = false;
-		}
 
-		if (ic == vc / 3) {
+		if (ic == vc / 3)
 			f.invert = ri[i];
-		} else {
+		else
 			f.invert = false;
-		}
 
 		if (mc == vc / 3) {
+
 			Ref<Material> mat = rm[i];
 			if (mat.is_valid()) {
+
 				const Map<Ref<Material>, 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 Vector<Vector3> &p_vertices, const Vector<
 	_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();
 }
 
-////////////////////////
-
-void CSGBrushOperation::BuildPoly::create(const CSGBrush *p_brush, int p_face, MeshMerge &mesh_merge, bool p_for_B) {
-
-	//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) {
+// CSGBrushOperation
 
-	//keep track of what was inserted
-	Vector<int> inserted_points;
+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) {
 
-	//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) {
-
-		if (segment_idx[0] == segment_idx[1]) {
-			return; //segment was too tiny, both mapped to same point
-		}
-
-		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++) {
+	// Add faces to MeshMerge.
+	MeshMerge mesh_merge;
+	mesh_merge.vertex_snap = p_vertex_snap;
 
-			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);
+	for (int i = 0; i < p_brush_a.faces.size(); i++) {
 
-			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;
+		Ref<Material> material;
+		if (p_brush_a.faces[i].material != -1) {
+			material = p_brush_a.materials[p_brush_a.faces[i].material];
 		}
 
-		//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;
+		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 points[3];
+			Vector2 uvs[3];
+			for (int j = 0; j < 3; j++) {
+				points[j] = p_brush_a.faces[i].vertices[j];
+				uvs[j] = p_brush_a.faces[i].uvs[j];
 			}
-			inserted_points.push_back(point_idx);
+			mesh_merge.add_face(points, uvs, p_brush_a.faces[i].smooth, p_brush_a.faces[i].invert, material, false);
 		}
 	}
 
-	//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];
+	for (int i = 0; i < p_brush_b.faces.size(); 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);
+		Ref<Material> material;
+		if (p_brush_b.faces[i].material != -1) {
+			material = p_brush_b.materials[p_brush_b.faces[i].material];
 		}
-	}
-
-	//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.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 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<int, BuildPoly> &build_polys_a, const CSGBrush *B, int p_face_b, Map<int, BuildPoly> &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_b.faces[i].vertices[j];
+				uvs[j] = p_brush_b.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_b.faces[i].smooth, p_brush_b.faces[i].invert, material, true);
 		}
 	}
 
-	// do a quick pre-check for no-intersection using the SAT theorem
-
-	{
-
-		//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
-		}
-
-		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
+	// Mark faces that ended up inside the intersection.
+	mesh_merge.mark_inside_faces();
 
-		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++;
-		}
+	// Create new brush and fill with new faces.
+	r_merged_brush.faces.clear();
 
-		if (over_count == 0 || under_count == 0)
-			return; //no intersection, something needs to be under AND over
+	switch (p_operation) {
 
-		//edge pairs (cross product combinations), see SAT theorem
+		case OPERATION_UNION: {
 
-		for (int i = 0; i < 3; i++) {
+			int outside_count = 0;
 
-			Vector3 axis_a = (va[i] - va[(i + 1) % 3]).normalized();
+			for (int i = 0; i < mesh_merge.faces.size(); i++) {
+				if (mesh_merge.faces[i].inside)
+					continue;
+				outside_count++;
+			}
 
-			for (int j = 0; j < 3; j++) {
+			r_merged_brush.faces.resize(outside_count);
 
-				Vector3 axis_b = (vb[j] - vb[(j + 1) % 3]).normalized();
+			outside_count = 0;
 
-				Vector3 sep_axis = axis_a.cross(axis_b);
-				if (sep_axis == Vector3())
-					continue; //colineal
-				sep_axis.normalize();
+			for (int i = 0; i < mesh_merge.faces.size(); i++) {
 
-				real_t min_a = 1e20, max_a = -1e20;
-				real_t min_b = 1e20, max_b = -1e20;
+				if (mesh_merge.faces[i].inside)
+					continue;
 
-				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);
+				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;
-
-				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
-				}
+				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++;
 			}
-		}
-	}
-
-	//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;
+			r_merged_brush._regen_face_aabbs();
 
-	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<Vector<int> > &vertex_process, Vector<bool> &edge_process, Vector<PolyPoints> &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<EdgeSort> 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<int> 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<EdgeSort> next_edges;
-
-			for (int i = 0; i < vertex_process[to_point].size(); i++) {
+		} break;
 
-				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
+		case OPERATION_INTERSECTION: {
 
-				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;
+			int inside_count = 0;
 
-				next_edges.push_back(e2);
+			for (int i = 0; i < mesh_merge.faces.size(); i++) {
+				if (!mesh_merge.faces[i].inside)
+					continue;
+				inside_count++;
 			}
 
-			//finally, sort by minimum angle
-			next_edges.sort();
+			r_merged_brush.faces.resize(inside_count);
 
-			int next_point = -1;
-			int next_edge = -1;
+			inside_count = 0;
 
-			for (int i = 0; i < next_edges.size(); i++) {
+			for (int i = 0; i < mesh_merge.faces.size(); i++) {
 
-				if (i == 0) {
-					//minimum angle found is the next point
-					next_point = next_edges[i].edge_point;
-					next_edge = next_edges[i].edge;
+				if (!mesh_merge.faces[i].inside)
+					continue;
 
-				} 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 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 (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.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++;
 			}
 
-			points.push_back(to_point);
-
-			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);
-		}
-	}
-}
-
-void CSGBrushOperation::_add_poly_outline(const BuildPoly &p_poly, int p_from_point, int p_to_point, const Vector<Vector<int> > &vertex_process, Vector<int> &r_outline) {
-
-	//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);
+			r_merged_brush._regen_face_aabbs();
 
-	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;
+		} break;
 
-		for (int i = 0; i < vertex_process[to_point].size(); i++) {
+		case OPERATION_SUBSTRACTION: {
 
-			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
+			int face_count = 0;
 
-			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;
+			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++;
 			}
-		}
-
-		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<Vector<int> > vertex_process;
-	Vector<bool> 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<PolyPoints> polys;
+			r_merged_brush.faces.resize(face_count);
 
-	//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
+			face_count = 0;
 
-				int intersections = 0;
+			for (int i = 0; i < mesh_merge.faces.size(); i++) {
 
-				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 (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;
 
-					if (Geometry::segment_intersects_segment_2d(ref_point, out_point, p1, p2, NULL)) {
-						intersections++;
-					}
+				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 (intersections % 2 == 1) {
-					//hole is inside this poly
-					intersect_poly = j;
-					break;
+				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]);
 				}
-			}
-		}
 
-		if (intersect_poly != -1) {
-			//must add this as a hole
-			Vector<int> 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);
+				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++;
 			}
-		}
-		_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;
+			r_merged_brush._regen_face_aabbs();
 
-							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);
-		}
+		} break;
 	}
 
-	//triangulate polygons
-
-	for (int i = 0; i < polys.size(); i++) {
-
-		Vector<Vector2> 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<int> 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);
-		}
+	// Update the list of materials.
+	r_merged_brush.materials.resize(mesh_merge.materials.size());
+	for (const Map<Ref<Material>, int>::Element *E = mesh_merge.materials.front(); E; E = E->next()) {
+		r_merged_brush.materials.write[E->get()] = E->key();
 	}
 }
 
-//use a limit to speed up bvh and limit the depth
+// CSGBrushOperation::MeshMerge
+
+// 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<BVH *, BVHCmpX> sort_x;
-			sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			SortArray<FaceBVH *, FaceBVHCmpX> 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<BVH *, BVHCmpY> sort_y;
-			sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			SortArray<FaceBVH *, FaceBVHCmpY> 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<BVH *, BVHCmpZ> sort_z;
-			sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			SortArray<FaceBVH *, FaceBVHCmpZ> 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<real_t> &r_intersectionsA, List<real_t> &r_intersectionsB, bool p_from_B, real_t p_distance) const {
+
+	List<real_t> &intersections = p_from_B ? r_intersectionsB : r_intersectionsA;
+
+	// Check if distance exists.
+	for (const List<real_t>::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<real_t> intersectionsA;
+	List<real_t> 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) {
-
-					const BVH *bp = &b;
 
-					while (bp) {
-
-						bool valid = segment_aabb.intersects(bp->aabb) && bp->aabb.intersects_segment(p_begin, p_end);
-
-						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;
+			case TEST_AABB_BIT: {
 
-							if (f3.intersects_segment(p_begin, p_end, &res)) {
-								intersections++;
+				if (current_facebvhptr->face >= 0) {
+
+					while (current_facebvhptr) {
+
+						if (p_face_idx != current_facebvhptr->face &&
+								current_facebvhptr->aabb.intersects_ray(face_center, face_normal)) {
+
+							const Face &current_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;
+
+							// 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<FaceBVH> 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<BVH> 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<BVH *> bvhtrvec;
+	Vector<FaceBVH *> 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;
-
-		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);
+		// Check if face AABB intersects the intersection AABB.
+		if (!intersection_aabb.intersects_inclusive(facebvh[i].aabb))
+			continue;
 
-		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<Material> &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<Material> &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;
-
-	//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);
-			}
-		}
+	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;
+}
 
-	//merge the already cliped polys back to 3D
-	for (Map<int, BuildPoly>::Element *E = cd.build_polys_A.front(); E; E = E->next()) {
-		_merge_poly(mesh_merge, E->key(), E->get(), false);
-	}
+int CSGBrushOperation::Build2DFaces::_add_vertex(const Vertex2D &p_vertex) {
 
-	for (Map<int, BuildPoly>::Element *E = cd.build_polys_B.front(); E; E = E->next()) {
-		_merge_poly(mesh_merge, E->key(), E->get(), true);
-	}
+	// Check if vertex exists.
+	int vertex_id = _get_point_idx(p_vertex.point);
+	if (vertex_id != -1) return vertex_id;
 
-	//merge the non clipped faces back
+	vertices.push_back(p_vertex);
+	return vertices.size() - 1;
+}
 
-	for (int i = 0; i < p_A.faces.size(); i++) {
+void CSGBrushOperation::Build2DFaces::_add_vertex_idx_sorted(Vector<int> &r_vertex_indices, int p_new_vertex_index) {
 
-		if (cd.build_polys_A.has(i))
-			continue; //made from buildpoly, skipping
+	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.");
 
-		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> material;
-		if (p_A.faces[i].material != -1) {
-			material = p_A.materials[p_A.faces[i].material];
+		// The first vertex.
+		if (r_vertex_indices.size() == 0) {
+			// Simply add it.
+			r_vertex_indices.push_back(p_new_vertex_index);
+			return;
 		}
-		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++) {
+		// The second vertex.
+		if (r_vertex_indices.size() == 1) {
 
-		if (cd.build_polys_B.has(i))
-			continue; //made from buildpoly, skipping
+			Vector2 first_point = vertices[r_vertex_indices[0]].point;
+			Vector2 new_point = vertices[p_new_vertex_index].point;
 
-		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];
+			// 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;
 		}
-		Ref<Material> material;
-		if (p_B.faces[i].material != -1) {
-			material = p_B.materials[p_B.faces[i].material];
+
+		// 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;
+			}
 		}
-		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);
+
+		// New largest, add it to the end.
+		r_vertex_indices.push_back(p_new_vertex_index);
 	}
+}
 
-	//mark faces that ended up inside the intersection
-	mesh_merge.mark_inside_faces();
+void CSGBrushOperation::Build2DFaces::_merge_faces(const Vector<int> &p_segment_indices) {
 
-	//regen new brush to start filling it again
-	result.clear();
+	int segments = p_segment_indices.size() - 1;
+	if (segments < 2) return;
 
-	switch (p_operation) {
+	// 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) {
 
-		case OPERATION_UNION: {
+		int closest_idx = 0;
+		int inner_idx = p_segment_indices[sorted_idx];
 
-			int outside_count = 0;
+		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];
+		}
 
-			for (int i = 0; i < mesh_merge.faces.size(); i++) {
-				if (mesh_merge.faces[i].inside)
-					continue;
+		// Find the mergable faces.
+		Vector<int> merge_faces_idx;
+		Vector<Face2D> merge_faces;
+		Vector<int> 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);
+				}
+			}
+		}
 
-				outside_count++;
+		Vector<int> 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;
 			}
 
-			result.faces.resize(outside_count);
+			// 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);
+		}
 
-			outside_count = 0;
+		// 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;
 
-			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];
+				// 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;
+					}
 				}
+			}
+		}
+	}
+}
 
-				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++;
+void CSGBrushOperation::Build2DFaces::_find_edge_intersections(const Vector2 p_segment_points[2], Vector<int> &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;
+				}
 			}
 
-			result._regen_face_aabbs();
+			// 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;
+				}
 
-		} break;
-		case OPERATION_INTERSECTION: {
+				// 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;
+				}
 
-			int inside_count = 0;
+				// 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;
+			}
+		}
+	}
+}
 
-			for (int i = 0; i < mesh_merge.faces.size(); i++) {
-				if (!mesh_merge.faces[i].inside)
-					continue;
+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];
+		}
 
-				inside_count++;
-			}
+		// 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;
+				}
 
-			result.faces.resize(inside_count);
+				// 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;
+				}
 
-			inside_count = 0;
+				// 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;
+			}
+		}
 
-			for (int i = 0; i < mesh_merge.faces.size(); i++) {
-				if (!mesh_merge.faces[i].inside)
+		// 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;
-				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++;
+				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);
 
-			result._regen_face_aabbs();
+			// No need to check other faces.
+			break;
+		}
+	}
 
-		} break;
-		case OPERATION_SUBSTRACTION: {
+	return new_vertex_idx;
+}
 
-			int face_count = 0;
+void CSGBrushOperation::Build2DFaces::insert(const CSGBrush &p_brush, int p_face_idx) {
 
-			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;
+	// 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.
 
-				face_count++;
+	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);
 			}
+		}
+	}
 
-			result.faces.resize(face_count);
+	Vector<int> segment_indices;
+	Vector2 segment[2];
+	int inserted_index[3] = { -1, -1, -1 };
 
-			face_count = 0;
+	// Insert points.
+	for (int i = 0; i < points_count; ++i) {
+		inserted_index[i] = _insert_point(points_2D[i]);
+	}
 
-			for (int i = 0; i < mesh_merge.faces.size(); 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 (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;
+	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();
+		}
+	}
+}
 
-				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];
-				}
+void CSGBrushOperation::Build2DFaces::addFacesToMesh(MeshMerge &r_mesh_merge, bool p_smooth, bool p_invert, const Ref<Material> &p_material, bool p_from_b) {
 
-				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]);
+	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);
 				}
 
-				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++;
-			}
+				min_b -= (max_a - min_a) * 0.5;
+				max_b += (max_a - min_a) * 0.5;
 
-			result._regen_face_aabbs();
+				real_t dmin = min_b - (min_a + max_a) * 0.5;
+				real_t dmax = max_b - (min_a + max_a) * 0.5;
 
-		} break;
+				if (dmin > CMP_EPSILON || dmax < -CMP_EPSILON) {
+					return; // Does not contain zero, so they don't overlap.
+				}
+			}
+		}
 	}
 
-	//updatelist of materials
-	result.materials.resize(mesh_merge.materials.size());
-	for (const Map<Ref<Material>, int>::Element *E = mesh_merge.materials.front(); E; E = E->next()) {
-		result.materials.write[E->get()] = E->key();
+	// 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);
 }