C++で回転行列を計算する

angleとaxisを指定すれば計算できる。

#define _USE_MATH_DEFINES
#include <iostream>
#include <cmath>

void getRotationMatrix(double angle, double axis[3], double R[3][3]) {
    double x = axis[0], y = axis[1], z = axis[2];
    double c = cos(angle), s = sin(angle);
    double t = 1 - c;
    double tx = t * x, ty = t * y, tz = t * z;
    double txy = tx * y, txz = tx * z, tyz = ty * z;
    double sx = s * x, sy = s * y, sz = s * z;

    R[0][0] = c + tx * x;
    R[0][1] = txy + sz;
    R[0][2] = txz - sy;
    R[1][0] = txy - sz;
    R[1][1] = c + ty * y;
    R[1][2] = tyz + sx;
    R[2][0] = txz + sy;
    R[2][1] = tyz - sx;
    R[2][2] = c + tz * z;
}

int main() {
    double angle = M_PI / 3;
    double axis[3] = { 0, 0, 1 }; 

    double R[3][3];
    getRotationMatrix(angle, axis, R);

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            std::cout << R[i][j] << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}

コメント

タイトルとURLをコピーしました