3D坐标下,一点在某一线段上的左右方向的判定

3D坐标下,一点在某一线段上的左右方向的判定

代码

C++ 复制代码
#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 = 0
    planeParams << 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->P6
    Eigen::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;
}
相关推荐
视觉人机器视觉16 分钟前
机器视觉Halcon3D中,六大类3D处理算子
人工智能·计算机视觉·3d·视觉检测
Damon小智29 分钟前
从零开始XR开发:Three.js实现交互式3D积木搭建器
javascript·3d·xr
-dzk-9 小时前
【3DGS复现】Autodl服务器复现3DGS《简单快速》《一次成功》《新手练习复现必备》
运维·服务器·python·计算机视觉·3d·三维重建·三维
夏婵语冰1 天前
C4D R20新功能实战指南:深度解析域、节点材质与OpenVDB,提升你的3D创作效率
3d·材质·动画制作·三维设计·c4d教程
元让_vincent3 天前
论文Review SLAM R3LIVE | ICRA2022 港大MARS | 可以生成Mesh的激光视觉惯性SLAM
3d·机器人·图形渲染·slam·建图
Demoncode_y4 天前
Vue3 + Three.js 实现 3D 汽车个性化定制及展示
前端·javascript·vue.js·3d·汽车·three.js
lqjun08274 天前
VTK相机正射投影中通过多个2D坐标计算3D坐标
数码相机·计算机视觉·3d
zenithdev15 天前
开源库入门教程 Cesium:3D地球和地图库
其他·3d·arcgis
研梦非凡6 天前
探索3D空间的视觉基础模型系列
人工智能·深度学习·神经网络·机器学习·计算机视觉·3d
CG_MAGIC6 天前
VR 太阳光参数与快速渲染
3d·vr·3dmax·vray·渲云渲染·灯光设置