typescriptで三次元ベクトルを回転させてみた。

17479 ワード

概要

つい最近 typescript で3次元ベクトルを回転させる機会がありました。
計算式を書くのが結構面倒くさかったのでコピペで使えるように共有しておきます。

コード

/**
 * 3次元ベクトルの回転を行う。
 *
 * @param number vector_x
 * @param number vector_y
 * @param number vector_z
 * @param number angle_x
 * @param number angle_y
 * @param number angle_z
 * @return {x:number, y:number, z:number}
 */
const rotate3dVector = (
  vector_x: number,
  vector_y: number,
  vector_z: number,
  angle_x: number,
  angle_y: number,
  angle_z: number
): { x: number; y: number; z: number } => {
  // 角度→ラジアンに変換
  const razian_x = angle_x * (Math.PI / 180)
  const razian_y = angle_y * (Math.PI / 180)
  const razian_z = angle_z * (Math.PI / 180)

  // x軸周りに右回転した座標を取得する表現行列
  const matrix_x = [
    [1, 0, 0],
    [0, Math.cos(razian_x), -Math.sin(razian_x)],
    [0, Math.sin(razian_x), Math.cos(razian_x)],
  ]

  // // y軸周り右回転した座標を取得する表現行列
  const matrix_y = [
    [Math.cos(razian_y), 0, Math.sin(razian_y)],
    [0, 1, 0],
    [-Math.sin(razian_y), 0, Math.cos(razian_y)],
  ]

  // z軸周りに右回転した座標を取得する表現行列
  const matrix_z = [
    [Math.cos(razian_z), -Math.sin(razian_z), 0],
    [Math.sin(razian_z), Math.cos(razian_z), 0],
    [0, 0, 1],
  ]

  /**
   * 回転行列を使ってベクトルの回転を行う。
   *
   * @param number[][] matrix
   * @param number[] vector
   * @return {x:number, y:number, z:number}
   */
  const calc = (
    matrix: number[][],
    vector: number[]
  ): { x: number; y: number; z: number } => {
    return {
      x:
        matrix[0][0] * vector[0] +
        matrix[0][1] * vector[1] +
        matrix[0][2] * vector[2],
      y:
        matrix[1][0] * vector[0] +
        matrix[1][1] * vector[1] +
        matrix[1][2] * vector[2],
      z:
        matrix[2][0] * vector[0] +
        matrix[2][1] * vector[1] +
        matrix[2][2] * vector[2],
    }
  }

  // x軸回りの回転
  let rotational_vector = calc(matrix_x, [vector_x, vector_y, vector_z])
  // y軸回りの回転
  rotational_vector = calc(matrix_y, [
    rotational_vector.x,
    rotational_vector.y,
    rotational_vector.z,
  ])
  // z軸回りの回転
  rotational_vector = calc(matrix_z, [
    rotational_vector.x,
    rotational_vector.y,
    rotational_vector.z,
  ])

  return {
    x: rotational_vector.x,
    y: rotational_vector.y,
    z: rotational_vector.z,
  }
}

参考記事