Project/plc调度节点/tool/point2D_tool.cpp

//
// Created by huli on 2020/9/1.
//

#include "point2D_tool.h"

//极坐标 -> 平面坐标
bool Point2D_tool::Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D)
{
	if ( p_polar_coordinates == NULL || p_point2D == NULL)
	{
		return false;
	}
	else
	{
		p_point2D->x = p_polar_coordinates->distance * cos(p_polar_coordinates->angle);
		p_point2D->y = p_polar_coordinates->distance * sin(p_polar_coordinates->angle);
		return true;
	}
	return true;
}
//平面坐标 -> 极坐标
bool Point2D_tool::Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates)
{
	if ( p_polar_coordinates == NULL || p_point2D == NULL)
	{
		return false;
	}
	else
	{
		p_polar_coordinates->distance = sqrt(p_point2D->x * p_point2D->x + p_point2D->y * p_point2D->y);
		p_polar_coordinates->angle = atan(p_point2D->y / p_point2D->x);
		return true;
	}
	return true;
}

//判断极坐标点是否在限制范围
bool Point2D_tool::limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box)
{
	if ( p_polar_coordinates_box == NULL )
	{
		return false;
	}
	else
	{
		if ( angle >= p_polar_coordinates_box->angle_min &&
			 angle <= p_polar_coordinates_box->angle_max &&
			 distance >= p_polar_coordinates_box->distance_min &&
			 distance <= p_polar_coordinates_box->distance_max )
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	return true;
}

//判断平面坐标点是否在限制范围
bool Point2D_tool::limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box)
{
	if ( p_point2D_box == NULL )
	{
		return false;
	}
	else
	{
		if ( x >= p_point2D_box->x_min &&
			 x <= p_point2D_box->x_max &&
			 y >= p_point2D_box->y_min &&
			 y <= p_point2D_box->y_max )
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	return true;
}
#ifdef OPEN_PCL_FLAG
//平面坐标的转换, 可以进行旋转和平移, 不能缩放
bool Point2D_tool::transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
										 Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if ( p_x_in == NULL || p_x_in == NULL || p_x_out == NULL || p_y_out == NULL || p_point2D_transform == NULL)
	{
		return false;
	}
	else
	{
		*p_x_out = *p_x_in * p_point2D_transform->m00 + *p_y_in * p_point2D_transform->m01 + p_point2D_transform->m02;
		*p_y_out = *p_x_in * p_point2D_transform->m10 + *p_y_in * p_point2D_transform->m11 + p_point2D_transform->m12;
		return true;
	}
	return true;
}
bool Point2D_tool::transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
												Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if (p_point3D_in == NULL || p_point3D_out == NULL || p_point2D_transform == NULL)
	{
		return false;
	}
	else
	{
		p_point3D_out->x = p_point3D_in->x * p_point2D_transform->m00 + p_point3D_in->y * p_point2D_transform->m01 +
						   p_point2D_transform->m02;
		p_point3D_out->y = p_point3D_in->x * p_point2D_transform->m10 + p_point3D_in->y * p_point2D_transform->m11 +
						   p_point2D_transform->m12;
		return true;
	}
	return true;
}
bool Point2D_tool::transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
												pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
												Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if ( p_cloud_in.get() == NULL || p_cloud_out.get() == NULL || p_point2D_transform == NULL)
	{
	    return false;
	}
	else
	{
		pcl::PointXYZ point;
		for (int i = 0; i < p_cloud_in->size(); ++i)
		{
			point.x = p_cloud_in->points[i].x * p_point2D_transform->m00 + p_cloud_in->points[i].y * p_point2D_transform->m01 + p_point2D_transform->m02;
			point.y = p_cloud_in->points[i].x * p_point2D_transform->m10 + p_cloud_in->points[i].y * p_point2D_transform->m11 + p_point2D_transform->m12;
			p_cloud_out->push_back(point);
		}
		return true;
	}
	return true;
}
#endif