BLASを使って3次元の回転を計算する

?rot_

CGなどで座標を回転させたいときは行列を使って計算できますが、行列演算ライブラリのBLASを使って実装する方法です。BLASのAPIは以下のようになってますが、使いやすくしたcblas.hもありそれぞれ以下のように定義されてます。

void srot_(int *n, float *x, int *incx, float *y, int *incy, float *c, float *s);
void drot_(int *n, double *x, int *incx, double *y, int *incy, double *c, double *s);

cblas.h

void cblas_srot(int n, float *x, int incx, float *y, int incy, float c, float s);
void cblas_drot(int n, double *x, int incx, double *y, int incy, double c, double s);

どちらも実行結果は同じで、計算結果はxとyに直接上書きされます。
式としては

\pmatrix{ x_{1} &x_{2} &\cdots &x_{n} \\ y_{1} &y_{2} &\cdots &y_{n} } \leftarrow \pmatrix{ cos(\theta) &sin(\theta) \\ -sin(\theta) &cos(\theta)  } \cdot \pmatrix{ x_{1} &x_{2} &\cdots &x_{n} \\ y_{1} &y_{2} &\cdots &y_{n} }

と同等の計算です。

引数の意味はそれぞれ

引数	io	意味
n	in	xとyの要素数
x	in,out	入力として$(x_1 \cdots x_n)$を受け取り結果が格納される
incx	in	$x$の配列の次の要素にアクセスするための増加量。1ならx[0]→x[1]→x[2]とアクセスしていく。2ならx[0]→x[2]となる。
y	in,out	入力として$(y_z \cdots y_n)$を受け取り結果が格納される
incy	in	$y$の配列の次の要素にアクセスするための増加量。1ならy[0]→y[1]とアクセスしていく。2ならy[0]→y[2]となる。
c	in	$cos(\theta)$の値
s	in	$sin(\theta)$の値

nが1なら以下の式になります。

\pmatrix{ x \\ y } \leftarrow \pmatrix{ cos(\theta) &sin(\theta) \\ -sin(\theta) &cos(\theta) } \cdot \pmatrix{ x \\ y }

回転の仕方としては、カメラが$\theta$回転したときの座標のイメージです。

使ってみる

2次元の回転

まずは２次元でx=1, y=1を$\frac{\pi}{4}$回転する場合。

 \pmatrix{ cos(\frac{\pi}{4}) &sin(\frac{\pi}{4}) \\ -sin(\frac{\pi}{4}) &cos(\frac{\pi}{4}) } \cdot \pmatrix{ 1 \\ 1 } = \pmatrix{ \sqrt{2} \\ 0 } \approx \pmatrix { 1.414214 \\ 0 }

になります。
リンクするライブラリはlibblas.soです。

main.c

#include <stdio.h>
#include <cblas.h>
#define _USE_MATH_DEFINES
#include <math.h>

int main()
{
    double x = 1, y = 1;
    cblas_drot( 1, &x, 1, &y, 1, cos(M_PI/4), sin(M_PI/4) );
    printf( "x:%lf, y:%lf\n", x, y );
    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( cblas_drot_test )
add_executable( cblas_drot_test main.c )
target_link_libraries( cblas_drot_test m blas )

3次元でy軸周りに回転

3次元空間で回転する場合$x$, $y$, $z$の3つの軸で回転できます。例として$y$軸周りに回転する場合、以下の式になります。

\pmatrix{ x \\ y \\ z } \leftarrow \pmatrix{ cos(\theta) &0 &-sin(\theta) \\ 0 &1 &0 \\ sin(\theta) &0 &cos(\theta)  } \cdot \pmatrix{ x \\ y \\ z }

2次元と同じように$\frac{\pi}{4}$回転する場合。

main.c

#include <stdio.h>
#include <cblas.h>
#define _USE_MATH_DEFINES
#include <math.h>

int main()
{
    double p[3] = { 1, 1, 1 };
    cblas_drot( 1, &p[2], 1, &p[0], 1, cos(M_PI/4), sin(M_PI/4) );
    printf( "x:%lf, y:%lf, z:%lf\n", p[0], p[1], p[2] );
    return 0;
}

CMakeLists.txtは同じです。

行列同士の積

配列で管理している行列を回転したい場合一工夫必要になります。特にBLASは行列を配列として扱う時にC言語だとちょっと不思議に見える管理をしてるので他のAPIも使うつもりなら注意がいります。

具体的には配列double m[9]があった場合、3行3列の行列$M$の要素とは以下のように対応付けられます。

M = \pmatrix{ m[0] &m[3] &m[6] \\ m[1] &m[4] &m[7] \\ m[2] &m[5] &m[8] }

配列の次の要素が行方向に順番に増えていくことになります。C言語だと列方向に順番に増えていくことが多いのでここがポイントになります。

そこで以下のような$y$軸周りの回転を計算したい場合、

\pmatrix{ x'_{1} &x'_{2} &x'_{3} \\ y'_{1} &y'_{2} &y'_{3} \\ z'_{1} &z'_{2} &z'_{3} } = \pmatrix{ cos(\theta) &0 &-sin(\theta) \\ 0 &1 &0 \\ sin(\theta) &0 &cos(\theta)  } \cdot \pmatrix{ x_{1} &x_{2} &x_{3} \\ y_{1} &y_{2} &y_{3} \\ z_{1} &z_{2} &z_{3} }

C言語的には

\pmatrix{ m[0] &m[3] &m[6] \\ m[1] &m[4] &m[7] \\ m[2] &m[5] &m[8] } \leftarrow  \pmatrix{ cos(\theta) &0 &-sin(\theta) \\ 0 &1 &0 \\ sin(\theta) &0 &cos(\theta)  } \cdot \pmatrix{ m[0] &m[3] &m[6] \\ m[1] &m[4] &m[7] \\ m[2] &m[5] &m[8] }

と計算したいことになります。
この場合incxとincyを利用します。

例として単位行列を回転する

\pmatrix{ cos(\theta) &0 &-sin(\theta) \\ 0 &1 &0 \\ sin(\theta) &0 &cos(\theta)  } \cdot \pmatrix{1 &0 &0 \\ 0 &1 &0 \\ 0 &0 &1 } = \pmatrix{ cos(\theta) &0 &-sin(\theta) \\ 0 &1 &0 \\ sin(\theta) &0 &cos(\theta)  }

を計算してみます。

main.c

#include <stdio.h>
#include <cblas.h>
#define _USE_MATH_DEFINES
#include <math.h>

int main()
{
  double m[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
  cblas_drot( 3, &m[2], 3, &m[0], 3, cos(M_PI/4), sin(M_PI/4) );
  printf( "% lf % lf % lf\n", m[0], m[3], m[6] );
  printf( "% lf % lf % lf\n", m[1], m[4], m[7] );
  printf( "% lf % lf % lf\n", m[2], m[5], m[8] );
  return 0;
}

incxとincyに3を入れることでxはm[0]→m[3]→m[6]、yはm[2]→m[5]→m[8]とアクセスしていきます。