三种常见矩形框旋转方法推导及其C++实现

石凌风SLF

已于 2023-12-02 16:19:33 修改

阅读量3.6k

点赞数 7

分类专栏：传经送宝文章标签：矩形框旋转顶点旋转复数表示旋转欧拉公式代数几何复数的共轭 C++

于 2023-09-24 15:22:28 首次发布

本文链接：https://blog.csdn.net/jiqiren_dasheng/article/details/132423669

版权

传经送宝专栏收录该内容

12 篇文章

订阅专栏

在已知矩形中心点、长宽和旋转角度（定义为矩形最长边与X轴正方向的夹角），如何确定矩形四个顶点的坐标，通常有以下两种处理方法。

法一：直接对顶点进行旋转

比如下图虚线框矩形是实线框矩形绕矩形中心点旋转后得到。在已知矩形中心点坐标和长宽的前提下，实线框四顶点坐标可直接换算得到。然后就是分析计算经旋转后的虚线框矩形的四顶点坐标。

由于是绕矩形中心点旋转，因此可以将坐标系原点平移到矩形中心点位置。然后将矩形框四顶点用极坐标表示，并转换成直角坐标。

旋转前A顶点坐标：（其中 $r$ 表示矩形框四个顶点距离坐标原点的距离，α表示顶点与坐标原点连线与X轴的夹角）

$A=\left ( r\cdot \cos \alpha ,r\cdot \sin \alpha \right )=\left ( x-c_{x},y-c_{y} \right )$

则绕坐标原点旋转θ角度后A'顶点坐标：

$A'=\left ( r\cdot \cos \left ( \alpha+\theta \right ) ,r\cdot \sin \left ( \alpha+\theta \right ) \right )$

对A'顶点坐标按照三角函数的和差角公式展开：

$A'=\left ( r\cdot \cos \alpha \cdot \cos \theta -r\cdot \sin \alpha \cdot \sin \theta ,r\cdot \sin \alpha \cdot \cos \theta +r\cdot \sin \theta \cdot \cos \alpha \right )$

将A顶点坐标代入A'顶点坐标则有：

$A'=\left ( \left ( x-c_{x} \right ) \cdot \cos \theta -\left ( y-c_{y} \right ) \cdot \sin \theta ,\left ( y-c_{y} \right ) \cdot \cos \theta +\left ( x-c_{x} \right )\cdot \sin \theta \right )$

用矩阵形式表示：

$A'=\begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix}\cdot \left ( \begin{bmatrix} x\\ y \end{bmatrix}-\begin{bmatrix} c_{x}\\ c_{y} \end{bmatrix} \right )$

由于坐标系原点被平移到矩形中心点位置，因此最终还需将A'顶点坐标平移回去：

$A'=\begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix}\cdot \left ( \begin{bmatrix} x\\ y \end{bmatrix}-\begin{bmatrix} c_{x}\\ c_{y} \end{bmatrix} \right )+\begin{bmatrix} c_{x}\\ c_{y} \end{bmatrix}$

法二：根据三角形几何性质换算顶点坐标

针对四顶点分别绘制出下图所示辅助线，通过相似三角形不难得到下图中两相等辅助角。

矩形四顶点坐标分别为：

$A:\left\{\begin{matrix} x=c_{x}+\frac{l}{2}\cdot \cos \theta-\frac{w}{2}\cdot \sin \theta \\ y=c_{y}+\frac{l}{2}\cdot \sin \theta+\frac{w}{2}\cdot \cos \theta \end{matrix}\right.$

$B:\left\{\begin{matrix} x=c_{x}-\frac{l}{2}\cdot \cos \theta-\frac{w}{2}\cdot \sin \theta \\ y=c_{y}-\frac{l}{2}\cdot \sin \theta+\frac{w}{2}\cdot \cos \theta \end{matrix}\right.$

$C:\left\{\begin{matrix} x=c_{x}-\frac{l}{2}\cdot \cos \theta+\frac{w}{2}\cdot \sin \theta \\ y=c_{y}-\frac{l}{2}\cdot \sin \theta-\frac{w}{2}\cdot \cos \theta \end{matrix}\right.$

$D:\left\{\begin{matrix} x=c_{x}+\frac{l}{2}\cdot \cos \theta+\frac{w}{2}\cdot \sin \theta \\ y=c_{y}+\frac{l}{2}\cdot \sin \theta-\frac{w}{2}\cdot \cos \theta \end{matrix}\right.$

由于A与C、B与D分别是关于 $\left ( c_{x},c_{y} \right )$ 的对称点，所以各项正负号相反。

法三：复数表示旋转

复数相乘可以描述平面上的旋转：乘以+i会逆时针旋转90°，乘以-i会顺时针旋转90°。要证明该几何法则，可先考虑变换 $z=x+iy \rightarrow iz=-y+ix$ ，如下图(a)所示， $iz$ 就是把z逆时针旋转了90°。而对于一般的复数A，为了直观的表示 $z \rightarrow Az$ ，取 $A=4+i3=5\angle \theta$ ，如下图(b)所示：

此时对于 $Az$ ，按照复数的运算法则括号可展开： $Az=(4+3i)z=4z+3(iz)$ ，由前可知 $iz$ 就是把z逆时针旋转90°。利用复数与向量之间一一对应的关系，上图(c)刻画了这一变换过程。将A用更一般的表达式表示： $A=a+ib=\sqrt{a^{2}+b^{2}}\angle \theta$ ，其中 $\theta =arctan\frac{b}{a}$ ，则 $Az$ 就是将z旋转 $\theta$ 角度再缩放 $\sqrt{a^{2}+b^{2}}$ 倍。

将A用复变函数中欧拉公式的三角函数表示，并且出于简化计算的目的，取单位长度（旋转不改变大小）的复数，即： $A=cos\theta +isin\theta$

则有：

$x^{'}+iy^{'}=(cos\theta +isin\theta )(x+iy)=xcos\theta -ysin\theta +i(xsin\theta+ycos\theta )$

将该等式用矩阵形式表示：

$\begin{bmatrix} x^{'} \\ y^{'} \end{bmatrix}=\begin{bmatrix} cos\theta & -sin\theta\\ sin\theta & cos\theta \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix}$

其中含正余弦的系数矩阵就是二维旋转矩阵（可见与法一的系数矩阵一致）。

用R表示系数矩阵，并对其求逆：

$R^{-1}=\begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix}$

然后给用矩阵形式表示的等式两边同时左乘 $R^{-1}$ ：

$\begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix}\begin{bmatrix} x^{'} \\ y^{'} \end{bmatrix} =\begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix} \begin{bmatrix} cos\theta & -sin\theta\\ sin\theta & cos\theta \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix} =\begin{bmatrix} x \\ y \end{bmatrix}$

将该矩阵形式恢复成复数形式：

$x^{'}cos\theta + y^{'}sin\theta +i(-x^{'}sin\theta+y^{'}cos\theta ) = (cos\theta -isin\theta )(x^{'}+iy^{'}) = x+iy$

其中： $cos\theta - isin\theta = \bar{A}$ ，也就是复数的共轭表示一个相反的旋转（请牢记这一条性质，尤其是在推导四元数表示三维旋转时非常有用）。

C++代码实现

#include <iostream>
#include <cmath>
#include <vector>

// #include <Eigen/Core>
#include <eigen3/Eigen/Core>

#define MATH_PI 3.14159265358979323846264338327950288419716939937510L

template <typename T>
struct Point2D {
  T x = 0;
  T y = 0;
};
typedef Point2D<float> Point2DF;
typedef Point2D<double> Point2DD;

typedef struct {
    Point2DF center;
    float length;
    float width;
    float theta; //rad, (-pi,pi]
} ST_BOX_INFO;

typedef struct {
    Point2DF a;
    Point2DF b;
    Point2DF c;
    Point2DF d;
} ST_BOX_FOUR_VERTICES;

void RotateBoxVerticesMethod1(const ST_BOX_INFO& origin_box, ST_BOX_FOUR_VERTICES& rotated_box);
void RotateBoxVerticesMethod2(const ST_BOX_INFO& origin_box, ST_BOX_FOUR_VERTICES& rotated_box);

int main(void) {
    ST_BOX_INFO origin_box;
    origin_box.center.x = 4;
    origin_box.center.y = 3;
    origin_box.length = 4;
    origin_box.width = 2;
    origin_box.theta = 0.5 * MATH_PI;
    // origin_box.theta = 0.5 * 0.5 * MATH_PI;

    ST_BOX_FOUR_VERTICES rotated_box1, rotated_box2;
    RotateBoxVerticesMethod1(origin_box, rotated_box1);
    RotateBoxVerticesMethod2(origin_box, rotated_box2);

    return 0;
}

void RotateBoxVerticesMethod1(const ST_BOX_INFO& origin_box, ST_BOX_FOUR_VERTICES& rotated_box) {
    Eigen::MatrixXd R = Eigen::MatrixXd::Zero(8, 8);
    Eigen::VectorXd t(8);
    Eigen::VectorXd vertices(8);

    const auto l_half = 0.5 * origin_box.length;
    const auto w_half = 0.5 * origin_box.width;
    auto theta = origin_box.theta;
    if (1.0e-6 > (MATH_PI - std::fabs(theta))) {
        theta = 0.0;
    } else if (1.0e-6 > std::fabs((0.5 * MATH_PI) - std::fabs(theta))) {
        ;
    } else if ((0.5 * MATH_PI) < theta) {
        theta = theta - MATH_PI;
    } else if ((-0.5 * MATH_PI) > theta) {
        theta = theta + MATH_PI;
    }

    rotated_box.a.x = origin_box.center.x + l_half;
    rotated_box.a.y = origin_box.center.y + w_half;
    rotated_box.b.x = origin_box.center.x - l_half;
    rotated_box.b.y = origin_box.center.y + w_half;
    rotated_box.c.x = origin_box.center.x - l_half;
    rotated_box.c.y = origin_box.center.y - w_half;
    rotated_box.d.x = origin_box.center.x + l_half;
    rotated_box.d.y = origin_box.center.y - w_half;
    std::cout << "before rotated: (" << rotated_box.a.x << ',' << rotated_box.a.y << ')'
             << '(' << rotated_box.b.x << ',' << rotated_box.b.y << ')'
             << '(' << rotated_box.c.x << ',' << rotated_box.c.y << ')'
             << '(' << rotated_box.d.x << ',' << rotated_box.d.y << ')' << std::endl;

    R(0, 0) = R(1, 1) = R(2, 2) = R(3, 3) = R(4, 4) = R(5, 5) = R(6, 6) = R(7, 7) = std::cos(theta);
    R(1, 0) = R(3, 2) = R(5, 4) = R(7, 6) = std::sin(theta);
    R(0, 1) = R(2, 3) = R(4, 5) = R(6, 7) = -R(1, 0);

    t << origin_box.center.x, origin_box.center.y, origin_box.center.x, origin_box.center.y,
        origin_box.center.x, origin_box.center.y, origin_box.center.x, origin_box.center.y;

    vertices << rotated_box.a.x, rotated_box.a.y,
                rotated_box.b.x, rotated_box.b.y,
                rotated_box.c.x, rotated_box.c.y,
                rotated_box.d.x, rotated_box.d.y;

    const Eigen::VectorXd rslt = R * (vertices - t) + t;

    rotated_box.a.x = rslt(0);
    rotated_box.a.y = rslt(1);
    rotated_box.b.x = rslt(2);
    rotated_box.b.y = rslt(3);
    rotated_box.c.x = rslt(4);
    rotated_box.c.y = rslt(5);
    rotated_box.d.x = rslt(6);
    rotated_box.d.y = rslt(7);
    std::cout << "after rotated: (" << rotated_box.a.x << ',' << rotated_box.a.y << ')'
             << '(' << rotated_box.b.x << ',' << rotated_box.b.y << ')'
             << '(' << rotated_box.c.x << ',' << rotated_box.c.y << ')'
             << '(' << rotated_box.d.x << ',' << rotated_box.d.y << ')' << std::endl;
}

void RotateBoxVerticesMethod2(const ST_BOX_INFO& origin_box, ST_BOX_FOUR_VERTICES& rotated_box) {
    auto theta = origin_box.theta;
    if (1.0e-6 > (MATH_PI - std::fabs(theta))) {
        theta = 0.0;
    } else if (1.0e-6 > std::fabs((0.5 * MATH_PI) - std::fabs(theta))) {
        ;
    } else if ((0.5 * MATH_PI) < theta) {
        theta = theta - MATH_PI;
    } else if ((-0.5 * MATH_PI) > theta) {
        theta = theta + MATH_PI;
    }

    Eigen::Vector2d direction(std::cos(theta), std::sin(theta));
    Eigen::Vector2d orthog_dir(-direction.y(), direction.x());
    Eigen::Vector2d delta_x = 0.5 * origin_box.length * direction;
    Eigen::Vector2d delta_y = 0.5 * origin_box.width * orthog_dir;
    Eigen::Vector2d center = Eigen::Vector2d(origin_box.center.x, origin_box.center.y);
    std::vector<Eigen::Vector2d> vertices(4);
    vertices[0] = center + (delta_x + delta_y);
    vertices[1] = center + (-delta_x + delta_y);
    vertices[2] = center + (-delta_x - delta_y);
    vertices[3] = center + (delta_x - delta_y);
    
    rotated_box.a.x = vertices[0](0);
    rotated_box.a.y = vertices[0](1);
    rotated_box.b.x = vertices[1](0);
    rotated_box.b.y = vertices[1](1);
    rotated_box.c.x = vertices[2](0);
    rotated_box.c.y = vertices[2](1);
    rotated_box.d.x = vertices[3](0);
    rotated_box.d.y = vertices[3](1);
    std::cout << "after rotated: (" << rotated_box.a.x << ',' << rotated_box.a.y << ')'
             << '(' << rotated_box.b.x << ',' << rotated_box.b.y << ')'
             << '(' << rotated_box.c.x << ',' << rotated_box.c.y << ')'
             << '(' << rotated_box.d.x << ',' << rotated_box.d.y << ')' << std::endl;
}

若是要计算三维长方体的的八个顶点（已知中心点、长宽高和旋转角度，且pitch、roll角恒为0°），则用以上同样的方法先计算底部长方形的四顶点坐标，然后再将长方体的高累加到四顶点坐标对应轴上，即可得到顶部长方形四顶点的坐标。

其它方法可在评论区留言补充。