GODOT IS OPEN SOURCE

1 files changed, 628 insertions, 0 deletions

diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp
new file mode 100644
index 0000000000..7f838b1215
--- /dev/null
+++ b/core/math/bsp_tree.cpp
@@ -0,0 +1,628 @@
+/*************************************************************************/
+/*  bsp_tree.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "bsp_tree.h"
+#include "error_macros.h"
+#include "print_string.h"
+
+
+void BSP_Tree::from_aabb(const AABB& p_aabb) {
+
+	planes.clear();
+
+	for(int i=0;i<3;i++) {
+
+		Vector3 n;
+		n[i]=1;
+		planes.push_back(Plane(n,p_aabb.pos[i]+p_aabb.size[i]));
+		planes.push_back(Plane(-n,-p_aabb.pos[i]));
+	}
+
+	nodes.clear();
+
+	for(int i=0;i<6;i++) {
+
+		Node n;
+		n.plane=i;
+		n.under=(i==0)?UNDER_LEAF:i-1;
+		n.over=OVER_LEAF;
+		nodes.push_back(n);
+	}
+
+	aabb=p_aabb;
+	error_radius=0;
+}
+
+Vector<BSP_Tree::Node> BSP_Tree::get_nodes() const {
+
+	return nodes;
+}
+Vector<Plane> BSP_Tree::get_planes() const {
+
+	return planes;
+}
+	
+AABB BSP_Tree::get_aabb() const {
+
+	return aabb;
+}
+	
+int BSP_Tree::_get_points_inside(int p_node,const Vector3* p_points,int *p_indices, const Vector3& p_center,const Vector3& p_half_extents,int p_indices_count) const {
+
+
+	const Node *node =&nodes[p_node];
+	const Plane &p = planes[node->plane];
+
+	Vector3 min(
+			(p.normal.x>0) ? -p_half_extents.x : p_half_extents.x,
+			(p.normal.y>0) ? -p_half_extents.y : p_half_extents.y,
+			(p.normal.z>0) ? -p_half_extents.z : p_half_extents.z
+		);
+	Vector3 max=-min;
+	max+=p_center;
+	min+=p_center;
+
+	float dist_min = p.distance_to(min);
+	float dist_max = p.distance_to(max);
+
+	if ((dist_min * dist_max) < CMP_EPSILON ) { //intersection, test point by point
+
+
+		int under_count=0;
+
+		//sort points, so the are under first, over last
+		for(int i=0;i<p_indices_count;i++) {
+
+			int index=p_indices[i];
+
+			if (p.is_point_over(p_points[index])) {
+
+				// kind of slow (but cache friendly), should try something else,
+				// but this is a corner case most of the time
+
+				for(int j=index;j<p_indices_count-1;j++)
+					p_indices[j]=p_indices[j+1];
+
+				p_indices[p_indices_count-1]=index;
+
+			} else {
+				under_count++;
+			}
+
+		}
+
+		int total=0;
+
+		if (under_count>0) {
+			if (node->under==UNDER_LEAF) {
+				total+=under_count;
+			} else {
+				total+=_get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,under_count);
+			}
+		}
+
+		if (under_count!=p_indices_count) {
+			if (node->over==OVER_LEAF) {
+				//total+=0 //if they are over an OVER_LEAF, they are outside the model
+			} else {
+				total+=_get_points_inside(node->over,p_points,&p_indices[under_count],p_center,p_half_extents,p_indices_count-under_count);
+			}
+		}
+
+		return total;
+
+	} else if (dist_min > 0 ) { //all points over plane
+
+		if (node->over==OVER_LEAF) {
+
+			return 0; // all these points are not visible
+		}
+
+
+		return _get_points_inside(node->over,p_points,p_indices,p_center,p_half_extents,p_indices_count);
+	} else if (dist_min <= 0 ) { //all points behind plane
+
+		if (node->under==UNDER_LEAF) {
+
+			return p_indices_count; // all these points are visible
+		}
+		return _get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,p_indices_count);
+	}
+
+	return 0;
+}
+
+int BSP_Tree::get_points_inside(const Vector3* p_points,int p_point_count) const {
+
+
+	if (nodes.size()==0)
+		return 0;
+
+#if 1
+//this version is easier to debug, and and MUCH faster in real world cases
+
+	int pass_count = 0;
+	const Node *nodesptr=&nodes[0];
+	const Plane *planesptr=&planes[0];
+	int plane_count=planes.size();
+	int node_count=nodes.size();
+
+	if (node_count==0) // no nodes!
+		return 0;
+
+	for(int i=0;i<p_point_count;i++) {
+
+		const Vector3& point = p_points[i];
+		if (!aabb.has_point(point)) {
+			continue;
+		}
+
+		int idx=node_count-1;
+
+		bool pass=false;
+
+		while(true) {
+
+			if (idx==OVER_LEAF) {
+				pass=false;
+				break;
+			} else if (idx==UNDER_LEAF) {
+				pass=true;
+				break;
+			}
+
+			uint16_t plane=nodesptr[ idx ].plane;
+#ifdef DEBUG_ENABLED
+
+			ERR_FAIL_INDEX_V( plane, plane_count, false );
+#endif
+
+			idx = planesptr[ nodesptr[ idx ].plane ].is_point_over(point) ? nodes[ idx ].over : nodes[ idx ].under;
+
+#ifdef DEBUG_ENABLED
+
+			ERR_FAIL_COND_V( idx<MAX_NODES && idx>=node_count, false );
+#endif
+
+		}
+
+		if (pass)
+			pass_count++;
+	}
+
+	return pass_count;
+
+#else
+//this version scales better but it's slower for real world cases
+
+	int *indices = (int*)alloca(p_point_count*sizeof(int));
+	AABB bounds;
+
+	for(int i=0;i<p_point_count;i++) {
+
+		indices[i]=i;
+		if (i==0)
+			bounds.pos=p_points[i];
+		else
+			bounds.expand_to(p_points[i]);
+
+	}
+
+	Vector3 half_extents = bounds.size/2.0;
+	return _get_points_inside(nodes.size()+1,p_points,indices,bounds.pos+half_extents,half_extents,p_point_count);
+#endif
+}
+
+
+
+bool BSP_Tree::point_is_inside(const Vector3& p_point) const {
+
+	if (!aabb.has_point(p_point)) {
+		return false;
+	}
+	
+	int node_count=nodes.size();
+	
+	if (node_count==0) // no nodes!
+		return false;
+	
+	
+	const Node *nodesptr=&nodes[0];
+	const Plane *planesptr=&planes[0];
+	int plane_count=planes.size();
+		
+	int idx=node_count-1;
+	int steps=0;
+	
+	while(true) {
+	
+		if (idx==OVER_LEAF) {
+			return false;
+		}
+		if (idx==UNDER_LEAF) {
+
+			return true;
+		}
+			
+		uint16_t plane=nodesptr[ idx ].plane;
+#ifdef DEBUG_ENABLED
+			
+		ERR_FAIL_INDEX_V( plane, plane_count, false );
+#endif
+		bool over = planesptr[ nodesptr[ idx ].plane ].is_point_over(p_point);
+
+		idx =  over ? nodes[ idx ].over : nodes[ idx ].under;
+		
+#ifdef DEBUG_ENABLED
+			
+		ERR_FAIL_COND_V( idx<MAX_NODES && idx>=node_count, false );
+#endif
+		
+		steps++;
+	}
+
+	return false;
+}
+	
+	
+static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_indices,float p_tolerance) {
+
+	int ic = p_indices.size();
+	const int*indices=p_indices.ptr();
+
+	int best_plane = -1;
+	float best_plane_cost = 1e20;
+
+	// Loop to find the polygon that best divides the set.
+
+	for (int i=0;i<ic;i++) {
+
+		const Face3& f=p_faces[ indices[i] ];
+		Plane p = f.get_plane();
+		
+		int num_over=0,num_under=0,num_spanning=0;
+
+		for(int j=0;j<ic;j++) {
+
+			if (i==j)
+				continue;
+
+			const Face3& g=p_faces[ indices[j] ];
+			int over=0,under=0;
+
+			for(int k=0;k<3;k++) {
+
+				float d = p.distance_to(g.vertex[j]);
+
+				if (Math::abs(d)>p_tolerance) {
+
+					if (d > 0)
+						over++;
+					else
+						under++;
+				}
+
+			}
+
+			if (over && under)
+				num_spanning++;
+			else if (over)
+				num_over++;
+			else
+				num_under++;
+				
+		}
+
+
+
+		//double split_cost = num_spanning / (double) face_count;
+		double relation = Math::abs(num_over-num_under) / (double) ic;
+		
+		// being honest, i never found a way to add split cost to the mix in a meaninguful way
+		// in this engine, also, will likely be ignored anyway
+		
+		double plane_cost = /*split_cost +*/ relation;
+
+		//printf("plane %i, %i over, %i under, %i spanning, cost is %g\n",i,num_over,num_under,num_spanning,plane_cost);
+		if (plane_cost<best_plane_cost) {
+
+			best_plane=i;
+			best_plane_cost=plane_cost;
+		}
+
+	}
+
+	return best_plane;
+
+}
+	
+
+static int _bsp_create_node(const Face3 *p_faces,const Vector<int>& p_indices,Vector<Plane> &p_planes, Vector<BSP_Tree::Node> &p_nodes,float p_tolerance) {
+	
+	ERR_FAIL_COND_V( p_nodes.size() == BSP_Tree::MAX_NODES, -1 );
+
+	// should not reach here
+	ERR_FAIL_COND_V( p_indices.size() == 0, -1 )
+
+	int ic = p_indices.size();
+	const int*indices=p_indices.ptr();
+
+	int divisor_idx = _bsp_find_best_half_plane(p_faces,p_indices,p_tolerance);
+
+	// returned error
+	ERR_FAIL_COND_V( divisor_idx<0 , -1 );
+
+
+	Vector<int> faces_over;
+	Vector<int> faces_under;
+
+	Plane divisor_plane=p_faces[ indices[divisor_idx] ].get_plane();
+
+	for (int i=0;i<ic;i++) {
+
+		if (i==divisor_idx)
+			continue;
+			
+		const Face3& f=p_faces[ indices[i] ];
+
+		//if (f.get_plane().is_almost_like(divisor_plane))
+		//	continue;
+
+		int over_count=0;
+		int under_count=0;
+
+		for(int j=0;j<3;j++) {
+
+			float d = divisor_plane.distance_to(f.vertex[j]);
+			if (Math::abs(d)>p_tolerance) {
+
+				if (d > 0)
+					over_count++;
+				else
+					under_count++;
+			}
+		}
+
+		if (over_count)
+			faces_over.push_back( indices[i] );
+		if (under_count)
+			faces_under.push_back( indices[i] );
+
+	}
+
+
+	
+	uint16_t over_idx=BSP_Tree::OVER_LEAF,under_idx=BSP_Tree::UNDER_LEAF;
+
+	if (faces_over.size()>0) { //have facess above?
+
+		int idx = _bsp_create_node( p_faces, faces_over, p_planes, p_nodes,p_tolerance );
+		if (idx>=0)
+			over_idx=idx;
+	}
+
+	if (faces_under.size()>0) { //have facess above?
+
+		int idx = _bsp_create_node( p_faces,faces_under, p_planes, p_nodes,p_tolerance );
+		if (idx>=0)
+			under_idx=idx;
+	}
+
+	/* Create the node */
+	
+	// find existing divisor plane
+	int divisor_plane_idx=-1;
+	
+	
+	for (int i=0;i<p_planes.size();i++) {
+	
+		if (p_planes[i].is_almost_like( divisor_plane )) {
+			divisor_plane_idx=i;
+			break;
+		}
+	}
+
+	if (divisor_plane_idx==-1) {
+	
+		ERR_FAIL_COND_V( p_planes.size() == BSP_Tree::MAX_PLANES, -1 );
+		divisor_plane_idx=p_planes.size();
+		p_planes.push_back( divisor_plane );
+	}
+	
+	BSP_Tree::Node node;
+	node.plane=divisor_plane_idx;
+	node.under=under_idx;
+	node.over=over_idx;
+	
+	p_nodes.push_back(node);
+	
+	return p_nodes.size()-1;
+}
+	
+
+BSP_Tree::operator Variant() const {
+
+
+	Dictionary d;
+	d["error_radius"]=error_radius;
+
+	Vector<float> plane_values;
+	plane_values.resize(planes.size()*4);
+
+	for(int i=0;i<planes.size();i++) {
+
+		plane_values[i*4+0]=planes[i].normal.x;
+		plane_values[i*4+1]=planes[i].normal.y;
+		plane_values[i*4+2]=planes[i].normal.z;
+		plane_values[i*4+3]=planes[i].d;
+	}
+
+	d["planes"]=plane_values;
+
+	DVector<int> dst_nodes;
+	dst_nodes.resize(nodes.size()*3);
+
+	for(int i=0;i<nodes.size();i++) {
+
+		dst_nodes.set(i*3+0,nodes[i].over);
+		dst_nodes.set(i*3+1,nodes[i].under);
+		dst_nodes.set(i*3+2,nodes[i].plane);
+	}
+
+
+	d["nodes"]=dst_nodes;
+	d["aabb"] = aabb;
+
+	return Variant(d);
+}
+
+BSP_Tree::BSP_Tree() {
+
+}
+
+
+BSP_Tree::BSP_Tree(const Variant& p_variant) {
+
+	Dictionary d=p_variant;
+	ERR_FAIL_COND(!d.has("nodes"));
+	ERR_FAIL_COND(!d.has("planes"));
+	ERR_FAIL_COND(!d.has("aabb"));
+	ERR_FAIL_COND(!d.has("error_radius"));
+
+	DVector<int> src_nodes = d["nodes"];
+	ERR_FAIL_COND(src_nodes.size()%3);
+
+
+	if (d["planes"].get_type()==Variant::REAL_ARRAY) {
+
+		DVector<float> src_planes=d["planes"];
+		int plane_count=src_planes.size();
+		ERR_FAIL_COND(plane_count%4);
+		planes.resize(plane_count/4);
+
+		if (plane_count) {
+			DVector<float>::Read r = src_planes.read();
+			for(int i=0;i<plane_count/4;i++) {
+
+				planes[i].normal.x=r[i*4+0];
+				planes[i].normal.y=r[i*4+1];
+				planes[i].normal.z=r[i*4+2];
+				planes[i].d=r[i*4+3];
+			}
+		}
+
+
+	} else {
+
+		planes = d["planes"];
+	}
+
+
+	error_radius = d["error"];
+	aabb = d["aabb"];
+
+//	int node_count = src_nodes.size();
+	nodes.resize(src_nodes.size()/3);
+
+	DVector<int>::Read r = src_nodes.read();
+
+	for(int i=0;i<nodes.size();i++) {
+
+		nodes[i].over=r[i*3+0];
+		nodes[i].under=r[i*3+1];
+		nodes[i].plane=r[i*3+2];
+	}
+
+}
+
+BSP_Tree::BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius) {
+
+	// compute aabb
+		
+	int face_count=p_faces.size();
+	DVector<Face3>::Read faces_r=p_faces.read();
+	const Face3 *facesptr = faces_r.ptr();
+
+
+	bool first=true;
+
+	Vector<int> indices;
+
+	for (int i=0;i<face_count;i++) {
+			
+		const Face3& f=facesptr[i];
+		
+		if (f.is_degenerate())
+			continue;
+			
+		for (int j=0;j<3;j++) {
+		
+			if (first) {
+			
+				aabb.pos=f.vertex[0];
+				first=false;
+			} else {
+			
+				aabb.expand_to(f.vertex[j]);
+			}
+		}
+
+		indices.push_back(i);
+		
+	}
+
+	ERR_FAIL_COND( aabb.has_no_area() );
+
+	int top = _bsp_create_node(faces_r.ptr(),indices,planes,nodes,aabb.get_longest_axis_size()*0.0001);
+
+	if (top<0) {
+
+		nodes.clear();
+		planes.clear();
+		ERR_FAIL_COND( top < 0 );
+	}
+
+
+
+	
+	error_radius=p_error_radius;
+}
+
+BSP_Tree::BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB& p_aabb,float p_error_radius) {
+
+	nodes=p_nodes;
+	planes=p_planes;
+	aabb=p_aabb;
+	error_radius=p_error_radius;
+
+}
+
+BSP_Tree::~BSP_Tree() {
+
+
+}