godot/core/math/triangle_mesh.cpp

/*************************************************************************/
/*  triangle_mesh.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 "triangle_mesh.h"
#include "sort.h"



int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_depth,int&max_depth,int&max_alloc) {


	if (p_depth>max_depth) {
		max_depth=p_depth;
	}

	if (p_size==1) {


		return p_bb[p_from]-p_bvh;
	} else if (p_size==0) {

		return -1;
	}


	AABB aabb;
	aabb=p_bb[p_from]->aabb;
	for(int i=1;i<p_size;i++) {

		aabb.merge_with(p_bb[p_from+i]->aabb);
	}

	int li=aabb.get_longest_axis_index();

	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]);
			//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]);
			//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]);
			//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 index=max_alloc++;
	BVH *_new = &p_bvh[index];
	_new->aabb=aabb;
	_new->center=aabb.pos+aabb.size*0.5;
	_new->face_index=-1;
	_new->left=left;
	_new->right=right;

	return index;

}


void TriangleMesh::create(const DVector<Vector3>& p_faces) {

	valid=false;

	int fc=p_faces.size();
	ERR_FAIL_COND(!fc || ((fc%3) != 0));
	fc/=3;
	triangles.resize(fc);

	bvh.resize(fc*3); //will never be larger than this (todo make better)
	DVector<BVH>::Write bw = bvh.write();

	{

		//create faces and indices and base bvh
		//except for the Set for repeated triangles, everything
		//goes in-place.

		DVector<Vector3>::Read r = p_faces.read();
		DVector<Triangle>::Write w = triangles.write();
		Map<Vector3,int> db;

		for(int i=0;i<fc;i++) {

			Triangle&f=w[i];
			const Vector3 *v=&r[i*3];

			for(int j=0;j<3;j++) {

				int vidx=-1;
				Vector3 vs=v[j].snapped(0.0001);
				Map<Vector3,int>::Element *E=db.find(vs);
				if (E) {
					vidx=E->get();
				} else {
					vidx=db.size();
					db[vs]=vidx;
				}

				f.indices[j]=vidx;
				if (j==0)
					bw[i].aabb.pos=vs;
				else
					bw[i].aabb.expand_to(vs);
			}

			f.normal=Face3(r[i*3+0],r[i*3+1],r[i*3+2]).get_plane().get_normal();

			bw[i].left=-1;
			bw[i].right=-1;
			bw[i].face_index=i;
			bw[i].center=bw[i].aabb.pos+bw[i].aabb.size*0.5;
		}

		vertices.resize(db.size());
		DVector<Vector3>::Write vw = vertices.write();
		for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) {
			vw[E->get()]=E->key();
		}

	}

	DVector<BVH*> bwptrs;
	bwptrs.resize(fc);
	DVector<BVH*>::Write bwp = bwptrs.write();
	for(int i=0;i<fc;i++) {

		bwp[i]=&bw[i];
	}

	max_depth=0;
	int max_alloc=fc;
	int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc);

	bw=DVector<BVH>::Write(); //clearup
	bvh.resize(max_alloc); //resize back

	valid=true;

}


Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const {

	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth);

	enum {
		TEST_AABB_BIT=0,
		VISIT_LEFT_BIT=1,
		VISIT_RIGHT_BIT=2,
		VISIT_DONE_BIT=3,
		VISITED_BIT_SHIFT=29,
		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
		VISITED_BIT_MASK=~NODE_IDX_MASK,


	};

	int n_count=0;
	Vector3 n;

	//for(int i=0;i<max_depth;i++)
	//	stack[i]=0;

	int level=0;

	DVector<Triangle>::Read trianglesr = triangles.read();
	DVector<Vector3>::Read verticesr=vertices.read();
	DVector<BVH>::Read bvhr=bvh.read();

	const Triangle *triangleptr=trianglesr.ptr();
	const Vector3 *vertexptr=verticesr.ptr();
	int pos=bvh.size()-1;
	const BVH *bvhptr = bvhr.ptr();

	stack[0]=pos;
	while(true) {

		uint32_t node = stack[level]&NODE_IDX_MASK;
		const BVH &b = bvhptr[ node ];
		bool done=false;

		switch(stack[level]>>VISITED_BIT_SHIFT) {
			case TEST_AABB_BIT: {


				bool valid = b.aabb.intersects(p_aabb);
				if (!valid) {

					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;

				} else {

					if (b.face_index>=0) {

						const Triangle &s=triangleptr[ b.face_index ];
						n+=s.normal;
						n_count++;

						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;
				level++;

			} continue;
			case VISIT_RIGHT_BIT: {

				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
				stack[level+1]=b.right|TEST_AABB_BIT;
				level++;
			} continue;
			case VISIT_DONE_BIT: {

				if (level==0) {
					done=true;
					break;
				} else
					level--;

			} continue;
		}


		if (done)
			break;
	}


	if (n_count>0)
		n/=n_count;

	return n;

}


bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {


	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth);

	enum {
		TEST_AABB_BIT=0,
		VISIT_LEFT_BIT=1,
		VISIT_RIGHT_BIT=2,
		VISIT_DONE_BIT=3,
		VISITED_BIT_SHIFT=29,
		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
		VISITED_BIT_MASK=~NODE_IDX_MASK,


	};

	Vector3 n = (p_end-p_begin).normalized();
	real_t d=1e10;
	bool inters=false;

	//for(int i=0;i<max_depth;i++)
	//	stack[i]=0;

	int level=0;
	//AABB ray_aabb;
	//ray_aabb.pos=p_begin;
	//ray_aabb.expand_to(p_end);


	DVector<Triangle>::Read trianglesr = triangles.read();
	DVector<Vector3>::Read verticesr=vertices.read();
	DVector<BVH>::Read bvhr=bvh.read();

	const Triangle *triangleptr=trianglesr.ptr();
	const Vector3 *vertexptr=verticesr.ptr();
	int pos=bvh.size()-1;
	const BVH *bvhptr = bvhr.ptr();

	stack[0]=pos;
	while(true) {

		uint32_t node = stack[level]&NODE_IDX_MASK;
		const BVH &b = bvhptr[ node ];
		bool done=false;

		switch(stack[level]>>VISITED_BIT_SHIFT) {
			case TEST_AABB_BIT: {


				bool valid = b.aabb.intersects_segment(p_begin,p_end);
//				bool valid = b.aabb.intersects(ray_aabb);

				if (!valid) {

					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;

				} else {

					if (b.face_index>=0) {

						const Triangle &s=triangleptr[ b.face_index ];
						Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]);


						Vector3 res;

						if (f3.intersects_segment(p_begin,p_end,&res)) {


							float nd = n.dot(res);
							if (nd<d) {

								d=nd;
								r_point=res;
								r_normal=f3.get_plane().get_normal();
								inters=true;
							}

						}

						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;
				level++;

			} continue;
			case VISIT_RIGHT_BIT: {

				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
				stack[level+1]=b.right|TEST_AABB_BIT;
				level++;
			} continue;
			case VISIT_DONE_BIT: {

				if (level==0) {
					done=true;
					break;
				} else
					level--;

			} continue;
		}


		if (done)
			break;
	}


	if (inters) {

		if (n.dot(r_normal)>0)
			r_normal=-r_normal;
	}

	return inters;
}


bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vector3 &r_point, Vector3 &r_normal) const {


	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth);

	enum {
		TEST_AABB_BIT=0,
		VISIT_LEFT_BIT=1,
		VISIT_RIGHT_BIT=2,
		VISIT_DONE_BIT=3,
		VISITED_BIT_SHIFT=29,
		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
		VISITED_BIT_MASK=~NODE_IDX_MASK,


	};

	Vector3 n = p_dir;
	real_t d=1e20;
	bool inters=false;

	//for(int i=0;i<max_depth;i++)
	//	stack[i]=0;

	int level=0;

	DVector<Triangle>::Read trianglesr = triangles.read();
	DVector<Vector3>::Read verticesr=vertices.read();
	DVector<BVH>::Read bvhr=bvh.read();

	const Triangle *triangleptr=trianglesr.ptr();
	const Vector3 *vertexptr=verticesr.ptr();
	int pos=bvh.size()-1;
	const BVH *bvhptr = bvhr.ptr();

	stack[0]=pos;
	while(true) {

		uint32_t node = stack[level]&NODE_IDX_MASK;
		const BVH &b = bvhptr[ node ];
		bool done=false;

		switch(stack[level]>>VISITED_BIT_SHIFT) {
			case TEST_AABB_BIT: {


				bool valid = b.aabb.intersects_ray(p_begin,p_dir);
				if (!valid) {

					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;

				} else {

					if (b.face_index>=0) {

						const Triangle &s=triangleptr[ b.face_index ];
						Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]);


						Vector3 res;

						if (f3.intersects_ray(p_begin,p_dir,&res)) {


							float nd = n.dot(res);
							if (nd<d) {

								d=nd;
								r_point=res;
								r_normal=f3.get_plane().get_normal();
								inters=true;
							}

						}

						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;
				level++;

			} continue;
			case VISIT_RIGHT_BIT: {

				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
				stack[level+1]=b.right|TEST_AABB_BIT;
				level++;
			} continue;
			case VISIT_DONE_BIT: {

				if (level==0) {
					done=true;
					break;
				} else
					level--;

			} continue;
		}


		if (done)
			break;
	}


	if (inters) {

		if (n.dot(r_normal)>0)
			r_normal=-r_normal;
	}

	return inters;
}

bool TriangleMesh::is_valid() const {

	return valid;
}

DVector<Face3> TriangleMesh::get_faces() const {

	if (!valid)
		return DVector<Face3>();

	DVector<Face3> faces;
	int ts = triangles.size();
	faces.resize(triangles.size());

	DVector<Face3>::Write w=faces.write();
	DVector<Triangle>::Read r = triangles.read();
	DVector<Vector3>::Read rv = vertices.read();

	for(int i=0;i<ts;i++) {
		for(int j=0;j<3;j++) {
			w[i].vertex[j]=rv[r[i].indices[j]];
		}
	}

	w = DVector<Face3>::Write();
	return faces;
}

TriangleMesh::TriangleMesh() {

	valid=false;
	max_depth=0;
}