Haskellで3次元座標の回転を計算するプログラム

前回　C#で立体を描く簡易的なプログラム~その2~　の中でC#で3次元座標を回転させるプログラムを作りましたが、今回はHaskellで作ってみました。

プログラム

rot.hs

data Matrix n = M n --[[n,n,n],[n,n,n],[n,n,n]]

instance Functor Matrix where
    fmap func (M a)= M (func a)

instance Applicative Matrix where
    pure a= M a

    (M func) <*> a = fmap func a

instance Monad Matrix where
    return a = M a
    M a >>= f = f a

main::IO()
main = do

    let add ml1 ml2 = return (zipWith (zipWith (+)) ml1 ml2)
    let product ml2 ml1 = let ml1_1 = ml1 !! 0
                              ml1_2 = ml1 !! 1
                              ml1_3 = ml1 !! 2
                              ml2_1 = ml2 !! 0
                              ml2_2 = ml2 !! 1
                              ml2_3 = ml2 !! 2

                              m1_11 = ml1_1 !! 0
                              m1_12 = ml1_1 !! 1
                              m1_13 = ml1_1 !! 2
                              m1_21 = ml1_2 !! 0
                              m1_22 = ml1_2 !! 1
                              m1_23 = ml1_2 !! 2
                              m1_31 = ml1_3 !! 0
                              m1_32 = ml1_3 !! 1
                              m1_33 = ml1_3 !! 2

                              m2_11 = ml2_1 !! 0
                              m2_12 = ml2_1 !! 1
                              m2_13 = ml2_1 !! 2
                              m2_21 = ml2_2 !! 0
                              m2_22 = ml2_2 !! 1
                              m2_23 = ml2_2 !! 2
                              m2_31 = ml2_3 !! 0
                              m2_32 = ml2_3 !! 1
                              m2_33 = ml2_3 !! 2

                              a11 = (m1_11 * m2_11) + (m1_12 * m2_21) + (m1_13 * m2_31)
                              a12 = (m1_11 * m2_12) + (m1_12 * m2_22) + (m1_13 * m2_32)
                              a13 = (m1_11 * m2_13) + (m1_12 * m2_23) + (m1_13 * m2_33)
                              a21 = (m1_21 * m2_11) + (m1_22 * m2_21) + (m1_23 * m2_31)
                              a22 = (m1_21 * m2_12) + (m1_22 * m2_22) + (m1_23 * m2_32)
                              a23 = (m1_21 * m2_13) + (m1_22 * m2_23) + (m1_23 * m2_33)
                              a31 = (m1_31 * m2_11) + (m1_32 * m2_21) + (m1_33 * m2_31)
                              a32 = (m1_31 * m2_12) + (m1_32 * m2_22) + (m1_33 * m2_32)
                              a33 = (m1_31 * m2_13) + (m1_32 * m2_23) + (m1_33 * m2_33)
                          in
                              return [[a11,a12,a13],[a21,a22,a23],[a31,a32,a33]]
    let theta_x t = let theta = (t/180)*pi
                    in
                        [[1,0,0],[0,cos (theta),-sin (theta)],[0,sin (theta),cos (theta)]]
    let theta_y t = let theta = (t/180)*pi
                    in
                        [[cos (theta),0,sin (theta)],[0,1,0],[-sin (theta),0,cos (theta)]]
    let theta_z t = let theta = (t/180)*pi
                    in
                        [[cos (theta),-sin (theta),0],[sin (theta),cos (theta),0],[0,0,1]]

    let prt (M a) = a

    let mtrx = [[1,0,0],
                [2,0,0],
                [3,0,0]]

    let ans = return (theta_x 25) >>= product (theta_y 15) >>= product (theta_z 35) >>= product mtrx

    print (prt ans)

解説

data Matrix n = M n

行列を表しています。今回は3✕3の行列のみを想定しています。

instance Functor Matrix where
    fmap func (M a)= M (func a)

instance Applicative Matrix where
    pure a= M a

    (M func) <*> a = fmap func a

instance Monad Matrix where
    return a = M a
    M a >>= f = f a

FunctorとApplicativeとMonadのインスタンスです。

次にmain関数の中身です。

let add ml1 ml2 = return (zipWith (zipWith (+)) ml1 ml2)

2つの行列の和を計算します。

let product ml2 ml1 = ~~~

行列の積を計算します。

    let theta_x t = let theta = (t/180)*pi
                    in
                        [[1,0,0],[0,cos (theta),-sin (theta)],[0,sin (theta),cos (theta)]]
    let theta_y t = let theta = (t/180)*pi
                    in
                        [[cos (theta),0,sin (theta)],[0,1,0],[-sin (theta),0,cos (theta)]]
    let theta_z t = let theta = (t/180)*pi
                    in
                        [[cos (theta),-sin (theta),0],[sin (theta),cos (theta),0],[0,0,1]]

これらの関数はそれぞれX軸の回転角度、Y軸の回転角度、Z軸の回転角度を引数に取り、その回転行列を返します。

let ans = return (theta_x 25) >>= product (theta_y 15) >>= product (theta_z 35) >>= product mtrx

この部分で、X軸を25度、Y軸を15度、Z軸を35度回転させ、元の座標のどの位置に点が移動したかを演算します。
3✕3の行列しか対応していないため、座標上の点を表す行列mtrxも3✕3になっています。その代わり、3点を同時に計算できる・・・。

実行結果は以下の通りです。

実行結果

[[0.4596326640991393,0.0,0.0],[0.7441135892720026,0.0,0.0],[3.637998457992233,0.0,0.0]]

(0.459 , 0.744 , 3.637)の位置に移動したということになります。