三维空间上的旋转

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/sinat_20280061/article/details/52025762

    #include <stdio.h>
    #include <stdlib.h>
    #include <math.h>

    typedef struct point{  //二维平面的点
        double x;
        double y;
    }Point;

    typedef struct point_3D{ //三维空间上的点
     double x;
     double y;
     double z;
    }point_3D;

    double ret[3][3] ={0}; //存储矩阵的乘积

    /** \brief
     *  二维平面的转换
     * \param
     * \param
     * \return
     *
     */
    Point* Rot_2D(double x1, double y1, float ceta)
    {
        Point *p2 = (Point *)malloc(sizeof(Point));
        p2->x = cos(ceta)*x1 - sin(ceta)*y1;
        p2->y = cos(ceta)*y1 + sin(ceta)*x1;
        return p2;
    }

    /** \brief 矩阵乘法
     *
     * \param
     * \param
     * \return
     *
     */

    void matrix_multipy(double martix1[3][3], double martix2[3][3])
    {
     int i,j,k,l;
     for(i=0;i<3;i++) //前行数
     {
         for(j=0;j<3;j++) //后列数
         {
             ret[i][j]=0; //初始化为0，每次计算结果存储
             for(k=0;k<3;k++)
             {
                 ret[i][j] += (martix1[i][k] * martix2[k][j]);
             }
         }
     }
    }
    /** \brief 打印矩阵
     *
     * \param
     * \param
     * \return
     *
     */

    void output_martix(double martix[3][3])
    {
        int i,j;
        for(i=0;i<3;i++)
        {
            for(j=0;j<3;j++)
                printf("%.2lf\t",martix[i][j]);
            printf("\n");
        }
         printf("\n");
    }
    /** \brief 三维空间上的旋转变换
     *
     * \param
     * \param
     * \return
     *
     */

    point_3D *Rot_3D(double ceta1, double ceta2, double ceta3,double x1,double y1,double z1)
    {
        double rotZ[3][3] = {{cos(ceta3),-sin(ceta3),0},{sin(ceta3),cos(ceta3),0},{0,0,1}};
        double rotY[3][3] = {cos(ceta2),0,sin(ceta2),0,1,0,-sin(ceta2),0,cos(ceta2)};
        double rotX[3][3] = {1,0,0,0,cos(ceta1),-sin(ceta1),0,sin(ceta1),cos(ceta1)};
        /*
        output_martix(rotZ);
        output_martix(rotY);
        output_martix(rotX);
        */
        point_3D *rot_3D = (point_3D *)malloc(sizeof(point_3D));
        matrix_multipy(rotZ,rotY);
        double temp[3][3];
        memcpy(temp,ret,3*3*8); //保存中间值
        matrix_multipy(temp,rotX); //得到rot_3D
        rot_3D->x = ret[0][0]*x1 + ret[0][1]*y1 + ret[0][2]*z1;
        rot_3D->y = ret[1][0]*x1 + ret[1][1]*y1 + ret[1][2]*z1;
        rot_3D->z = ret[2][0]*x1 + ret[2][1]*y1 + ret[2][2]*z1;
        return rot_3D;
    }

    /** \brief 三维空间上直接套用公式
     *
     * \param
     * \param
     * \return
     *
     */

    point_3D *re(double ceta1, double ceta2, double ceta3,double x1, double y1, double z1)
    {
         point_3D *p3 = (point_3D *)malloc(sizeof(point_3D));
         p3->x = x1 * cos(ceta1)*cos(ceta2) + y1*(cos(ceta1)*sin(ceta2)*sin(ceta3)-sin(ceta1)*cos(ceta3)) + z1 *(sin(ceta1)*sin(ceta3)+cos(ceta1)*sin(ceta2)*cos(ceta3));
         p3->y = x1 * sin(ceta1)*cos(ceta2) + y1 *(cos(ceta1)*cos(ceta3)+ sin(ceta1)*sin(ceta2)*sin(ceta3)) + z1 *(sin(ceta1)*sin(ceta2)*cos(ceta3)-cos(ceta1)*sin(ceta3));
         p3->z = x1 * (-sin(ceta2)) + y1 *(cos(ceta2)*sin(ceta3)) +z1*(cos(ceta2)*cos(ceta3));
         return p3;
    }

    int main()
    {
        int sel;
        while(1){
            printf("select to ROT_2D(0) or ROT_3D(1) \n");
            scanf("%d",&sel);
            if(sel==0){
                printf("input p1->x p1->y the incuded angle between p1 and p2 ceta\n");
                double x1,y1;
                float ceta;
                scanf("%lf %lf %f",&x1, &y1, &ceta);
                Point *p2 = (Point *)malloc(sizeof(Point));
                p2 = Rot_2D(x1, y1, (float)M_PI/180*ceta);
                printf("P2->x = %lf, P2->y = %lf \n\n",p2->x,p2->y);
            }
           if(sel==1){
                printf("input p1->x1, p1->y1, p1->z1 and ceta1, ceta2, ceta3 \n");
                double x2,y2,z2;
                double ceta1,ceta2,ceta3;
                scanf("%lf %lf %lf %lf %lf %lf",&x2, &y2, &z2, &ceta1, &ceta2, &ceta3);
                point_3D *p3 = (point_3D *)malloc(sizeof(point_3D));
                p3 = Rot_3D(ceta1*M_PI/180,ceta2*M_PI/180,ceta3*M_PI/180,x2,y2,z2);
                //p3 = Rot_3D(M_PI/2, M_PI/2, -M_PI/2, 5, 5, 5);  //用例
                //p3 = re(M_PI/2,M_PI/2,-M_PI/2,5,5,5);   //直接套用公式
                printf("p2->x = %lf  p2->y = %lf  p2->z = %lf\n\n",p3->x,p3->y,p3->z);
            }
        }
        return 0;
    }