#include <iostream>
#include <Eigen/Dense>#define M_PI 3.1415926// 计算三点组成平面的参数和变换到XOY平面的变换矩阵
void computePlaneAndTransform(const Eigen::Vector3d & P1, const Eigen::Vector3d & P2, const Eigen::Vector3d & P3,Eigen::Vector4d& planeParams, Eigen::Matrix4d& transformMatrix) {// 确保P1在原点assert((P1 - Eigen::Vector3d ::Zero()).norm() < 1e-6);// 计算平面的法向量Eigen::Vector3d normal = P2.cross(P3);normal.normalize();std::cout << "z = " << normal.z() << std::endl;if (normal.z() < 0) {normal = -1 * normal;}std::cout << "z2 = " << normal.z() << std::endl;// 平面方程为 Ax + By + Cz + D = 0，这里P1在原点，D = 0planeParams << normal.x(), normal.y(), normal.z(), 0;// 计算旋转矩阵，使得平面的法向量与Z轴对齐Eigen::Vector3d zAxis(0, 0, 1);double cosTheta = normal.dot(zAxis);Eigen::Vector3d crossProduct = normal.cross(zAxis);double sinTheta = crossProduct.norm();if (sinTheta < 1e-6) {// 特殊情况，法向量与Z轴平行或反平行if (cosTheta < 0) {// 法向量与Z轴反平行transformMatrix = Eigen::Matrix4d::Identity();transformMatrix.block<3, 3>(0, 0) = Eigen::AngleAxisd(M_PI, Eigen::Vector3d (1, 0, 0)).toRotationMatrix();} else {// 法向量与Z轴平行transformMatrix = Eigen::Matrix4d::Identity();}} else {crossProduct.normalize();Eigen::Matrix3d rotationMatrix;rotationMatrix << cosTheta + crossProduct.x() * crossProduct.x() * (1 - cosTheta),crossProduct.x()* crossProduct.y()* (1 - cosTheta) - crossProduct.z() * sinTheta,crossProduct.x()* crossProduct.z()* (1 - cosTheta) + crossProduct.y() * sinTheta,crossProduct.y()* crossProduct.x()* (1 - cosTheta) + crossProduct.z() * sinTheta,cosTheta + crossProduct.y() * crossProduct.y() * (1 - cosTheta),crossProduct.y()* crossProduct.z()* (1 - cosTheta) - crossProduct.x() * sinTheta,crossProduct.z()* crossProduct.x()* (1 - cosTheta) - crossProduct.y() * sinTheta,crossProduct.z()* crossProduct.y()* (1 - cosTheta) + crossProduct.x() * sinTheta,cosTheta + crossProduct.z() * crossProduct.z() * (1 - cosTheta);transformMatrix.setIdentity();transformMatrix.block<3, 3>(0, 0) = rotationMatrix;}
}// 判断点在向量的哪一侧
int determineSide(const Eigen::Vector3d & p4, const Eigen::Vector3d & p5, const Eigen::Vector3d & p6) {// 投影到XOY平面Eigen::Vector2d p4_2d = p4.head<2>();Eigen::Vector2d p5_2d = p5.head<2>();Eigen::Vector2d p6_2d = p6.head<2>();// 计算向量 P4->P5 和 P4->P6Eigen::Vector2d v1 = p5_2d - p4_2d;Eigen::Vector2d v2 = p6_2d - p4_2d;double crossResult = v1.x() * v2.y() - v1.y() * v2.x();std::cout << "crossResult = " << crossResult << std::endl;if (crossResult < 0) {// 右侧return 1;} else if (crossResult > 0) {// 左侧return 2;} else {// 共线return 0;}
}int main() {Eigen::Vector3d P1 = Eigen::Vector3d ::Zero();Eigen::Vector3d P2(10, -10, -13);Eigen::Vector3d P3(0, 15, 0);Eigen::Vector4d planeParams;Eigen::Matrix4d mat;computePlaneAndTransform(P1, P2, P3, planeParams, mat);// 将点变换到XOY平面Eigen::Vector3d P4 = (mat * Eigen::Vector4d(P1.x(), P1.y(), P1.z(), 1)).head<3>();Eigen::Vector3d P5 = (mat * Eigen::Vector4d(P2.x(), P2.y(), P2.z(), 1)).head<3>();Eigen::Vector3d P6 = (mat * Eigen::Vector4d(P3.x(), P3.y(), P3.z(), 1)).head<3>();std::cout << "x, y, z = " << P4.x() << ", " << P4.y() << ", " << P4.z() << std::endl;std::cout << "x, y, z = " << P5.x() << ", " << P5.y() << ", " << P5.z() << std::endl;std::cout << "x, y, z = " << P6.x() << ", " << P6.y() << ", " << P6.z() << std::endl;int side = determineSide(P4, P5, P6);std::cout << "P3在向量P1->P2的";if (side == 1) {std::cout << "右侧" << std::endl;} else if (side == 2) {std::cout << "左侧" << std::endl;} else {std::cout << "共线" << std::endl;}system("pause");return 0;
}
