计算一个运行中的矩形AGV（自动导引车）前方的禁止区域矩形的顶点坐标

前言

在AGV（自动导向车）前方设置一个禁止区域：禁止区可以防止AGV与其他物体或人员发生碰撞，特别是在AGV前方有障碍物时。通过设置禁止区，AGV可以及时减速或停止，避免意外事故。

一、计算矩形的四个顶点

// 计算矩形顶点
void calculateRectangleVertices(double cx, double cy, double w, double h, double angle, Point vertices[4]) {
    // 矩形四个顶点相对于中心点的坐标
    Point relativeVertices[4] = {
        {-w / 2, h / 2},   // 左上角
        {w / 2, h / 2},    // 右上角
        {w / 2, -h / 2},   // 右下角
        {-w / 2, -h / 2}   // 左下角
    };

    for (int i = 0; i < 4; ++i) {
        // 旋转和平移
        Point rotated = rotatePoint(relativeVertices[i], angle);
        vertices[i].x = rotated.x + cx;
        vertices[i].y = rotated.y + cy;
    }
}

矩形agv旋转、平移后四个顶点的坐标

二、 确定禁止区域的顶点

// 计算禁止区域顶点
void calculateProhibitionZone(double agvCx, double agvCy, double agvAngle, double length, double width, Point prohibitionVertices[4]) {
    // 禁止区域的中心点
    double prohibitCx = agvCx + length * std::cos(agvAngle);
    double prohibitCy = agvCy + length * std::sin(agvAngle);

    // 计算禁止区域的顶点
    calculateRectangleVertices(prohibitCx, prohibitCy, width, length, agvAngle, prohibitionVertices);
}

在上述代码中agvCx、agvCy为矩形agv的中心坐标，agvAngle为旋转角度，length为agv矩形与禁止层矩形宽度之和的一半。

三、测试代码 

#include <iostream>
#include <cmath>

#define M_PI1 3.1415926

struct Point {
    double x, y;
};

// 旋转函数
Point rotatePoint(const Point& p, double angle) {
    Point rotated;
    rotated.x = p.x * std::cos(angle) - p.y * std::sin(angle);
    rotated.y = p.x * std::sin(angle) + p.y * std::cos(angle);
    return rotated;
}

// 计算矩形顶点 
void calculateRectangleVertices(double cx, double cy, double w, double h, double angle, Point vertices[4]) {
    // 矩形四个顶点相对于中心点的坐标
    Point relativeVertices[4] = {
        {-w / 2, h / 2},   // 左上角
        {w / 2, h / 2},    // 右上角
        {w / 2, -h / 2},   // 右下角
        {-w / 2, -h / 2}   // 左下角
    };

    for (int i = 0; i < 4; ++i) {
        // 旋转和平移
        Point rotated = rotatePoint(relativeVertices[i], angle);
        vertices[i].x = rotated.x + cx;
        vertices[i].y = rotated.y + cy;
    }
}

// 计算禁止区域顶点
void calculateProhibitionZone(double agvCx, double agvCy, double agvAngle, double length, double width, Point prohibitionVertices[4]) {
    // 禁止区域的中心点
    double prohibitCx = agvCx + length * std::cos(agvAngle);
    double prohibitCy = agvCy + length * std::sin(agvAngle);

    // 计算禁止区域的顶点
    calculateRectangleVertices(prohibitCx, prohibitCy, width, length, agvAngle, prohibitionVertices);
}

int main() {
    // AGV的中心坐标、宽度、高度、朝向角度
    double agvCx = 5, agvCy = 5; // AGV中心
    double agvWidth = 2, agvHeight = 1; // AGV宽度和高度
    double agvAngle = M_PI1 / 4; // AGV朝向角度（45度）

    // 禁止区域的长度和宽度
    double prohibitionLength = 3; // 禁止区域的长度
    double prohibitionWidth = 2;   // 禁止区域的宽度

    // 存储矩形的顶点
    Point agvVertices[4];
    Point prohibitionVertices[4];

    // 计算AGV的顶点
    calculateRectangleVertices(agvCx, agvCy, agvWidth, agvHeight, agvAngle, agvVertices);

    // 计算禁止区域的顶点
    prohibitionLength = (prohibitionLength + agvCx) / 2.0;
    calculateProhibitionZone(agvCx, agvCy, agvAngle, prohibitionLength, prohibitionWidth, prohibitionVertices);

    // 输出结果
    std::cout << "AGV Vertices:\n";
    for (int i = 0; i < 4; ++i) {
        std::cout << "AGV Vertex " << i + 1 << ": (" << agvVertices[i].x << ", " << agvVertices[i].y << ")\n";
    }

    std::cout << "\nProhibition Zone Vertices:\n";
    for (int i = 0; i < 4; ++i) {
        std::cout << "Prohibition Vertex " << i + 1 << ": (" << prohibitionVertices[i].x << ", " << prohibitionVertices[i].y << ")\n";
    }

    return 0;
}

分别确定两个矩形的中心点，得到中心点后可旋转、平移计算方式得到四个顶点的坐标。 

 

原文地址：https://blog.csdn.net/canyuemanyue/article/details/142459099

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！