平面向量及空间向量间角度的计算

平面向量（平面上的直线）存在关系：

如果根据斜率什么的判断可能会涉及到对斜率计算结果精确度的判断，这就很麻烦，所以并不推荐用斜率计算

更推荐的是使用向量的关系;

假设向量A（a1，a2） 向量B（b1，b2）

平行：

共线：a1*b1=a2*b2 ，这里就不需要担心计算斜率带来的精度的问题了

相交（则计算角度）：

代码实现：如下文贴出的空间向量计算的实现

 

空间向量（空间直线）存在关系：

平行：

共线：

相交（则计算角度）：

异面：

 

代码：利用Eigen库计算两个向量的旋转角以及旋转矩阵

代码中设置的旋转轴是z轴法向量

但是光计算出旋转矩阵还不够，还需要计算出平移矩阵，这样就能模拟出以自身作为旋转的中心旋转的效果了。这部分在代码中是通过计算旋转前后点云中心点的平移来实现的。

那么可能会有人问为什么不直接计算该点云的中心轴，直接以自身为中心进行旋转呢，关于这个方法，暂时并不知道如何去做。

	//【1】求两个向量间的旋转角angle(点积)
	double tem = vecbefore.dot(vecafter);//分子
	//cout << vecbefore<<endl;
	//cout << vecafter << endl;
	double tep = sqrt(vecbefore.dot(vecbefore) * vecafter.dot(vecafter));//分母
	double angle = acos(tem / tep);

	if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
	{
		angle = acos(tep / tem);
	}
	cout << angle * 180 / PI << endl;
	//cout << angle * PI / 180 << endl;
	//	std::cout << "角度： " << angle << std::endl;
		//【2】求旋转轴（叉积）
	Eigen::Vector3f axis1 = vecbefore.cross(vecafter);
	//	std::cout << "求旋转轴： " << axis1 << std::endl;
	Eigen::Vector3f axis2 = vecafter.cross(vecbefore);
	//std::cout << "求旋转轴： " << axis2 << std::endl;
	//std::cout << "求旋转轴(归一化)： " << axis2.normalized() << std::endl;
	//【3】求旋转矩阵
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	// Define a translation of 2.5 meters on the x axis.//平移

	// The same rotation matrix as before; theta radians arround Z axis
	transform_2.rotate(Eigen::AngleAxisf(angle, axis1.normalized()));
	//transform_2(0, 3) = 0;
	//transform_2(1, 3) = 0;
	//transform_2(2, 3) =0;
	// Print the transformation
	tra = transform_2;

	Mat H = (Mat_ <float>(3, 3) << tra(0, 0), tra(0, 1), tra(0, 2), tra(1, 0), tra(1, 1), tra(1, 2), tra(2, 0), tra(2, 1), tra(2, 2));
	Mat pointcenterTran = H * pointcenter;
	//cout << H << endl;
	//cout << pointcenterTran << endl;
	float x_ = pointcenter.at<float>(0, 0) - pointcenterTran.at<float>(0, 0);
	float y_ = pointcenter.at<float>(1, 0) - pointcenterTran.at<float>(1, 0);
	float z_ = pointcenter.at<float>(2, 0) - pointcenterTran.at<float>(2, 0);
	transform_2.translation() << x_, y_, z_;
	transform_2.rotate(Eigen::AngleAxisf(ang, Eigen::Vector3f::UnitZ()));
	// 可以使用 transform_1 或 transform_2; t它们是一样的
	tra = transform_2;