c语言编写载体坐标系到载体坐标系的转换并输出载体坐标系

假设我们需要将一个点在机体坐标系下的坐标(x,y,z)转换成载体坐标系下的坐标(x',y',z')，转换的公式如下：

x' = x * cos(a) * cos(b) + y * cos(a) * sin(b) + z * sin(a)
y' = -x * sin(b) + y * cos(b)
z' = -x * sin(a) * cos(b) - y * sin(a) * sin(b) + z * cos(a)

其中a和b分别是机体坐标系下的俯仰角和横滚角。为了让代码更易读，我们可以定义一个结构体来存储点的坐标和机体坐标系下的姿态：

typedef struct {
    double x;
    double y;
    double z;
} Point3D;

typedef struct {
    double pitch;  // 俯仰角
    double roll;   // 横滚角
} Attitude;

然后，我们可以编写一个函数来进行坐标系转换：

void convertCoordinate(Point3D* p, Attitude* attitude) {
    double a = attitude->pitch;
    double b = attitude->roll;
    double x = p->x;
    double y = p->y;
    double z = p->z;

    p->x = x * cos(a) * cos(b) + y * cos(a) * sin(b) + z * sin(a);
    p->y = -x * sin(b) + y * cos(b);
    p->z = -x * sin(a) * cos(b) - y * sin(a) * sin(b) + z * cos(a);
}

最后，我们可以在主函数中调用该函数来进行坐标系转换，并输出载体坐标系下的坐标：

int main() {
    Point3D p = {1.0, 2.0, 3.0};
    Attitude attitude = {30.0, 45.0};

    convertCoordinate(&p, &attitude);

    printf("The converted coordinate is (%f,%f,%f) in the carrier coordinate system.\n", p.x, p.y, p.z);

    return 0;
}

这样就完成了从机体坐标系到载体坐标系的转换，并输出了载体坐标系下的坐标。