text:
#include "Defined_Tensor.h"
#include <Eigen\Dense>
#include <queue>

defined_tensor::defined_tensor()
{
	m_tensor_name = Utility_Definition::T_IDENTITY;
	AABB_Tree = NULL;
	AABB_Segment_Tree = NULL;
	bm = NULL;
	ref_mesh_ = NULL;
	kdTree = NULL;
}

defined_tensor::~defined_tensor()
{
	if(AABB_Tree) delete AABB_Tree;
	if(AABB_Segment_Tree) delete AABB_Segment_Tree;
	if(bm) delete bm;
	if(kdTree) {delete kdTree; annClose();}
}

void defined_tensor::get_tensor( const OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M )
{
	double det = 1.0; double t = 1.0;
	switch (m_tensor_name)
	{
	case Utility_Definition::T_IDENTITY:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 1.0;
		break;
	case Utility_Definition::T_10_1_1:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 100.0;
		break;
	case Utility_Definition::T_X2_Y2_Z4: //0.5*(x^2 + y^2 + z^4)
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 6*p[2]*p[2];
		det = M[0]*M[3]*M[5] + M[1]*M[4]*M[2] + M[2]*M[1]*M[4] - M[2]*M[3]*M[2] - M[4]*M[4]*M[1]- M[5]*M[1]*M[1];
		t = std::pow( det, -0.2);
		M *= t;
		break;
	case Utility_Definition::T_EXP:
		M[0] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[0]*p[0]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[3] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[1]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[5] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[2]*p[2]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[1] = (p[0]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[4] = (p[2]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[2] = (p[2]*p[0]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		det = M[0]*M[3]*M[5] + M[1]*M[4]*M[2] + M[2]*M[1]*M[4] - M[2]*M[3]*M[2] - M[4]*M[4]*M[1]- M[5]*M[1]*M[1];
		t = std::pow( det, -0.2);
		M *= t;
		break;
	default:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 1.0;
		break;
	}

	if(Utility_Definition::T_SPHERICAL_SHOCK == m_tensor_name)
	{
		double t = 7;
		Eigen::Matrix3d R;
		double r = p.norm();
		OpenVolumeMesh::Geometry::Vec3d e_r = p / r;
		local_frame lf; lf.n = e_r; lf.find_e_x_y();
		R(0,0) =    e_r[0]; R(0,1) =    e_r[1]; R(0,2) =    e_r[2];
		R(1,0) = lf.e_y[0]; R(1,1) = lf.e_y[1]; R(1,2) = lf.e_y[2];
		R(2,0) = lf.e_x[0]; R(2,1) = lf.e_x[1]; R(2,2) = lf.e_x[2];

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.025 + 1.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 2.0 / 2.0; D(2,2) = 2.0 / 2.0;
		Eigen::Matrix3d TM  = R.transpose() *D * D * R;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}
	else if (Utility_Definition::T_CYLINDER_SHOCK == m_tensor_name)
	{
		double t = 7; Eigen::Matrix3d R;
		double r = std::sqrt(p[0]*p[0] + p[1]*p[1]);
		R(0,0) =  p[0]/r; R(0,1) = p[1]/r; R(0,2) = 0;
		R(1,0) = -p[1]/r; R(1,1) = p[0]/r; R(1,2) = 0;
		R(2,0) = 0; R(2,1) = 0; R(2,2) = 1;

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 2.0/( 0.1 + 2.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 2.0 / 2.0; D(2,2) = 2.0 / 2.0;
		Eigen::Matrix3d TM  = R.transpose() *D * D * R;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_PLANAR_SHOCK == m_tensor_name)
	{
		double t = 0.6;
		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.0025 + 0.2 * (1.0 - std::exp( -1.0*std::abs(p[0] - t) ) ) );
		D(1,1) = 1.0 / 0.2; D(2,2) = 1.0 / 0.2;
		Eigen::Matrix3d TM = D * D;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_SINE == m_tensor_name)
	{
		//printf("Sine tensor!!!!!!!\n");
		Eigen::Matrix3d R;
		OpenVolumeMesh::Geometry::Vec3d e_1( 2.0*std::cos(p[0]*6.0), 1.0, 0.0 ); e_1.normalize();
		OpenVolumeMesh::Geometry::Vec3d e_2(0.0, 1.0, 0.0);
		e_2 = OpenVolumeMesh::Geometry::cross(e_1, e_2); e_2.normalize();
		OpenVolumeMesh::Geometry::Vec3d e_3 = OpenVolumeMesh::Geometry::cross(e_1, e_2);

		R(0, 0) = e_1[0]; R(0, 1) = e_1[1]; R(0, 2) = e_1[2];
		R(1, 0) = e_2[0]; R(1, 1) = e_2[1]; R(1, 2) = e_2[2];
		R(2, 0) = e_3[0]; R(2, 1) = e_3[1]; R(2, 2) = e_3[2];

		//double R_det = R.determinant();

		Eigen::Matrix3d D; D.setZero();
		D(0, 0) = 100;
		D(1, 1) = 1.0; D(2, 2) = 1.0;
		D *= 10;
		Eigen::Matrix3d TM = R.transpose()* D * R;

		M[0] = TM(0, 0); M[1] = TM(0, 1); M[2] = TM(0, 2);
		M[3] = TM(1, 1); M[4] = TM(1, 2); M[5] = TM(2, 2);

		double D00 = 1000; double D11 = 10.0; double D22 = 10.0;

		/*M[0] = D00*e_1[0] * e_1[0] + D11*e_2[0] * e_2[0] + D22*e_3[0] * e_3[0];
		M[1] = e_1[0] * e_1[1] * D00 + e_2[0] * e_2[1] * D11 + e_3[0] * e_3[1] * D22;
		M[2] = e_1[0] * e_1[2] * D00 + e_2[0] * e_2[2] * D11 + e_3[0] * e_3[2] * D22;
		M[3] = D00*e_1[1] * e_1[1] + D11*e_2[1] * e_2[1] + D22*e_3[1] * e_3[1];
		M[4] = e_1[1] * e_1[2] * D00 + e_2[1] * e_2[2] * D11 + e_3[1] * e_3[2] * D22;
		M[5] = D00*e_1[2] * e_1[2] + D11*e_2[2] * e_2[2] + D22*e_3[2] * e_3[2];*/

		double M0 = D00*e_1[0] * e_1[0] + D11*e_2[0] * e_2[0] + D22*e_3[0] * e_3[0];
		double M1 = e_1[0] * e_1[1] * D00 + e_2[0] * e_2[1] * D11 + e_3[0] * e_3[1] * D22;
		double M2 = e_1[0] * e_1[2] * D00 + e_2[0] * e_2[2] * D11 + e_3[0] * e_3[2] * D22;
		double M3 = D00*e_1[1] * e_1[1] + D11*e_2[1] * e_2[1] + D22*e_3[1] * e_3[1];
		double M4 = e_1[1] * e_1[2] * D00 + e_2[1] * e_2[2] * D11 + e_3[1] * e_3[2] * D22;
		double M5 = D00*e_1[2] * e_1[2] + D11*e_2[2] * e_2[2] + D22*e_3[2] * e_3[2];

		/*M[0] = D00*e_1[0] * e_1[0] + D11*e_1[1] * e_1[1] + D22*e_1[2] * e_1[2];
		M[1] = e_1[0] * e_2[0] * D00 + e_1[1] * e_2[1] * D11 + e_1[2] * e_2[2] * D22;
		M[2] = e_1[0] * e_3[0] * D00 + e_1[1] * e_3[1] * D11 + e_1[2] * e_3[2] * D22;
		M[3] = D00*e_2[0] * e_2[0] + D11*e_2[1] * e_2[1] + D22*e_2[2] * e_2[2];
		M[4] = e_2[0] * e_3[0] * D00 + e_2[1] * e_3[1] * D11 + e_2[2] * e_3[2] * D22;
		M[5] = D00*e_3[0] * e_3[0] + D11*e_3[1] * e_3[1] + D22*e_3[2] * e_3[2];*/
	}

	if(Utility_Definition::T_SINK == m_tensor_name)
	{
		Eigen::Matrix3d R;
		double r = std::sqrt(p[0]*p[0] + p[1]*p[1]);
		R(0,0) =  p[0]/r; R(0,1) = p[1]/r; R(0,2) = 0;
		R(1,0) = -p[1]/r; R(1,1) = p[0]/r; R(1,2) = 0;
		R(2,0) = 0; R(2,1) = 0; R(2,2) = 1;

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0;
		D(1,1) = 1.0 + r*5;
		D(2,2) = 1.0 + r*5;
		D *= 1.2/(0.5 + 1 - std::exp(-0.05*(r*r - 2.56)) );
		Eigen::Matrix3d TM = R.transpose() *D * D * R ;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_TEST == m_tensor_name)
	{
		double t = 4;
		Eigen::Matrix3d R;
		double r = p.norm();
		OpenVolumeMesh::Geometry::Vec3d e_r = p / r;
		local_frame lf; lf.n = e_r; lf.find_e_x_y();
		R(0,0) =    e_r[0]; R(0,1) =    e_r[1]; R(0,2) =    e_r[2];
		R(1,0) = lf.e_y[0]; R(1,1) = lf.e_y[1]; R(1,2) = lf.e_y[2];
		R(2,0) = lf.e_x[0]; R(2,1) = lf.e_x[1]; R(2,2) = lf.e_x[2];

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.05 + 1.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 1; D(2,2) = 1;
		D *= 1.25;
		Eigen::Matrix3d TM = R.transpose() *D * D * R ;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}
}

void defined_tensor::build_AABB_Tree_using_ref_mesh(SurfaceMesh* mesh_)
{
	if(AABB_Tree) {delete AABB_Tree; AABB_Tree = NULL;}

	unsigned nf = mesh_->n_faces();
	if (nf == 0) return;
	opp_face_id.resize(nf); face_pos.resize(nf);
	std::vector<int> one_face_opp_face_id(3); std::vector<OpenVolumeMesh::Geometry::Vec3d> one_face_pos(3);
	visited_ref_face_id.resize(nf, -1); visited_ref_face.resize(nf, -1);

	std::vector<CGAL_double_3_Point> v_pos( mesh_->n_vertices() ); OpenMesh::Vec3d p;
	for(SurfaceMesh::VertexIter v_it = mesh_->vertices_begin(); v_it != mesh_->vertices_end(); ++v_it)
	{
		int vertex_id = v_it.handle().idx();
		p = mesh_->point( v_it );
		v_pos[vertex_id] = CGAL_double_3_Point( p[0], p[1], p[2] );
	}

	triangle_vectors.clear();
	triangle_vectors.resize(nf); int fv_id[3];
	for(SurfaceMesh::FaceIter f_it = mesh_->faces_begin(); f_it != mesh_->faces_end(); ++f_it)
	{
		SurfaceMesh::FaceVertexIter fv_it = mesh_->fv_iter(f_it);
		fv_id[0] = fv_it.handle().idx(); ++fv_it;
		fv_id[1] = fv_it.handle().idx(); ++fv_it;
		fv_id[2] = fv_it.handle().idx(); ++fv_it;
		one_face_pos[0] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[0]].x(), v_pos[fv_id[0]].y(), v_pos[fv_id[0]].z());
		one_face_pos[1] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[1]].x(), v_pos[fv_id[1]].y(), v_pos[fv_id[1]].z());
		one_face_pos[2] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[2]].x(), v_pos[fv_id[2]].y(), v_pos[fv_id[2]].z());
		face_pos[f_it.handle().idx()] = one_face_pos;
		SurfaceMesh::FaceHalfedgeIter fhe_it = mesh_->fh_iter(f_it);
		one_face_opp_face_id[0] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		++fhe_it; one_face_opp_face_id[1] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		++fhe_it; one_face_opp_face_id[2] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		opp_face_id[f_it.handle().idx()] = one_face_opp_face_id;

		triangle_vectors[f_it.handle().idx()] = CGAL_3_Triangle(v_pos[fv_id[0]], v_pos[fv_id[1]], v_pos[fv_id[2]]);

		if (fv_it)
		{
			fv_id[1] = fv_id[2];
			fv_id[2] = fv_it.handle().idx();
			triangle_vectors.push_back(CGAL_3_Triangle(v_pos[fv_id[0]], v_pos[fv_id[1]], v_pos[fv_id[2]]));
		}
	}
	AABB_Tree = new CGAL_AABB_Tree(triangle_vectors.begin(), triangle_vectors.end());
	AABB_Tree->accelerate_distance_queries();

	printf("Finish Constructing AABB Tree.\n");
}

void defined_tensor::build_AABB_Tree_using_ref_mesh(VolumeMesh* mesh_)
{
	if (AABB_Tree) { delete AABB_Tree; AABB_Tree = NULL; }
	triangle_vectors.clear();
	for (OpenVolumeMesh::FaceIter f_it = mesh_->faces_begin(); f_it != mesh_->faces_end(); ++f_it)
	{
		if (!mesh_->is_boundary(*f_it)) continue;

		OpenVolumeMesh::HalfFaceHandle hfHandle = mesh_->halfface_handle(*f_it, 0);
		OpenVolumeMesh::HalfFaceHandle hfHandle2 = mesh_->halfface_handle(*f_it, 1);
		int cell_id = mesh_->incident_cell(hfHandle2).idx();
		if (cell_id < 0)
		{
			hfHandle2 = hfHandle;
			cell_id = mesh_->incident_cell(hfHandle2).idx();
			hfHandle = mesh_->halfface_handle(*f_it, 1);
		}

		OpenVolumeMesh::HalfFaceVertexIter hfv_it = mesh_->hfv_iter(hfHandle);
		OpenVolumeMesh::Geometry::Vec3d p0 = mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = mesh_->vertex(*hfv_it);

		CGAL_double_3_Point q0(p0[0], p0[1], p0[2]);
		CGAL_double_3_Point q1(p1[0], p1[1], p1[2]);
		CGAL_double_3_Point q2(p2[0], p2[1], p2[2]);

		triangle_vectors.push_back( CGAL_3_Triangle(q0, q1, q2) );
	}

	AABB_Tree = new CGAL_AABB_Tree(triangle_vectors.begin(), triangle_vectors.end());
	AABB_Tree->accelerate_distance_queries();

	printf("Finish Constructing AABB Tree.\n");
}

void defined_tensor::build_AABB_Segment_Tree_using_ref_mesh(VolumeMesh* mesh_, const std::vector<int>& edge_feature)
{
	if(AABB_Segment_Tree) {delete AABB_Segment_Tree; AABB_Segment_Tree = NULL;}
	if(mesh_->n_edges() == 0) return;

	std::vector<CGAL_3_Segment> segment_vectors;
	for(unsigned i=0;i<edge_feature.size();++i)
	{
		if(edge_feature[i] == 1)
		{
			OpenVolumeMesh::EdgeHandle eh(i);
			OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge(eh);
			OpenVolumeMesh::VertexHandle vh_from = edge.from_vertex();
			OpenVolumeMesh::VertexHandle vh_to = edge.to_vertex();
			OpenVolumeMesh::Geometry::Vec3d p_from = mesh_->vertex(vh_from);
			OpenVolumeMesh::Geometry::Vec3d p_to = mesh_->vertex(vh_to);
			CGAL_double_3_Point pf(p_from[0], p_from[1], p_from[2]);
			CGAL_double_3_Point pt(p_to[0], p_to[1], p_to[2]);
			segment_vectors.push_back( CGAL_3_Segment(pf, pt) );
		}
	}
	AABB_Segment_Tree = new CGAL_AABB_Segment_Tree(segment_vectors.begin(), segment_vectors.end());
	AABB_Segment_Tree->accelerate_distance_queries();
	printf("Finish Constructing AABB Segment Tree.\n");
}

void defined_tensor::project_on_ref_mesh(OpenVolumeMesh::Geometry::Vec3d& p)
{
	CGAL_double_3_Point pos = AABB_Tree->closest_point( CGAL_double_3_Point(p[0], p[1], p[2]) );
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );
}

//if input face id = -1, will change face_id , p
void defined_tensor::project_on_ref_mesh_with_guess_face(OpenVolumeMesh::Geometry::Vec3d& p, int& guess_face_id, const double& radius)
{
	if (guess_face_id == -1)
	{
		CGAL_AABB_Tree::Point_and_primitive_id point_primitive = AABB_Tree->closest_point_and_primitive(CGAL_double_3_Point(p[0], p[1], p[2]));
		CGAL_double_3_Point pos = point_primitive.first;
		CGAL_Triangle_Iterator it = point_primitive.second;
		guess_face_id = std::distance(triangle_vectors.begin(), it);
		p = OpenVolumeMesh::Geometry::Vec3d(pos.x(), pos.y(), pos.z());
	}
	else
	{
		int visited_face_count = 0; double* p_data = p.data();
		visited_ref_face_id[0] = guess_face_id; visited_ref_face[guess_face_id] = 1; ++visited_face_count;
		OpenVolumeMesh::Geometry::Vec3d np; OpenVolumeMesh::Geometry::Vec3d np_; double min_d = 1.0e30; int min_face_id = guess_face_id;
		double distance_th = 25.0*radius; int current_face_count = 0;
		while (current_face_count < visited_face_count)
		{
			//int face_id = Q.front(); Q.pop();
			int face_id = visited_ref_face_id[current_face_count]; ++current_face_count;
			std::vector<OpenVolumeMesh::Geometry::Vec3d>& one_face_pos = face_pos[face_id];
			//double d = distPointTriangleSquared(p, one_face_pos[0], one_face_pos[1], one_face_pos[2], np);
			double d = distPointTriangleSquared2(p_data, one_face_pos[0].data(), one_face_pos[1].data(), one_face_pos[2].data(), np.data());
			if (d < min_d) { min_d = d; min_face_id = face_id; np_ = np; }

			std::vector<int>& one_face_opp_face_id = opp_face_id[face_id];
			for (unsigned i = 0; i < 3; ++i)
			{
				if (one_face_opp_face_id[i] >= 0 && visited_ref_face[one_face_opp_face_id[i]] == -1)
				{
					std::vector<OpenVolumeMesh::Geometry::Vec3d>& opp_face_pos = face_pos[one_face_opp_face_id[i]];
					OpenVolumeMesh::Geometry::Vec3d& opp_face_pos1 = opp_face_pos[0];
					double d1 = (p[0] - opp_face_pos1[0])*(p[0] - opp_face_pos1[0]) + (p[1] - opp_face_pos1[1])*(p[1] - opp_face_pos1[1]) + (p[2] - opp_face_pos1[2])*(p[2] - opp_face_pos1[2]);
					if (d1 < distance_th)
					{
						visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
						visited_ref_face[one_face_opp_face_id[i]] = 1;
						++visited_face_count;
					}
					else
					{
						OpenVolumeMesh::Geometry::Vec3d& opp_face_pos2 = opp_face_pos[1];
						double d2 = (p[0] - opp_face_pos2[0])*(p[0] - opp_face_pos2[0]) + (p[1] - opp_face_pos2[1])*(p[1] - opp_face_pos2[1]) + (p[2] - opp_face_pos2[2])*(p[2] - opp_face_pos2[2]);
						if (d2 < distance_th)
						{
							visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
							visited_ref_face[one_face_opp_face_id[i]] = 1;
							++visited_face_count;
						}
						else
						{
							OpenVolumeMesh::Geometry::Vec3d& opp_face_pos3 = opp_face_pos[2];
							double d3 = (p[0] - opp_face_pos3[0])*(p[0] - opp_face_pos3[0]) + (p[1] - opp_face_pos3[1])*(p[1] - opp_face_pos3[1]) + (p[2] - opp_face_pos3[2])*(p[2] - opp_face_pos3[2]);
							if (d3 < distance_th)
							{
								visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
								visited_ref_face[one_face_opp_face_id[i]] = 1;
								++visited_face_count;
							}
						}
					}
				}
			}
		}

		for (unsigned i = 0; i < visited_face_count; ++i)
		{
			visited_ref_face[visited_ref_face_id[i]] = -1;
		}

		guess_face_id = min_face_id;
		p = np_;
	}

}

void defined_tensor::project_on_ref_mesh_with_metric(OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M)
{
	CGAL_AABB_Tree::Point_and_primitive_id point_primitive = AABB_Tree->closest_point_and_primitive( CGAL_double_3_Point(p[0], p[1], p[2]) );
	CGAL_double_3_Point pos = point_primitive.first;
	CGAL_Triangle_Iterator it = point_primitive.second;
	unsigned face_id = std::distance( triangle_vectors.begin(), it);
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );

	//printf("F : %d\n", face_id);

	OpenMesh::Vec3d p_(p[0], p[1], p[2]);
	SurfaceMesh::ConstFaceVertexIter fv_it = bm->cfv_iter( bm->face_handle(face_id) );
	OpenMesh::Vec3d p0 = bm->point(fv_it.handle()); int v0_id = fv_it.handle().idx();
	++fv_it; OpenMesh::Vec3d p1 = bm->point(fv_it.handle()); int v1_id = fv_it.handle().idx();
	++fv_it; OpenMesh::Vec3d p2 = bm->point(fv_it.handle()); int v2_id = fv_it.handle().idx();

	OpenMesh::Vec3d bc;
	if (!baryCoord(p_, p0, p1, p2, bc))
		bc[0] = bc[1] = bc[2] = 1.0/3.0;

	M = bc[0] *bm_metric[v0_id] + bc[1] *bm_metric[v1_id] + bc[2] *bm_metric[v2_id];
}

void defined_tensor::project_on_ref_edge(OpenVolumeMesh::Geometry::Vec3d& p)
{
	CGAL_double_3_Point pos = AABB_Segment_Tree->closest_point( CGAL_double_3_Point(p[0], p[1], p[2]) );
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );
}

void defined_tensor::construct_boundary_mesh()
{
	OpenVolumeMesh::BoundaryFaceIter bf_it(ref_mesh_->bf_iter());
	OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter( ref_mesh_->halfface_handle(*bf_it,0) );

	std::vector<SurfaceMesh::VertexHandle> vertexHandleVec;
	std::vector<SurfaceMesh::VertexHandle> faceVertexHandle;
	std::map<int,int> VolumeSurfaceVertex;
	std::map<int,int>::iterator map_it;
	int indexOnSurface = 0;
	int InvalidHF =0;
	bm->clear();
	for (; bf_it.valid(); ++bf_it)
	{
		OpenVolumeMesh::HalfFaceHandle hfHandle = ref_mesh_->halfface_handle(*bf_it,0);
		OpenVolumeMesh::HalfFaceHandle hfHandle2 = ref_mesh_->halfface_handle(*bf_it,1);
		int cell_id = ref_mesh_->incident_cell(hfHandle2).idx();
		if( cell_id >= 0 )
		{
			hfHandle2 = hfHandle;
			cell_id = ref_mesh_->incident_cell(hfHandle2).idx();
			hfHandle = ref_mesh_->halfface_handle(*bf_it,1);
		}
		++InvalidHF;
		hfv_it = ref_mesh_->hfv_iter( hfHandle );
		faceVertexHandle.clear();
		for( hfv_it; hfv_it.valid(); ++hfv_it)
		{
			map_it = VolumeSurfaceVertex.find(hfv_it->idx()) ;
			if( map_it == VolumeSurfaceVertex.end() )
			{
				VolumeSurfaceVertex.insert(std::pair<int,int>( hfv_it->idx(), indexOnSurface ));
				OpenVolumeMesh::Geometry::Vec3d v1 = ref_mesh_->vertex(*hfv_it);
				SurfaceMesh::Point v2( v1[0], v1[1], v1[2] );
				vertexHandleVec.push_back( bm->add_vertex( v2 ) );
				bm->data(vertexHandleVec.back()).set_tet_vertex_id(hfv_it->idx());
				faceVertexHandle.push_back( vertexHandleVec.back() );
				++indexOnSurface;
			}
			else
			{
				faceVertexHandle.push_back(vertexHandleVec[map_it->second]);
			}
		}

		std::reverse(faceVertexHandle.begin(), faceVertexHandle.end());
		OpenMesh::FaceHandle fh = bm->add_face(faceVertexHandle);
		bm->data(fh).set_cell_id( cell_id );
	}

	bm->update_normals();
}

void defined_tensor::save_boundary_mesh_metric()
{
	FILE* f_b_m = fopen("A:\\Code\\VolumeMeshProcessing\\Models\\Tetrahedrization\\elphant\\boundary.off", "w");
	unsigned nv = bm->n_vertices(); unsigned nf = bm->n_faces();
	fprintf(f_b_m, "OFF");
	fprintf(f_b_m, "\n%d %d 0", nv, nf);
	for(SurfaceMesh::VertexIter v_it = bm->vertices_begin(); v_it != bm->vertices_end(); ++v_it)
	{
		OpenMesh::Vec3d p = bm->point(v_it);
		fprintf(f_b_m, "\n%20.19f %20.19f %20.19f", p[0], p[1], p[2]);
	}
	for(SurfaceMesh::FaceIter f_it = bm->faces_begin(); f_it != bm->faces_end(); ++f_it)
	{
		fprintf(f_b_m, "\n3");
		for( SurfaceMesh::FaceVertexIter fv_it = bm->fv_iter(f_it); fv_it; ++fv_it )
		{
			fprintf(f_b_m, " %d", fv_it.handle().idx());
		}
	}
	for(unsigned i=0;i<bm_metric.size();++i)
	{
		fprintf(f_b_m, "\n%20.19f %20.19f %20.19f %20.19f %20.19f %20.19f", bm_metric[i][0],bm_metric[i][1],bm_metric[i][2],bm_metric[i][3],bm_metric[i][4],bm_metric[i][5]);
	}

	fclose(f_b_m);
}

void defined_tensor::smmoth_metric_to_interior(VolumeMesh* mesh_)
{
	if(ref_mesh_) delete ref_mesh_;
	ref_mesh_ = mesh_;

	if(bm) delete bm;
	bm = new SurfaceMesh();
	bm->request_face_status();
	bm->request_vertex_status();
	bm->request_edge_status();
	bm->request_face_normals();
	bm->request_vertex_normals();

	construct_boundary_mesh();
	//printf("1\n");

	std::vector<double> k1, k2; std::vector<OpenMesh::Vec3d> d1, d2;
	compute_principal_curvature(bm,k1,k2,d1,d2); bm_metric.resize(k1.size());
	tet_vertex_metric.clear(); unsigned tet_nv = mesh_->n_vertices(); tet_vertex_metric.resize(tet_nv);
	std::vector<int> tet_vertex_flag(tet_nv, -1); std::vector<int> new_one_ring_vertex(tet_nv, -1); int new_one_ring_vertex_count = 0;
	Eigen::Matrix3d R, D, M; D.setZero();
	//printf("2 %d %d %d\n", k1.size(), bm->n_vertices(), tet_nv);
	for(unsigned i=0;i<k1.size();++i)
	{
		SurfaceMesh::VertexHandle vh = bm->vertex_handle(i);
		int tet_v_id = bm->data(vh).get_tet_vertex_id();
		//printf("%d\n", tet_v_id);
		//use curvature to construct metric
		OpenMesh::Vec3d n = OpenMesh::cross(d1[i], d2[i]); n.normalize();
		R(0,0) = d1[i][0]; R(1,0) = d1[i][1]; R(2,0) = d1[i][2];
		R(0,1) = d2[i][0]; R(1,1) = d2[i][1]; R(2,1) = d2[i][2];
		R(0,2) =     n[0]; R(1,2) =     n[1]; R(2,2) =     n[2];
		D(0,0) = std::abs(k1[i]) < 1.0e-4 ? 1.0e-4 : std::abs(k1[i]);
		D(1,1) = std::abs(k2[i]) < 1.0e-4 ? 1.0e-4 : std::abs(k2[i]);
		double max_k = std::abs(k1[i]) > std::abs(k2[i]) ? std::abs(k1[i]) : std::abs(k2[i]);
		D(2,2) = max_k < 1e-4 ? 1e-4 : max_k;
		//D(2,2) = 0.0;
		M = R*D*R.transpose();
		tet_vertex_metric[tet_v_id] = OpenVolumeMesh::Geometry::Vec6d(M(0,0), M(0,1), M(0,2), M(1,1), M(1,2), M(2,2));
		bm_metric[i] = tet_vertex_metric[tet_v_id];
		tet_vertex_flag[tet_v_id] = 1; OpenVolumeMesh::VertexHandle tet_vh(tet_v_id);
		for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
		{
			OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
			OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
			OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
			if(f_vh != tet_vh && !mesh_->is_boundary(f_vh))
			{
				new_one_ring_vertex[f_vh.idx()] = 1; ++new_one_ring_vertex_count;
			}
			else if(t_vh != tet_vh && !mesh_->is_boundary(t_vh))
			{
				new_one_ring_vertex[t_vh.idx()] = 1; ++new_one_ring_vertex_count;
			}
		}
	}

	//printf("3,1\n");

	save_boundary_mesh_metric();

	//printf("3,2\n");

	//init value
	while(new_one_ring_vertex_count > 0)
	{
		std::vector<int> old_v; old_v.reserve(new_one_ring_vertex_count);
		for(unsigned i=0;i<new_one_ring_vertex.size();++i)
		{
			if(new_one_ring_vertex[i] == 1)
			{
				old_v.push_back(i); OpenVolumeMesh::VertexHandle tet_vh(i);
				OpenVolumeMesh::Geometry::Vec6d ave_M(0,0,0,0,0,0); double sum_count = 0.0;
				for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
				{
					OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
					OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
					OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
					if(f_vh != tet_vh)
					{
						if(tet_vertex_flag[f_vh.idx()] == 1)
						{
							ave_M += tet_vertex_metric[f_vh.idx()]; sum_count += 1.0;
						}
					}
					else if(t_vh != tet_vh)
					{
						if(tet_vertex_flag[t_vh.idx()] == 1)
						{
							ave_M += tet_vertex_metric[t_vh.idx()]; sum_count += 1.0;
						}
					}
				}

				ave_M /= sum_count;
				tet_vertex_metric[i] = ave_M;
			}
		}

		for(unsigned i=0;i<old_v.size();++i)
		{
			tet_vertex_flag[old_v[i]] = 1; new_one_ring_vertex[old_v[i]] = -1;
		}

		new_one_ring_vertex_count = 0;
		for(unsigned i=0;i<old_v.size();++i)
		{
			OpenVolumeMesh::VertexHandle tet_vh(old_v[i]);
			for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
			{
				OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
				OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
				OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
				if(f_vh != tet_vh && tet_vertex_flag[f_vh.idx()] == -1)
				{
					new_one_ring_vertex[f_vh.idx()] = 1; ++new_one_ring_vertex_count;
				}
				else if(t_vh != tet_vh && tet_vertex_flag[t_vh.idx()] == -1)
				{
					new_one_ring_vertex[t_vh.idx()] = 1; ++new_one_ring_vertex_count;
				}
			}
		}
	}

	//printf("4\n");
	//smooth
	for(unsigned i=0;i<50;++i)
	{
		printf("%d ", i);
		for(OpenVolumeMesh::VertexIter v_it = mesh_->vertices_begin(); v_it != mesh_->vertices_end(); ++v_it)
		{
			if(mesh_->is_boundary(*v_it)) continue;
			OpenVolumeMesh::Geometry::Vec6d ave_M(0,0,0,0,0,0); double sum_count = 0.0;
			for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(*v_it); voh_it; ++voh_it)
			{
				OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
				OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
				OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
				if( f_vh != *v_it )
				{
					ave_M += tet_vertex_metric[f_vh.idx()]; sum_count += 1.0;
				}
				else if( t_vh != *v_it )
				{
					ave_M += tet_vertex_metric[t_vh.idx()]; sum_count += 1.0;
				}
			}
			ave_M /= sum_count;
			tet_vertex_metric[v_it->idx()] = ave_M;
		}
	}
	printf("\n");
}

void defined_tensor::save_tet_vertex_metric(const char* filename)
{
	FILE* f_v_m = fopen(filename, "w");

	fprintf(f_v_m, "%d", tet_vertex_metric.size());
	for(unsigned i=0;i<tet_vertex_metric.size();++i)
	{
		fprintf(f_v_m, "\n%20.19f %20.19f %20.19f %20.19f %20.19f %20.19f", tet_vertex_metric[i][0],tet_vertex_metric[i][1],tet_vertex_metric[i][2],tet_vertex_metric[i][3],tet_vertex_metric[i][4],tet_vertex_metric[i][5] );
	}

	fclose(f_v_m);
}

void defined_tensor::load_tet_vertex_metric(const char* filename)
{
	FILE* f_v_m = fopen(filename, "r");
	int nv = 0;
	char buf[4096];  fgets(buf, 4096, f_v_m);
	sscanf(buf, "%d", &nv); tet_vertex_metric.clear(); tet_vertex_metric.resize(nv); int v_count = 0;
	char h0[128]; char h1[128]; char h2[128]; char h4[128]; char h5[128]; char h8[128];
	while ( !feof(f_v_m) )
	{
		fgets(buf, 4096, f_v_m);
		sscanf(buf, "%s %s %s %s %s %s", h0, h1, h2, h4, h5, h8);
		tet_vertex_metric[v_count] = OpenVolumeMesh::Geometry::Vec6d(atof(h0), atof(h1),atof(h2), atof(h4), atof(h5), atof(h8));
		++v_count;
	}

	fclose(f_v_m);
}

void defined_tensor::load_ref_tet_mesh(VolumeMesh* mesh_)
{
	if(tet_vertex_metric.size() == 0)
	{
		printf("Please Load Metric First!!!\n");
		return;
	}
	if(ref_mesh_) delete ref_mesh_;
	ref_mesh_ = mesh_;
	if(bm) delete bm;
	bm = new SurfaceMesh();
	construct_boundary_mesh();
	bm_metric.resize(bm->n_vertices());
	for(unsigned i=0;i<bm->n_vertices(); ++i)
	{
		SurfaceMesh::VertexHandle vh = bm->vertex_handle(i);
		int tet_v_id = bm->data(vh).get_tet_vertex_id();
		bm_metric[i] = tet_vertex_metric[tet_v_id];
	}
	build_AABB_Tree_using_ref_mesh(bm);
	build_ANN_KD_tree();
}

void defined_tensor::build_ANN_KD_tree()
{
	int nc = ref_mesh_->n_cells();
	ANNpointArray dataPts = annAllocPts(nc, 3);
	for(OpenVolumeMesh::CellIter c_it = ref_mesh_->cells_begin(); c_it != ref_mesh_->cells_end();++c_it)
	{
		OpenVolumeMesh::Geometry::Vec3d c_c(0,0,0); double count = 0.0;
		for(OpenVolumeMesh::CellVertexIter cv_it = ref_mesh_->cv_iter(*c_it); cv_it; ++cv_it )
		{
			c_c += ref_mesh_->vertex(*cv_it); count += 1.0;
		}
		c_c /= count;
		int c_id = c_it->idx();
		dataPts[c_id][0] = c_c[0]; dataPts[c_id][1] = c_c[1]; dataPts[c_id][2] = c_c[2];
	}
	if(kdTree) delete kdTree;
	kdTree = new ANNkd_tree(dataPts, nc, 3);
}

bool defined_tensor::check_in_tet(const OpenVolumeMesh::Geometry::Vec3d& p, const int& cel_id)
{
	if(cel_id < 0) return false;

	OpenVolumeMesh::CellHandle ch(cel_id);
	OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
	std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces();
	for(unsigned i=0;i<hfh_vec.size();++i)
	{
		OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter(hfh_vec[i]);
		OpenVolumeMesh::Geometry::Vec3d p0 = ref_mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = ref_mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = ref_mesh_->vertex(*hfv_it);

		double v = OpenVolumeMesh::Geometry::dot(p-p0, OpenVolumeMesh::Geometry::cross(p1-p0,p2-p0));
		if(v < 0) return false;
	}
	return true;
}

int defined_tensor::find_nearest_cell_id(const OpenVolumeMesh::Geometry::Vec3d& p)
{
	ANNpoint tp = annAllocPt(3); tp[0] = p[0]; tp[1] = p[1]; tp[2] = p[2];
	ANNidxArray nnIdx = new ANNidx[1]; ANNdistArray dists = new ANNdist[1];
	kdTree->annkSearch(tp, 1, nnIdx, dists);
	int cell_id = nnIdx[0];
	delete [] nnIdx;

	std::vector<int> visited_cell_id(ref_mesh_->n_cells(), -1);
	visited_cell_id[cell_id] = 1;
	std::queue<int> Q; Q.push(cell_id);
	while (Q.size() > 0)
	{
		cell_id = Q.front(); Q.pop();
		if( check_in_tet(p, cell_id) ) return cell_id;
		OpenVolumeMesh::CellHandle ch(cell_id);
		OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
		std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces();
		for(unsigned i=0;i<hfh_vec.size();++i)
		{
			OpenVolumeMesh::HalfFaceHandle hfh = ref_mesh_->opposite_halfface_handle(hfh_vec[i]);
			OpenVolumeMesh::CellHandle t_ch = ref_mesh_->incident_cell(hfh);
			if(t_ch != VolumeMesh::InvalidCellHandle && visited_cell_id[t_ch.idx()] == -1)
			{
				Q.push( t_ch.idx() ); visited_cell_id[t_ch.idx()] = 1;
			}
		}
	}

	return -1;
}

void defined_tensor::find_cell_bay_cor(const OpenVolumeMesh::Geometry::Vec3d& p,const int& cell_id,
									   OpenVolumeMesh::Geometry::Vec4i& v_id, OpenVolumeMesh::Geometry::Vec4d& b)
{
	OpenVolumeMesh::CellHandle ch(cell_id);
	OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
	std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces(); double sum_b= 0.0;
	for(unsigned i=0;i<hfh_vec.size();++i)
	{
		OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter(hfh_vec[i]);
		OpenVolumeMesh::Geometry::Vec3d p0 = ref_mesh_->vertex(*hfv_it); int v0 = hfv_it->idx();
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = ref_mesh_->vertex(*hfv_it); int v1 = hfv_it->idx();
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = ref_mesh_->vertex(*hfv_it); int v2 = hfv_it->idx();

		b[i] = OpenVolumeMesh::Geometry::dot(p - p0, OpenVolumeMesh::Geometry::cross(p1-p0,p2-p0));
		sum_b += b[i];
		unsigned j = (i+1) % hfh_vec.size();
		for( hfv_it = ref_mesh_->hfv_iter(hfh_vec[j]); hfv_it; ++hfv_it)
		{
			if(hfv_it->idx() != v0 &&hfv_it->idx() != v1 &&hfv_it->idx() != v2 )
			{
				v_id[i] = hfv_it->idx();
				break;
			}
		}
	}
	b /= sum_b;
}

void defined_tensor::project_for_interior(OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M)
{
	//printf("...........\n");
	int cell_id = find_nearest_cell_id(p);
	if(cell_id < 0) printf("%d\n", cell_id);

	OpenVolumeMesh::Geometry::Vec4i v_id; OpenVolumeMesh::Geometry::Vec4d b;
	find_cell_bay_cor(p, cell_id, v_id, b);
	M = tet_vertex_metric[v_id[0]] * b[0] + tet_vertex_metric[v_id[1]] * b[1] + tet_vertex_metric[v_id[2]] * b[2] + tet_vertex_metric[v_id[3]] * b[3];
}

max_stars_repo_name: msraig/CE-PolyCube

max_issues_repo_name: xh-liu-tech/CE-PolyCube

max_forks_repo_name: xh-liu-tech/CE-PolyCube


text:
/**

Copyright (c) 2016, Aumann Florian, Heller Florian, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "VisualisationUtils/tfvisualizer.h"
#ifndef Q_MOC_RUN
#include <boost/foreach.hpp>
#include <cv_bridge/cv_bridge.h>
#endif

TF_Visualizer::TF_Visualizer(const std::vector<std::string>& frame_ids, std::string camera_input_topic, std::string camera_output_topic)
: it_(nh_), frame_ids_(frame_ids), camera_input_topic_(camera_input_topic), camera_output_topic_(camera_output_topic)
{
}

void TF_Visualizer::imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
{
    cv::Mat image;
    cv_bridge::CvImagePtr input_bridge;
    try {
      input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
      image = input_bridge->image;
    }
    catch (cv_bridge::Exception& ex){
      ROS_ERROR("[draw_frames] Failed to convert image");
      return;
    }

    cam_model_.fromCameraInfo(info_msg);

    BOOST_FOREACH(const std::string& frame_id, frame_ids_) {
      tf::StampedTransform transform;
      try {
        ros::Time acquisition_time = info_msg->header.stamp;
        ros::Duration timeout(1.0 / 30);
        tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id,
                                      acquisition_time, timeout);
        tf_listener_.lookupTransform(cam_model_.tfFrame(), frame_id,
                                     acquisition_time, transform);
      }
      catch (tf::TransformException& ex) {
        ROS_WARN("[draw_frames] TF exception:\n%s", ex.what());
        return;
      }

      tf::Point pt = transform.getOrigin();
      cv::Point3d pt_cv(pt.x(), pt.y(), pt.z());
      cv::Point2d uv;
      uv = cam_model_.project3dToPixel(pt_cv);

      static const int RADIUS = 3;
      cv::circle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
      CvSize text_size;
      int baseline;
      cvGetTextSize(frame_id.c_str(), &font_, &text_size, &baseline);
      CvPoint origin = cvPoint(uv.x - text_size.width / 2,
                               uv.y - RADIUS - baseline - 3);
      cv::putText(image, frame_id.c_str(), origin, cv::FONT_HERSHEY_SIMPLEX, 12, CV_RGB(255,0,0));
    }

    pub_.publish(input_bridge->toImageMsg());
}

void TF_Visualizer::run()
{
    std::string image_topic = nh_.resolveName(camera_input_topic_);
    sub_ = it_.subscribeCamera(image_topic, 1, &TF_Visualizer::imageCb, this);
    pub_ = it_.advertise(camera_output_topic_, 1);
    cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
}

void TF_Visualizer::stop()
{
    //sub_ = null;
    //pub_ = null;
}

ext: cpp


text:
/*=============================================================================
    Copyright (c) 2013-2014 Damien Buhl

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#ifndef BOOST_FUSION_ADAPTED_STRUCT_DETAIL_ADAPT_BASE_ASSOC_ATTR_FILLER_HPP
#define BOOST_FUSION_ADAPTED_STRUCT_DETAIL_ADAPT_BASE_ASSOC_ATTR_FILLER_HPP

#include <boost/config.hpp>

#include <boost/fusion/adapted/struct/detail/adapt_base_attr_filler.hpp>

#include <boost/preprocessor/control/if.hpp>
#include <boost/preprocessor/arithmetic/sub.hpp>
#include <boost/preprocessor/variadic/size.hpp>
#include <boost/preprocessor/empty.hpp>
#include <boost/preprocessor/facilities/is_empty.hpp>

#if BOOST_PP_VARIADICS

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_0(...)                           \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(__VA_ARGS__)                      \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_1

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_1(...)                           \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(__VA_ARGS__)                      \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_0

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(...)                          \
      ((BOOST_PP_VARIADIC_SIZE(__VA_ARGS__), (__VA_ARGS__)))

#else // BOOST_PP_VARIADICS

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_0(X, Y, Z)                       \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(X, Y, Z)                          \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_1

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_1(X, Y, Z)                       \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(X, Y, Z)                          \
    BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_0

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAP_ATTR(X, Y, Z)                      \
    BOOST_PP_IF(BOOST_PP_IS_EMPTY(X),                                           \
      ((2, (Y,Z))),                                                             \
      ((3, (X,Y,Z)))                                                            \
    )

#endif // BOOST_PP_VARIADICS

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_0_END
#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_FILLER_1_END

#define BOOST_FUSION_ADAPT_ASSOC_STRUCT_WRAPPEDATTR_GET_KEY(ATTRIBUTE)          \
    BOOST_PP_TUPLE_ELEM(                                                        \
        BOOST_FUSION_ADAPT_STRUCT_WRAPPEDATTR_SIZE(ATTRIBUTE),                  \
        BOOST_PP_SUB(BOOST_FUSION_ADAPT_STRUCT_WRAPPEDATTR_SIZE(ATTRIBUTE), 1), \
        BOOST_FUSION_ADAPT_STRUCT_WRAPPEDATTR(ATTRIBUTE))

#endif

ext: hpp

max_stars_repo_name: LazyPlanet/MX-Client

max_issues_repo_name: LazyPlanet/MX-Client

max_forks_repo_name: LazyPlanet/MX-Client


text:
/*
Copyright (c) 2019 - 2022 Advanced Micro Devices, Inc. All rights reserved.

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 <cassert>
#include <commons.h>
#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <algorithm>
#include <memory.h>
#include <stdint.h>
#include "mxnet_recordio_reader.h"
#include <boost/filesystem.hpp>

namespace filesys = boost::filesystem;

using namespace std;

MXNetRecordIOReader::MXNetRecordIOReader():
_shuffle_time("shuffle_time", DBG_TIMING)
{
    _src_dir = nullptr;
    _entity = nullptr;
    _curr_file_idx = 0;
    _current_file_size = 0;
    _loop = false;
    _shuffle = false;
    _file_id = 0;
    _file_count_all_shards = 0;
}

unsigned MXNetRecordIOReader::count_items()
{
    if (_loop)
        return _file_names.size();

    int ret = ((int)_file_names.size() - _read_counter);
    return ((ret < 0) ? 0 : ret);
}

Reader::Status MXNetRecordIOReader::initialize(ReaderConfig desc)
{
    auto ret = Reader::Status::OK;
    _file_id = 0;
    _path = desc.path();
    _shard_id = desc.get_shard_id();
    _shard_count = desc.get_shard_count();
    _batch_count = desc.get_batch_size();
    _loop = desc.loop();
    _shuffle = desc.shuffle();
    ret = record_reading();
    // the following code is required to make every shard the same size:: required for multi-gpu training
    if (_shard_count > 1 && _batch_count > 1) {
        int _num_batches = _file_names.size()/_batch_count;
        int max_batches_per_shard = (_file_count_all_shards + _shard_count-1)/_shard_count;
        max_batches_per_shard = (max_batches_per_shard + _batch_count-1)/_batch_count;
        if (_num_batches < max_batches_per_shard) {
            replicate_last_batch_to_pad_partial_shard();
        }
    }
    //shuffle dataset if set
    _shuffle_time.start();
    if( ret==Reader::Status::OK && _shuffle)
        std::random_shuffle(_file_names.begin(), _file_names.end());
    _shuffle_time.end();

    return ret;

}

void MXNetRecordIOReader::incremenet_read_ptr()
{
    _read_counter++;
    _curr_file_idx = (_curr_file_idx + 1) % _file_names.size();
}
size_t MXNetRecordIOReader::open()
{
    auto file_path = _file_names[_curr_file_idx]; // Get next file name
    _last_id = file_path;
    auto it = _record_properties.find(_file_names[_curr_file_idx]);
    std::tie(_current_file_size, _seek_pos, _data_size_to_read) = it->second;
    return _current_file_size;
}

size_t MXNetRecordIOReader::read_data(unsigned char *buf, size_t read_size)
{
    auto it = _record_properties.find(_file_names[_curr_file_idx]);
    std::tie(_current_file_size, _seek_pos, _data_size_to_read) = it->second;
    read_image(buf, _seek_pos, _data_size_to_read);
    incremenet_read_ptr();
    return read_size;
}

int MXNetRecordIOReader::close()
{
    return release();
}

MXNetRecordIOReader::~MXNetRecordIOReader()
{
}

int MXNetRecordIOReader::release()
{
    return 0;
}

void MXNetRecordIOReader::reset()
{
    _shuffle_time.start();
    if (_shuffle)
        std::random_shuffle(_file_names.begin(), _file_names.end());
    _shuffle_time.end();
    _read_counter = 0;
    _curr_file_idx = 0;
}

Reader::Status MXNetRecordIOReader::record_reading()
{
    auto ret = Reader::Status::OK;
    if (MXNet_reader() != Reader::Status::OK)
        WRN("MXNetRecordIOReader ShardID [" + TOSTR(_shard_id) + "] MXNetRecordIOReader cannot access the storage at " + _path);

    if (_in_batch_read_count > 0 && _in_batch_read_count < _batch_count)
    {
        replicate_last_image_to_fill_last_shard();
        LOG("MXNetRecordIOReader ShardID [" << TOSTR(_shard_id) << "] Replicated " << _path + _last_file_name << " " << TOSTR((_batch_count - _in_batch_read_count)) << " times to fill the last batch")
    }
    if (!_file_names.empty())
        LOG("MXNetRecordIOReader ShardID [" << TOSTR(_shard_id) << "] Total of " << TOSTR(_file_names.size()) << " images loaded from " << _path)
    return ret;
}

void MXNetRecordIOReader::replicate_last_image_to_fill_last_shard()
{
    for (size_t i = _in_batch_read_count; i < _batch_count; i++)
    {
        _file_names.push_back(_last_file_name);
        _record_properties.insert(pair<std::string, std::tuple<unsigned int, int64_t, int64_t>>(_last_file_name, std::make_tuple(_last_file_size, _last_seek_pos, _last_data_size)));
    }
}

void MXNetRecordIOReader::replicate_last_batch_to_pad_partial_shard()
{
    if (_file_names.size() >=  _batch_count) {
        for (size_t i = 0; i < _batch_count; i++)
        {
            _file_names.push_back(_file_names[i - _batch_count]);
            auto it = _record_properties.find(_file_names[i - _batch_count]);
            std::tie(_current_file_size, _seek_pos, _data_size_to_read) = it->second;
            _record_properties.insert(pair<std::string, std::tuple<unsigned int, int64_t, int64_t>>(_file_names[i - _batch_count], std::make_tuple(_current_file_size, _seek_pos, _data_size_to_read)));
        }
    }
}

Reader::Status MXNetRecordIOReader::MXNet_reader()
{
    std::string _rec_file, _idx_file;
    if ((_src_dir = opendir (_path.c_str())) == nullptr)
        THROW("MXNetReader ShardID ["+ TOSTR(_shard_id)+ "] ERROR: Failed opening the directory at " + _path);

    while((_entity = readdir (_src_dir)) != nullptr)
    {
        std::string file_name = _path + "/" + _entity->d_name;
        filesys::path pathObj(file_name);
        if(filesys::exists(pathObj) && filesys::is_regular_file(pathObj))
        {
            auto file_extension_idx = file_name.find_last_of(".");
            if (file_extension_idx  != std::string::npos)
            {
                std::string file_extension = file_name.substr(file_extension_idx+1);
                if (file_extension == "rec")
                    _rec_file = file_name;
                else if(file_extension == "idx")
                    _idx_file = file_name;
                else
                    continue;
            }
        }
    }
    closedir(_src_dir);
    uint rec_size;
    _file_contents.open(_rec_file, ios::binary);
    if (!_file_contents)
        THROW("MXNetRecordIOReader ERROR: Failed opening the file " + _rec_file);
    _file_contents.seekg(0, ifstream::end);
    rec_size = _file_contents.tellg();
    _file_contents.seekg(0, ifstream::beg);

    ifstream index_file(_idx_file);
    if(!index_file)
        THROW("MXNetRecordIOReader ERROR: Could not open RecordIO index file. Provided path: " + _idx_file);

    std::vector<size_t> _index_list;
    size_t _index, _offset;
    while (index_file >> _index >> _offset)
        _index_list.push_back(_offset);
    if(_index_list.empty())
        THROW("MXNetRecordIOReader ERROR: RecordIO index file doesn't contain any indices. Provided path: " + _idx_file);
    _index_list.push_back(rec_size);
    std::sort(_index_list.begin(), _index_list.end());
    for (size_t i = 0; i < _index_list.size() - 1; ++i)
        _indices.emplace_back(_index_list[i], _index_list[i + 1] - _index_list[i]);
    read_image_names();
    return Reader::Status::OK;
}

size_t MXNetRecordIOReader::get_file_shard_id()
{
    if (_batch_count == 0 || _shard_count == 0)
        THROW("Shard (Batch) size cannot be set to 0")
    return _file_id  % _shard_count;
}

void MXNetRecordIOReader::read_image_names()
{
    for(int current_index = 0; current_index < (int)_indices.size(); current_index++ )
    {
        uint32_t _magic, _length_flag;
        std::tie(_seek_pos, _data_size_to_read) = _indices[current_index];
        _file_contents.seekg(_seek_pos, ifstream::beg);
        uint8_t* _data = new uint8_t[_data_size_to_read];
        uint8_t* _data_ptr = _data;
        auto ret = _file_contents.read((char *)_data_ptr, _data_size_to_read).gcount();
        if(ret == -1 || ret != _data_size_to_read)
            THROW("MXNetRecordIOReader ERROR:  Unable to read the data from the file ");

        _magic = *((uint32_t *) _data_ptr);
        _data_ptr += sizeof(_magic);
        if(_magic != _kMagic)
            THROW("MXNetRecordIOReader ERROR: Invalid MXNet RecordIO: wrong _magic number");
        _length_flag = *((uint32_t *) _data_ptr);
        _data_ptr += sizeof(_length_flag);
        uint32_t _clength =  DecodeLength(_length_flag);
        _hdr = *((ImageRecordIOHeader *) _data_ptr);

        if (_hdr.flag == 0)
            _image_key = to_string(_hdr.image_id[0]);
        else
        {
            WRN("\nMXNetRecordIOReader Multiple record reading has not supported");
            continue;
        }
        /* _clength - sizeof(ImageRecordIOHeader) to get the data size.
        Subtracting label size(_hdr.flag * sizeof(float)) from data size to get image size*/
        int64_t image_size = (_clength - sizeof(ImageRecordIOHeader)) - (_hdr.flag * sizeof(float));
        delete[] _data;

        if (get_file_shard_id() != _shard_id)
        {
            incremenet_file_id();
            _file_count_all_shards++;
            continue;
        }
        _in_batch_read_count++;
        _in_batch_read_count = (_in_batch_read_count % _batch_count == 0) ? 0 : _in_batch_read_count;

        _file_names.push_back(_image_key.c_str());
        _last_file_name = _image_key.c_str();

        incremenet_file_id();
        _file_count_all_shards++;

        _last_file_size = image_size;
        _last_seek_pos = _seek_pos;
        _last_data_size = _data_size_to_read;
        //_record_properties vector used to keep track of image size, seek position and data size of the single
        _record_properties.insert(pair<std::string, std::tuple<unsigned int, int64_t, int64_t>>(_last_file_name, std::make_tuple(_last_file_size, _last_seek_pos, _last_data_size)));
    }
}

void MXNetRecordIOReader::read_image(unsigned char *buff, int64_t seek_position, int64_t _data_size_to_read)
{
    uint32_t _magic, _length_flag;
    _file_contents.seekg(seek_position, ifstream::beg);
    uint8_t* _data = new uint8_t[_data_size_to_read];
    uint8_t* _data_ptr = _data;
    auto ret = _file_contents.read((char *)_data_ptr, _data_size_to_read).gcount();
    if(ret == -1 || ret != _data_size_to_read)
        THROW("MXNetRecordIOReader ERROR:  Unable to read the data from the file ");
    _magic = *((uint32_t *) _data_ptr);
    _data_ptr += sizeof(_magic);
    if(_magic != _kMagic)
        THROW("MXNetRecordIOReader ERROR: Invalid RecordIO: wrong _magic number");
    _length_flag = *((uint32_t *) _data_ptr);
    _data_ptr += sizeof(_length_flag);
    uint32_t _cflag = DecodeFlag(_length_flag);
    uint32_t _clength =  DecodeLength(_length_flag);
    _hdr = *((ImageRecordIOHeader *) _data_ptr);
    _data_ptr += sizeof(_hdr);

    int64_t data_size = _clength - sizeof(ImageRecordIOHeader);
    int64_t label_size = _hdr.flag * sizeof(float);
    int64_t image_size = data_size - label_size;
    if (_cflag == 0)
        memcpy(buff, _data_ptr + label_size, image_size);
    else
        THROW("\nMultiple record reading has not supported");
    delete[] _data;
}

max_stars_repo_name: acxz/MIVisionX

max_issues_repo_name: acxz/MIVisionX

max_forks_repo_name: acxz/MIVisionX


text:
/*This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

Copyright (c) 2018 Dmitry Vinokurov */

#ifndef _METRIC_CROSSFILTER_DETAILS_FEATURE_HPP
#define _METRIC_CROSSFILTER_DETAILS_FEATURE_HPP
#include <vector>
#include <functional>
#include <unordered_map>
#include <utility>
#include <type_traits>
//#include <boost/signals2.hpp>
#include "../detail/feature_impl.hpp"
#include "../detail/crossfilter_impl.hpp"
#include "../detail/dimension_impl.hpp"
#include "../detail/thread_policy.hpp"

namespace cross {
template <typename Key, typename Reduce, typename Dimension, bool isGroupAll = false>
struct feature : private impl::feature_impl<Key, Reduce, Dimension, isGroupAll> {
    using feature_impl_t = impl::feature_impl<Key, Reduce, Dimension, isGroupAll>;

public:
    using group_type_t = typename feature_impl_t::group_type_t;
    using reduce_type_t = typename feature_impl_t::reduce_type_t;
    using record_type_t = typename feature_impl_t::record_type_t;
    using value_type_t = typename feature_impl_t::value_type_t;

    using this_type_t = feature<Key, Reduce, Dimension, isGroupAll>;

    using reader_lock_t = cross::thread_policy::read_lock_t;
    using writer_lock_t = cross::thread_policy::write_lock_t;

private:
    template <typename T, typename H>
    friend struct impl::filter_impl;
    template <typename, typename, typename, typename>
    friend struct impl::dimension_impl;
    template <typename KeyFunc, typename AddFunc, typename RemoveFunc, typename InitialFunc, typename Base>
    feature(Base* dim, KeyFunc key_, AddFunc add_func_, RemoveFunc remove_func_, InitialFunc initial_func_)
        : feature_impl_t(dim, key_, add_func_, remove_func_, initial_func_)
    {
    }

public:
    void dispose() { feature_impl_t::dispose(); }

    feature(feature<Key, Reduce, Dimension, isGroupAll>&& g)
        : feature_impl_t(std::move(g))
    {
    }

    /**
     Returns a new array containing the top k groups,
     according to the group order of the associated reduce value. The returned array is in descending order by reduce value.
   */
    std::vector<group_type_t> top(std::size_t k) noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        return feature_impl_t::top(k);
    }

    /**
     Returns a new array containing the top k groups,
     according to the order defined by orderFunc of the associated reduce value.
   */
    template <typename OrderFunc>
    std::vector<group_type_t> top(std::size_t k, OrderFunc orderFunc) noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        return feature_impl_t::top(k, orderFunc);
    }

    /**
     Returns the array of all groups, in ascending natural order by key.
   */
    std::vector<group_type_t>& all() noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        return feature_impl_t::all();
    }

    /**
     Equivalent to all()[0].second.
   */
    reduce_type_t value() noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        return feature_impl_t::value();
    }

    /**
     Specifies the order value for computing the top-K groups.
     The default order is the identity function,
     which assumes that the reduction values are naturally-ordered (such as simple counts or sums).
   */
    template <typename OrderFunc>
    this_type_t& order(OrderFunc value) noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        feature_impl_t::order(value);
        return *this;
    }

    /**
     A convenience method for using natural order for reduce values. Returns this grouping.
   */
    this_type_t& order_natural() noexcept
    {
        writer_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        feature_impl_t::order_natural();
        return *this;
    }
    /**
     Returns the number of distinct values in the group, independent of any filters; the cardinality.
   */
    std::size_t size() const noexcept
    {
        reader_lock_t lk(feature_impl_t::dimension->lock());
        (void)lk;  // avoid AppleClang warning about unused variable;
        return feature_impl_t::size();
    }
};
}  //namespace cross
#endif


text:
// Copyright (c) 2020 Vitaly Chipounov
//
// 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 <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <curl/curl.h>
#include <iostream>
#include <math.h>
#include <mutex>

#include <gpsmap/tilemanager.h>

namespace pt = boost::property_tree;

// https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames#C.2FC.2B.2B
template <typename T> static T long2tilex(double lon, int z) {
    return (T)(((lon + 180.0) / 360.0 * (double) (1 << z)));
}

template <typename T> static T lat2tiley(double lat, int z) {
    double latrad = lat * M_PI / 180.0;
    return (T)(((1.0 - asinh(tan(latrad)) / M_PI) / 2.0 * (double) (1 << z)));
}

static bool DownloadFile(const std::string &url, const std::string &filePath) {
    static bool inited = false;
    CURL *curl = nullptr;
    CURLcode res = CURLE_OK;
    bool ret = false;
    FILE *fp = nullptr;

    if (!inited) {
        curl_global_init(CURL_GLOBAL_DEFAULT);
        inited = true;
    }

    std::cout << "Downloading " << url << std::endl;

    curl = curl_easy_init();
    if (!curl) {
        std::cerr << "Could not init curl" << std::endl;
        goto err;
    }

    fp = fopen(filePath.c_str(), "wb");
    if (!fp) {
        std::cerr << "Could not open " << filePath << std::endl;
        goto err;
    }

    curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
    curl_easy_setopt(curl, CURLOPT_WRITEDATA, fp);

    res = curl_easy_perform(curl);
    if (res != CURLE_OK) {
        std::cerr << "Could not download " << url << std::endl;
        goto err;
    }

    ret = true;
err:
    if (curl) {
        curl_easy_cleanup(curl);
    }

    if (fp) {
        fclose(fp);
    }

    return ret;
}

namespace gpsmap {

Tile::Tile(const std::string &filePath, const TileDesc &desc) : m_filePath(filePath), m_desc(desc), m_state(LOADED) {
    Load(filePath);
}

Tile::Tile(const TileDesc &desc) : m_desc(desc), m_state(LOADING) {
}

void Tile::Load(const std::string &filePath) {
    {
        std::lock_guard<std::mutex> lk(m_lock);
        if (m_state != LOADING) {
            return;
        }

        m_filePath = filePath;
        m_image = OIIO::ImageBuf(m_filePath);

        if (m_image.read()) {
            int channelorder[] = {0, 1, 2, -1 /*use a float value*/};
            float channelvalues[] = {0 /*ignore*/, 0 /*ignore*/, 0 /*ignore*/, 1.0};
            std::string channelnames[] = {"", "", "", "A"};
            m_image = OIIO::ImageBufAlgo::channels(m_image, 4, channelorder, channelvalues, channelnames);
            m_state = LOADED;
        } else {
            std::cerr << "Could not load " << m_filePath << "\n";
            m_state = FAILED;
        }
    }
    m_cv.notify_all();
}

TilePtr Tile::Create(const std::string &filePath, const TileDesc &desc) {
    return TilePtr(new Tile(filePath, desc));
}

TilePtr Tile::Create(const TileDesc &desc) {
    return TilePtr(new Tile(desc));
}

bool Tile::WaitUntilDownloaded() {
    std::unique_lock<std::mutex> lk(m_lock);
    if (m_state != LOADING) {
        return m_state == LOADED;
    }

    m_cv.wait(lk, [&] { return m_state != LOADING; });
    return m_state == LOADED;
}

void Tile::Fail() {
    {
        std::lock_guard<std::mutex> lk(m_lock);
        m_state = FAILED;
    }
    m_cv.notify_all();
}

TileManagerPtr TileManager::Create(const std::string &tilesRootPath, const std::string &mapDescPath) {
    if (!boost::filesystem::is_directory(tilesRootPath)) {
        std::cerr << tilesRootPath << " does not exist or is not a directory." << std::endl;
        return nullptr;
    }

    pt::ptree tree;
    pt::read_xml(mapDescPath, tree);
    auto mapUrl = tree.get_child("map.url").get_value("");
    if (mapUrl.size() == 0) {
        std::cerr << "Could not get map url from " << mapDescPath << std::endl;
        return nullptr;
    }

    std::cout << "Using map URL " << mapUrl << std::endl;

    return TileManagerPtr(new TileManager(tilesRootPath, mapUrl));
}

bool TileManager::DownloadTile(TilePtr &tilep) const {
    auto url = m_mapUrl;
    auto tile = tilep->Desc();
    boost::replace_all(url, "$x", std::to_string(tile.x));
    boost::replace_all(url, "$y", std::to_string(tile.y));
    boost::replace_all(url, "$z", std::to_string(tile.z));

    std::stringstream fp;
    fp << m_tilesRootPath << "/" << tile.z << "/" << tile.x;

    auto dir = fp.str();
    if (!boost::filesystem::is_directory(dir)) {
        if (!boost::filesystem::create_directories(dir)) {
            std::cerr << "Could not create directory " << dir << std::endl;
            return false;
        }
    }

    fp << "/" << tile.y << ".png";
    auto filePath = fp.str();

    if (boost::filesystem::exists(filePath)) {
        if (boost::filesystem::file_size(filePath) > 0) {
            tilep->Load(filePath);
            return true;
        } else {
            boost::filesystem::remove(filePath);
        }
    }

    if (!DownloadFile(url, filePath)) {
        boost::filesystem::remove(filePath);
        return false;
    }

    tilep->Load(fp.str());
    return !tilep->Failed();
}

TilePtr TileManager::GetTile(int x, int y, int zoom) {
    bool found = false;
    TilePtr ret;
    {
        std::unique_lock<std::mutex> lock(m_lock);

        auto d = TileDesc(x, y, zoom);
        auto it = m_tiles.find(d);

        if (it != m_tiles.end()) {
            ret = it->second;
            found = true;
        } else {
            ret = Tile::Create(d);
            m_tiles[d] = ret;
        }
    }

    if (found) {
        ret->WaitUntilDownloaded();
    } else {
        if (!DownloadTile(ret)) {
            std::cerr << "Could not download " << ret->FilePath() << "\n";
            ret->Fail();
            return nullptr;
        }
    }

    if (ret->Failed()) {
        return nullptr;
    }

    return ret;
}

void TileManager::GetTileCoords(double lat, double lon, int zoom, int &xt, int &yt, int &px, int &py) const {
    auto xd = long2tilex<double>(lon, zoom);
    auto yd = lat2tiley<double>(lat, zoom);

    // Get the pixel coordinates inside the tile
    double dummy;
    px = modf(xd, &dummy) * m_tileWidth;
    py = modf(yd, &dummy) * m_tileHeight;
    xt = xd;
    yt = yd;
}

TilePtr TileManager::GetTile(double lat, double lon, int &px, int &py, int zoom) {
    int x = 0, y = 0;
    GetTileCoords(lat, lon, zoom, x, y, px, py);

    TilePtr ret = GetTile(x, y, zoom);
    if (!ret) {
        return nullptr;
    }

    return ret;
}

} // namespace gpsmap

max_stars_repo_name: vitalych/gpsmap

max_issues_repo_name: vitalych/gpsmap

max_forks_repo_name: vitalych/gpsmap


text:
#include <boost/filesystem.hpp>

#include "parser.hpp"
#include "search.hpp"

namespace fs = boost::filesystem;

int main(int argc, char** argv) {
    po::options_description desc(
        "Usage: sas [options] search-string path [path...]");
    desc.add_options()                                                      //
        ("help,h", "Print help messages")                                   //
        ("debug", "Enable debugging output")                                //
        ("search-string", po::value<std::string>(), "The search string")    //
        ("paths", po::value<std::vector<std::string>>(), "Paths to search") //
        ("search-all-extensions,a",                                         //
         "By default only file with h, hpp, c or cpp are searched."         //
         " Use this flag to disable this behavior.")                        //
        ("expressions,e", "Match expressions")                              //
        ("declarations,d", "Match declaratons")                             //
        ("definitions,D",                                                   //
         "Only match declarations that are also definitions (implies -d)")  //
        ("recursive,r", "Read all files under each directory recursively");

    po::positional_options_description p;
    p.add("search-string", 1);
    p.add("paths", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv)
                  .options(desc)
                  .positional(p)
                  .run(),
              vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    // If no match-mode is specified, look for anything
    if (!vm.count("expressions") && !vm.count("declarations") &&
        !vm.count("definitions")) {
        vm.insert(
            std::make_pair("expressions", po::variable_value(true, true)));
        vm.insert(
            std::make_pair("declarations", po::variable_value(true, true)));
    }
    // Matching definitions implies matching declarations
    else if (!vm.count("declarations") && vm.count("definitions")) {
        vm.insert(
            std::make_pair("declarations", po::variable_value(true, true)));
    }

    po::notify(vm);

    const auto search_string = vm["search-string"].as<std::string>();
    auto term = parse_search_string(search_string, vm);

    auto paths = vm["paths"].as<std::vector<std::string>>();

    for (const auto& path : paths) {
        if (fs::is_directory(path)) {
            if (!vm.count("recursive")) {
                std::cerr << "sas: " << path << ": Is a directory" << std::endl;
                continue;
            } else {
                for (fs::recursive_directory_iterator iter(path), end;
                     iter != end; ++iter) {
                    print_matches(iter->path().string(), term, vm);
                }
            }
        } else {
            print_matches(path, term, vm);
        }
    }

    return 0;
}

max_stars_repo_name: ALSchwalm/lagrep

max_issues_repo_name: ALSchwalm/lagrep

max_forks_repo_name: ALSchwalm/lagrep


text:
/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Josh Faust */

/*
 * Test callback queue
 */

#include <gtest/gtest.h>
#include <ros/callback_queue.h>
#include <ros/console.h>
#include <ros/timer.h>

#include <boost/atomic.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/function.hpp>

using namespace ros;

class CountingCallback : public CallbackInterface
{
public:
  CountingCallback()
  : count(0)
  {}

  virtual CallResult call()
  {
    boost::mutex::scoped_lock lock(mutex);
    ++count;

    return Success;
  }

  boost::mutex mutex;
  size_t count;
};
typedef boost::shared_ptr<CountingCallback> CountingCallbackPtr;

TEST(CallbackQueue, singleCallback)
{
  CountingCallbackPtr cb(boost::make_shared<CountingCallback>());
  CallbackQueue queue;
  queue.addCallback(cb, 1);
  queue.callOne();

  EXPECT_EQ(cb->count, 1U);

  queue.addCallback(cb, 1);
  queue.callAvailable();

  EXPECT_EQ(cb->count, 2U);

  queue.callOne();
  EXPECT_EQ(cb->count, 2U);

  queue.callAvailable();
  EXPECT_EQ(cb->count, 2U);
}

TEST(CallbackQueue, multipleCallbacksCallAvailable)
{
  CountingCallbackPtr cb(boost::make_shared<CountingCallback>());
  CallbackQueue queue;
  for (uint32_t i = 0; i < 1000; ++i)
  {
    queue.addCallback(cb, 1);
  }

  queue.callAvailable();

  EXPECT_EQ(cb->count, 1000U);
}

TEST(CallbackQueue, multipleCallbacksCallOne)
{
  CountingCallbackPtr cb(boost::make_shared<CountingCallback>());
  CallbackQueue queue;
  for (uint32_t i = 0; i < 1000; ++i)
  {
    queue.addCallback(cb, 1);
  }

  for (uint32_t i = 0; i < 1000; ++i)
  {
    queue.callOne();
    EXPECT_EQ(cb->count, i + 1);
  }
}

TEST(CallbackQueue, remove)
{
  CountingCallbackPtr cb1(boost::make_shared<CountingCallback>());
  CountingCallbackPtr cb2(boost::make_shared<CountingCallback>());
  CallbackQueue queue;
  queue.addCallback(cb1, 1);
  queue.addCallback(cb2, 2);
  queue.removeByID(1);
  queue.callAvailable();

  EXPECT_EQ(cb1->count, 0U);
  EXPECT_EQ(cb2->count, 1U);
}

class SelfRemovingCallback : public CallbackInterface
{
public:
  SelfRemovingCallback(CallbackQueue* queue, uint64_t id)
  : count(0)
  , queue(queue)
  , id(id)
  {}

  virtual CallResult call()
  {
    ++count;

    queue->removeByID(id);

    return Success;
  }

  size_t count;

  CallbackQueue* queue;
  uint64_t id;
};
typedef boost::shared_ptr<SelfRemovingCallback> SelfRemovingCallbackPtr;

TEST(CallbackQueue, removeSelf)
{
  CallbackQueue queue;
  SelfRemovingCallbackPtr cb1(boost::make_shared<SelfRemovingCallback>(&queue, 1));
  CountingCallbackPtr cb2(boost::make_shared<CountingCallback>());
  queue.addCallback(cb1, 1);
  queue.addCallback(cb2, 1);
  queue.addCallback(cb2, 1);

  queue.callOne();

  queue.addCallback(cb2, 1);

  queue.callAvailable();

  EXPECT_EQ(cb1->count, 1U);
  EXPECT_EQ(cb2->count, 1U);
}

class RecursiveCallback : public CallbackInterface
{
public:
  RecursiveCallback(CallbackQueue* queue, bool use_available)
  : count(0)
  , queue(queue)
  , use_available(use_available)
  {}

  virtual CallResult call()
  {
    ++count;

    if (count < 3)
    {
      if (use_available)
      {
        queue->callAvailable();
      }
      else
      {
        queue->callOne();
      }
    }

    return Success;
  }

  size_t count;

  CallbackQueue* queue;
  bool use_available;
};
typedef boost::shared_ptr<RecursiveCallback> RecursiveCallbackPtr;

TEST(CallbackQueue, recursive1)
{
  CallbackQueue queue;
  RecursiveCallbackPtr cb(boost::make_shared<RecursiveCallback>(&queue, true));
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.callAvailable();

  EXPECT_EQ(cb->count, 3U);
}

TEST(CallbackQueue, recursive2)
{
  CallbackQueue queue;
  RecursiveCallbackPtr cb(boost::make_shared<RecursiveCallback>(&queue, false));
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.callOne();

  EXPECT_EQ(cb->count, 3U);
}

TEST(CallbackQueue, recursive3)
{
  CallbackQueue queue;
  RecursiveCallbackPtr cb(boost::make_shared<RecursiveCallback>(&queue, false));
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.callAvailable();

  EXPECT_EQ(cb->count, 3U);
}

TEST(CallbackQueue, recursive4)
{
  CallbackQueue queue;
  RecursiveCallbackPtr cb(boost::make_shared<RecursiveCallback>(&queue, true));
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.addCallback(cb, 1);
  queue.callOne();

  EXPECT_EQ(cb->count, 3U);
}

void callAvailableThread(CallbackQueue* queue, bool& done)
{
  while (!done)
  {
    queue->callAvailable(ros::WallDuration(0.1));
  }
}

size_t runThreadedTest(const CountingCallbackPtr& cb, const boost::function<void(CallbackQueue*, bool&)>& threadFunc)
{
  CallbackQueue queue;
  boost::thread_group tg;
  bool done = false;

  for (uint32_t i = 0; i < 10; ++i)
  {
    tg.create_thread(boost::bind(threadFunc, &queue, boost::ref(done)));
  }

  ros::WallTime start = ros::WallTime::now();
  size_t i = 0;
  while (ros::WallTime::now() - start < ros::WallDuration(5))
  {
    queue.addCallback(cb);
    ++i;
  }

  while (!queue.isEmpty())
  {
    ros::WallDuration(0.01).sleep();
  }

  done = true;
  tg.join_all();

  return i;
}

TEST(CallbackQueue, threadedCallAvailable)
{
  CountingCallbackPtr cb(boost::make_shared<CountingCallback>());
  size_t i = runThreadedTest(cb, callAvailableThread);
  ROS_INFO_STREAM(i);
  EXPECT_EQ(cb->count, i);
}

void callOneThread(CallbackQueue* queue, bool& done)
{
  while (!done)
  {
    queue->callOne(ros::WallDuration(0.1));
  }
}

TEST(CallbackQueue, threadedCallOne)
{
  CountingCallbackPtr cb(boost::make_shared<CountingCallback>());
  size_t i = runThreadedTest(cb, callOneThread);
  ROS_INFO_STREAM(i);
  EXPECT_EQ(cb->count, i);
}

// this class is just an ugly hack
// to access the constructor Timer(TimerOptions)
namespace ros
{
class NodeHandle
{
public:
  static Timer createTimer(const TimerOptions& ops)
  {
    return Timer(ops);
  }
};
}

void dummyTimer(const ros::TimerEvent&)
{
}

CallbackQueueInterface* recursiveTimerQueue;

void recursiveTimer(const ros::TimerEvent&)
{
  // wait until the timer is TimerRecreationCallback is garbaged
  WallDuration(2).sleep();

  TimerOptions ops(Duration(0.1), dummyTimer, recursiveTimerQueue, false, false);
  Timer t = ros::NodeHandle::createTimer(ops);
  t.start();
}

class TimerRecursionCallback : public CallbackInterface
{
public:
  TimerRecursionCallback(CallbackQueueInterface* _queue)
  : queue(_queue)
  {}

  virtual CallResult call()
  {
    TimerOptions ops(Duration(0.1), recursiveTimer, queue, false, false);
    Timer t = ros::NodeHandle::createTimer(ops);
    t.start();

    // wait until the recursiveTimer has been fired
    WallDuration(1).sleep();

    return Success;
  }

  CallbackQueueInterface* queue;
};
typedef boost::shared_ptr<TimerRecursionCallback> TimerRecursionCallbackPtr;

TEST(CallbackQueue, recursiveTimer)
{
  // ensure that the test does not dead-lock, see #3867
  ros::Time::init();
  CallbackQueue queue;
  recursiveTimerQueue = &queue;
  TimerRecursionCallbackPtr cb(boost::make_shared<TimerRecursionCallback>(&queue));
  queue.addCallback(cb, 1);

  boost::thread_group tg;
  bool done = false;

  for (uint32_t i = 0; i < 2; ++i)
  {
    tg.create_thread(boost::bind(callOneThread, &queue, boost::ref(done)));
  }

  while (!queue.isEmpty())
  {
    ros::WallDuration(0.01).sleep();
  }

  done = true;
  tg.join_all();
}

class ConditionObject
{
public:
  ConditionObject(CallbackQueue * _queue)
  : id(0), queue(_queue) {
    condition_sync.store(true);
    condition_one.store(false);
    condition_stop.store(false);
  }

  void add();

  unsigned long id;
  CallbackQueue * queue;
  boost::atomic<bool> condition_one;
  boost::atomic<bool> condition_sync;
  boost::atomic<bool> condition_stop;
};

class RaceConditionCallback : public CallbackInterface
{
public:
  RaceConditionCallback(ConditionObject * _condition_object, unsigned long * _id)
  : condition_object(_condition_object), id(_id)
  {}

  virtual CallResult call()
  {
    condition_object->condition_one.store(false);
    return Success;
  }

  ConditionObject * condition_object;
  unsigned long * id;
};

void ConditionObject::add()
{
  while(!condition_stop.load())
  {
    if(condition_sync.load() && queue->isEmpty())
    {
      condition_one.store(true);
      id++;
      queue->addCallback(boost::make_shared<RaceConditionCallback>(this, &id), id);
    }
    boost::this_thread::sleep(boost::posix_time::microseconds(1));
  }
}

TEST(CallbackQueue, raceConditionCallback)
{
  CallbackQueue queue;
  ConditionObject condition_object(&queue);

  boost::thread t(boost::bind(&ConditionObject::add, &condition_object));
  for(unsigned int i = 0; i < 1000000; ++i)
  {
    condition_object.condition_sync.store(false);
    if (queue.callOne() == CallbackQueue::Called)
    {
      if(condition_object.condition_one.load())
      {
        condition_object.condition_stop.store(true);
        ASSERT_FALSE(condition_object.condition_one.load());
      }
    }

    queue.clear();
    condition_object.condition_sync.store(true);
  }
  condition_object.condition_stop.store(true);
  t.join();
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

max_stars_repo_name: Bookiebookie/LieSpline

max_issues_repo_name: Bookiebookie/LieSpline

max_forks_repo_name: Bookiebookie/LieSpline


text:
/*
 * Copyright (c) 2014, Stanislav Vorobiov
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "PuzzleHintAreaComponent.h"
#include "CameraComponent.h"
#include "SceneObjectFactory.h"
#include "Scene.h"
#include "Settings.h"
#include "Utils.h"
#include <boost/make_shared.hpp>

namespace af
{
    PuzzleHintAreaComponent::PuzzleHintAreaComponent()
    : PhasedComponent(phasePreRender),
      triggered_(false),
      t_(0.0f),
      inside_(false),
      hintLastPos_(b2Vec2_zero)
    {
    }

    PuzzleHintAreaComponent::~PuzzleHintAreaComponent()
    {
    }

    void PuzzleHintAreaComponent::accept(ComponentVisitor& visitor)
    {
        visitor.visitPhasedComponent(shared_from_this());
    }

    void PuzzleHintAreaComponent::preRender(float dt)
    {
        if (obj_ && obj_->dead()) {
            inside_ = false;
        }

        if (!triggered_) {
            if (!inside_) {
                return;
            }

            CameraComponentPtr cc = scene()->camera()->findComponent<CameraComponent>();

            for (HintTriggers::const_iterator it = hintTriggers_.begin();
                it != hintTriggers_.end(); ++it) {
                if (cc->entireRectVisible(it->first, it->second * 2.0f, it->second * 2.0f)) {
                    triggered_ = true;
                    break;
                }
            }

            return;
        }

        t_ -= dt;

        if (t_ >= 0.0f) {
            return;
        }

        for (Hints::iterator it = hints_.begin(); it != hints_.end();) {
            if (!it->obj) {
                it->obj = sceneObjectFactory.createPuzzleHint();
                it->obj->setPos(it->pos);
                scene()->addObject(it->obj);
            }

            if (it->removing) {
                it->alpha -= (settings.puzzleHint.alpha / settings.puzzleHint.timeout) * dt;
                if (it->alpha <= 0.0f) {
                    it->obj->removeFromParent();
                    it = hints_.erase(it);
                    continue;
                } else {
                    it->obj->setAlpha(it->alpha);
                }
            } else if (it->alpha < settings.puzzleHint.alpha) {
                it->alpha += (settings.puzzleHint.alpha / settings.puzzleHint.timeout) * dt;
                if (it->alpha >= settings.puzzleHint.alpha) {
                    it->alpha = settings.puzzleHint.alpha;
                }
                it->obj->setAlpha(it->alpha);
            }

            ++it;
        }

        if (inside_ && !indicator_) {
            indicator_ = boost::make_shared<PuzzleHintIndicatorComponent>(settings.puzzleHint.indicatorLength, -2);

            indicator_->setPadding(settings.puzzleHint.indicatorLength);
            indicator_->setColor(Color(1.0f, 1.0f, 1.0f, 0.0f));

            parent()->addComponent(indicator_);
        } else if (!inside_ && indicator_) {
            indicator_->setDirection(hintLastPos_ - obj_->smoothPos());
            if (fade(dt, false)) {
                indicator_->removeFromParent();
                indicator_.reset();
                obj_.reset();
            }
            return;
        }

        if (!indicator_) {
            return;
        }

        b2AABB aabb;

        if (!hints_.empty()) {
            aabb = scene()->camera()->findComponent<CameraComponent>()->getAABB();
        }

        const Hint* hint = NULL;
        float minDistSq = b2_maxFloat;

        for (Hints::const_iterator it = hints_.begin();
             it != hints_.end(); ++it) {
            b2Vec2 pos;
            if (it->removing) {
                continue;
            } else {
                pos = it->obj->pos();
            }
            bool visible = pointInRect(pos, aabb);
            if (visible) {
                hint = NULL;
                break;
            }

            float distSq = b2DistanceSquared(obj_->smoothPos(), pos);
            if (distSq <= minDistSq) {
                minDistSq = distSq;
                hint = &(*it);
            }
        }

        if (hint) {
            hintLastPos_ = hint->obj->pos();
        }

        fade(dt, hint != NULL);
        indicator_->setDirection(hintLastPos_ - obj_->smoothPos());
    }

    void PuzzleHintAreaComponent::sensorEnter(const SceneObjectPtr& other)
    {
        setObject(other);
    }

    void PuzzleHintAreaComponent::sensorExit(const SceneObjectPtr& other)
    {
        setObject(SceneObjectPtr());
    }

    void PuzzleHintAreaComponent::setObject(const SceneObjectPtr& value)
    {
        inside_ = !!value;
        if (value) {
            obj_ = value;
        }
    }

    void PuzzleHintAreaComponent::addHintTrigger(const b2Vec2& pos, float radius)
    {
        hintTriggers_.push_back(std::make_pair(pos, radius));
    }

    void PuzzleHintAreaComponent::addHint(const b2Vec2& pos)
    {
        removeHint(pos);
        hints_.push_back(Hint(pos));
    }

    void PuzzleHintAreaComponent::removeHint(const b2Vec2& pos)
    {
        for (Hints::iterator it = hints_.begin();
             it != hints_.end();) {
            if (it->pos == pos) {
                if (!it->obj) {
                    it = hints_.erase(it);
                    continue;
                } else {
                    it->removing = true;
                }
            }
            ++it;
        }
    }

    void PuzzleHintAreaComponent::removeAllHints()
    {
        for (Hints::iterator it = hints_.begin();
             it != hints_.end();) {
            if (!it->obj) {
                it = hints_.erase(it);
                continue;
            } else {
                it->removing = true;
            }
            ++it;
        }
    }

    void PuzzleHintAreaComponent::onRegister()
    {
    }

    void PuzzleHintAreaComponent::onUnregister()
    {
        obj_.reset();
        if (indicator_) {
            indicator_->removeFromParent();
            indicator_.reset();
        }
        for (Hints::iterator it = hints_.begin();
             it != hints_.end(); ++it) {
            if (it->obj) {
                it->obj->removeFromParent();
            }
        }
        hints_.clear();
    }

    bool PuzzleHintAreaComponent::fade(float dt, bool in)
    {
        Color c = indicator_->color();
        bool res = false;

        if (in) {
            c.rgba[3] += (settings.puzzleHint.alpha / settings.puzzleHint.timeout) * dt;
            if (c.rgba[3] > settings.puzzleHint.alpha) {
                c.rgba[3] = settings.puzzleHint.alpha;
                res = true;
            }
            indicator_->setColor(c);
        } else {
            c.rgba[3] -= (settings.puzzleHint.alpha / settings.puzzleHint.timeout) * dt;
            if (c.rgba[3] < 0.0f) {
                c.rgba[3] = 0.0f;
                res = true;
            }
            indicator_->setColor(c);
        }

        return res;
    }
}

max_stars_repo_name: Sheph/TriggerTime

max_issues_repo_name: Sheph/TriggerTime

max_forks_repo_name: Sheph/TriggerTime


text:
/**
 * \ file GainExpanderFilter.cpp
 */

#include <ATK/Dynamic/GainExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainExpanderFilter, softness_test)
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainExpanderFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainExpanderFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.1);
  filter.set_ratio(2);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 0, 0.001);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.700553358, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_2_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.700553358, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_twice)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_twice_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

max_stars_repo_name: AudioTK/AudioTK

max_issues_repo_name: AudioTK/AudioTK

max_forks_repo_name: AudioTK/AudioTK


text:
// Copyright (c) 2009-2014 Vladimir Batov.
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. See http://www.boost.org/LICENSE_1_0.txt.

#include <boost/convert.hpp>
#include <boost/convert/lexical_cast.hpp>

//[stream_headers1
#include <boost/convert.hpp>
#include <boost/convert/stream.hpp>
#include <boost/detail/lightweight_test.hpp>

using std::string;
using boost::convert;

struct boost::cnv::by_default : public boost::cnv::cstream {};
//]
//[stream_headers2
namespace cnv = boost::cnv;
namespace arg = boost::cnv::parameter;
//]

#include "../test/test.hpp"

static
void
example1()
{
    //[stream_example1
    int    i2 = convert<int>("123").value();      // Throws when fails.
    int    i3 = convert<int>("uhm").value_or(-1); // Returns -1 when fails.
    string s2 = convert<string>(123).value();

    BOOST_TEST(i2 == 123);
    BOOST_TEST(i3 == -1);
    BOOST_TEST(s2 == "123");
    //]
}

static
void
example2()
{
    //[stream_example2
    boost::cnv::cstream ccnv;
    boost::cnv::wstream wcnv;

    int v01 = convert<int>("  FF", ccnv(std::hex)(std::skipws)).value_or(0);
    int v02 = convert<int>(L"  F", wcnv(std::hex)(std::skipws)).value_or(0);
    int v03 = convert<int>("  FF", ccnv(std::dec)(std::skipws)).value_or(-5);
    int v04 = convert<int>(L"  F", wcnv(std::dec)(std::skipws)).value_or(-5);

    BOOST_TEST(v01 == 255); // "FF"
    BOOST_TEST(v02 ==  15); // L"F"
    BOOST_TEST(v03 ==  -5); // Failed to convert "FF" as decimal.
    BOOST_TEST(v04 ==  -5); // Failed to convert L"F" as decimal.
    //]
    //[stream_example3
    ccnv(std::showbase)(std::uppercase)(std::hex);

    BOOST_TEST(convert<string>(255, ccnv, "bad") == "0XFF");
    BOOST_TEST(convert<string>( 15, ccnv, "bad") ==  "0XF");
    //]
    //[stream_example4
    ccnv(arg::base = cnv::base::dec)
        (arg::uppercase = false)
        (arg::notation = cnv::notation::scientific);
    //]
    //[stream_example5
    ccnv(std::dec)(std::uppercase)(std::scientific);
    //]
}

static
void
example6()
{
    //[stream_example6
    boost::cnv::cstream      cnv1;
    boost::cnv::lexical_cast cnv2;

    change chg = change::up;
    string  s1 = convert<string>(chg, cnv1, "bad");                // Input type (change) deduced
    string  s2 = convert<string, change>(change::dn, cnv1, "bad"); // Input type (change) forced

    BOOST_TEST(convert<change>("up", cnv1, change::no) == change::up);
    BOOST_TEST(convert<change>("up", cnv2, change::no) == change::up);
    BOOST_TEST(s1 == "up");
    BOOST_TEST(s2 == "dn");
    //]
}

int
main(int argc, char const* argv[])
{
    example1();
    example2();
    example6();

    return boost::report_errors();
}