スポンサーサイト

-------- | --:--

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

回転行列の分解

2008-02-01 | 22:59

別なエントリーで、あるオブジェクトの法線ベクトルを特定の方向に傾けて、その後法線ベクトル中心に回転させたらどういう姿勢(回転行列)になるかを 考えた。
今度は、このような2段階の回転による最終的な姿勢(一番右の図)の回転行列がわかっている場合に、法線ベクトルの傾け方(回転軸と回転角)と法線ベクトル中心の回転角 を求めることを考える。

最終的な回転行列をP
法線ベクトルを傾ける回転行列をA
法線ベクトル中心の回転行列をB

とすると、 元の姿勢(基本姿勢)の法線ベクトル(Y軸方向)の向きv1は(0,1,0)であり、Aの回転後のY軸方向の向きはv2(P.n12, P.n22, P.n32)である。(AとPのY軸は同じ方向なので)
二つの内積はP.n22になるので、法線ベクトルを傾けるときの回転角は arccos(P.n22)。またそのときの回転軸はv1とv2の外積から求まる。
回転軸と回転角がわかったので、回転行列Aがわかる。

また、Aの回転を作用させた後、Aの(グローバル座標ではなく)ローカル座標のY軸を中心とした回転を作用させた後の結果がPなので
AB = P
である。よって B = (Aの逆行列) x P
これでBも求まる。
さらに、BはY軸を中心とした回転行列なので成分は
| cos, 0, -sin |
| 0, 1, 0 |
| sin, 0, cos|
のはずである。
cos(t) = B.n11
sin(t) = B.n31
から t (=Y軸中心の回転角)が求まる。

下の赤いキューブはマウスドラッグで回転できます。回転後(マウスを離した後)、この方法で求めた回転角を使って2段階に回転させる様子がアニメーション 表示されます。(アニメーション表示中は赤いキューブは回転できません)

package {
    import flash.display.*;
    import flash.events.*;
    import flash.geom.*;
    import flash.text.*;

    import org.papervision3d.core.*;
    import org.papervision3d.core.geom.*;
    import org.papervision3d.core.math.*;
    import org.papervision3d.scenes.* ;
    import org.papervision3d.objects.*;
    import org.papervision3d.objects.primitives.*;
    import org.papervision3d.cameras.*;
    import org.papervision3d.materials.*;
    import org.papervision3d.materials.utils.*;
    import org.papervision3d.view.*;
    import org.papervision3d.render.*;
    import org.papervision3d.materials.special.LineMaterial ;

    [SWF(width="600", height="400", frameRate="10", backgroundColor="#ffffff")]
        public class Mat2rad extends Sprite{
            private var renderer:BasicRenderEngine = new BasicRenderEngine();
            private var viewport:Viewport3D = new Viewport3D(600, 800, false, true);
            private var scene:Scene3D = new Scene3D();
            private var camera:FreeCamera3D = new FreeCamera3D();

            private var mouseDown:Boolean = false;
            private var mousePos:Point=null;
            private var pose:Matrix3D;

            private var vl:Lines3D = new Lines3D(new LineMaterial(0xff));

            private var redCube:Cube;
            private var redCube2:Cube=null;
            private var lines:Array = null;
            private var tf:TextField;
            private var tf2:TextField;

            private var rad:Number;
            private var rad2:Number;
            private var rv:Number3D;

            private var cubes:Array = [];
            private var processing = false;

            public function Mat2rad():void
            {
                tf = new TextField();
                addChild(tf);
                tf2 = new TextField();
                tf2.y = 200;
                addChild(tf2);

                stage.align = StageAlign.TOP_LEFT;
                stage.scaleMode = StageScaleMode.NO_SCALE;

                //viewport.y = 50;
                addChild(viewport);

                camera.y = -300;
                camera.z = -2100;
                camera.x = 200;
                camera.focus = 2100;
                camera.zoom = 1;

                var redwire:WireframeMaterial = new WireframeMaterial( 0xff0000 );
                redCube = new Cube(new MaterialsList({all:redwire}), 100, 200, 300);
                cubes.push(redCube);
                scene.addChild(redCube);
                showVector();

                renderer.renderScene(scene, camera, viewport);

                stage.addEventListener(MouseEvent.MOUSE_DOWN, function(e:*):void{
                        if(!processing){
                        mouseDown = true;
                        mousePos = new Point(stage.mouseX, stage.mouseY);
                        pose = Matrix3D.clone(redCube.transform);
                        }
                        });
                stage.addEventListener(MouseEvent.MOUSE_MOVE, rotate);
                stage.addEventListener(MouseEvent.MOUSE_UP, function(e:*):void{
                        if(!processing && mousePos){
                        mouseDown = false;
                        processing = true;
                        animation();
                        }
                        });
            }

            private function rotate(e:*):void{
                if(mouseDown && !processing){
                    var xv:int = stage.mouseX - mousePos.x;
                    var yv:int = stage.mouseY - mousePos.y;

                    // X方向(Y軸周り)に回転する回転行列
                    var my:Matrix3D = Matrix3D.rotationMatrix(0,1,0, -0.01*xv);
                    // Y方向(X軸周り)に回転する回転行列
                    var mx:Matrix3D = Matrix3D.rotationMatrix(1,0,0, -0.01*yv);
                    // 回転の合成
                    var m:Matrix3D = Matrix3D.multiply(mx,my);

                    redCube.transform = Matrix3D.multiply(m, pose);

                    renderer.renderScene(scene, camera, viewport);

                    showVector();
                }
            }

            private function showVector():void{

                if(lines != null){
                    for each(var l:Lines3D in lines){
                        scene.removeChild(l);
                    }
                }
                lines = [];

                for each (var cube:Cube in cubes){

                    var m:Matrix3D = cube.transform;

                    var xl:Lines3D = new Lines3D(new LineMaterial());
                    scene.addChild(xl);
                    xl.addNewLine(2, cube.x,cube.y,cube.z,
                            cube.x + m.n11*100, cube.y + m.n21*100 , cube.z + m.n31*100);
                    lines.push(xl);

                    var yl:Lines3D =  new Lines3D(new LineMaterial(0xff));
                    scene.addChild(yl);
                    yl.addNewLine(2, cube.x,cube.y,cube.z,
                            cube.x + m.n12*100, cube.y + m.n22*100 , cube.z + m.n32*100);
                    lines.push(yl);

                    var zl:Lines3D = new Lines3D(new LineMaterial(0xff00));
                    scene.addChild (zl);
                    zl.addNewLine(2, cube.x,cube.y,cube.z,
                            cube.x + m.n13*100, cube.y + m.n23*100 , cube.z + m.n33*100);
                    lines.push(zl);
                }

                renderer.renderScene(scene, camera, viewport);

            }

            private function animation():void{
                if(redCube2 == null){

                    redCube2 = new Cube(new MaterialsList({all:new WireframeMaterial()}), 100, 200, 300);
                    cubes.push(redCube2);
                    scene.addChild(redCube2);
                }

                // 最終姿勢
                var m:Matrix3D = redCube.transform;

                // 回転角1 
                rad = Math.acos(m.n22);

                // 回転軸ベクトル
                rv= Number3D.cross( new Number3D(m.n12, m.n22, m.n32), new Number3D(0,1,0));
                rv.normalize();

                // 法線ベクトルを合わせる回転行列
                var ma:Matrix3D = Matrix3D.rotationMatrix(rv.x, rv.y, rv.z, rad);

                // 法線ベクトル中心の回転行列
                var mb:Matrix3D = Matrix3D.multiply(Matrix3D.inverse(ma), m);

                // 回転角2 
                rad2 = Math.acos(mb.n11);
                if(mb.n31 < 0)
                    rad2 *= -1;

                tf.text = "rad1 = " + rad.toString();
                tf2.text = "rad2 = " + rad2.toString();

                // 基本姿勢に戻す
                redCube2.transform = new Matrix3D();
                redCube2.x = 400;
                renderer.renderScene(scene, camera, viewport);

                var sl:Slerp = new Slerp();
                sl.fromV = new Number3D(0,1,0);
                m = Matrix3D.rotationMatrix(rv.x, rv.y, rv.z, rad);
                sl.toV = new Number3D(m.n12, m.n22, m.n32);
                sl.callback = showCube;
                sl.onComplete = animation2;
                sl.time = 2;
                sl.start();
            }

            private function showCube(q:Object, p:*):void{
                redCube2.transform = Matrix3D.quaternion2matrix(q.x, q.y, q.z, q.w);
                redCube2.x = 400;
                renderer.renderScene(scene, camera, viewport);
                showVector();
            }

            private function animation2():void{
                var sl:Slerp = new Slerp();
                // 法線ベクトルを合わせる回転を行った後の姿勢
                var m:Matrix3D = Matrix3D.rotationMatrix(rv.x, rv.y, rv.z, rad);

                sl.fromV = new Number3D(m.n12, m.n22, m.n32);
                sl.toV = new Number3D(m.n12, m.n22, m.n32);
                sl.callback = showCube2;
                sl.time = 2;
                sl.rad = rad2;
                sl.onComplete = onComplete;
                sl.start();
            }

            private function showCube2(q:Object, p:*):void{
                // 法線ベクトルを合わせる回転を行った後の姿勢
                var m:Matrix3D = Matrix3D.rotationMatrix(rv.x, rv.y, rv.z, rad);

                var m2:Matrix3D =  Matrix3D.quaternion2matrix(q.x, q.y, q.z, -q.w);

                redCube2.transform = Matrix3D.multiply(m2, m);
                redCube2.x = 400;
                renderer.renderScene(scene, camera, viewport);
                showVector();
            }

            private function onComplete():void{
                processing = false;
                mousePos = null;
            }
        }
}

import flash.events.*;
import flash.utils.*;

import org.papervision3d.core.math.*;
import org.papervision3d.objects.primitives.*;

internal class Slerp {
    private var timer:Timer;
    private var cnt:int = 0;
    // 元の姿勢のクォータニオン
    private var qSrc:Object = {x:0, y:0, z:0, w:0};
    // 移動後の姿勢のクォータニオン
    private var qDst:Object = {x:0, y:0, z:0, w:0};

    // 変換前の位置
    public var fromPos:Number3D = new Number3D(0,0,0);
    // 変換前の姿勢の法線ベクトル
    public var fromV:Number3D = new Number3D(0,0,0);
    // 変換後の位置
    public var toPos:Number3D = new Number3D(0,0,0);
    // 変換後の姿勢の法線ベクトル
    public var toV:Number3D = new Number3D(0,0,0);
    // 法線ベクトルに対する回転角
    public var rad:Number=0;

    public var callback=null;
    public var onComplete=null;

    public var time:Number=1;

    public function Slerp():void
    {
    }

    public function start():void{

        fromV.normalize();
        toV.normalize();

        if(fromV.modulo == 0 || toV.modulo == 0){
            trace("fromV.modulo or toV.module == 0");
            return;
        }

        // 二つのベクトルから法線ベクトルをあわせるための回転軸(二つのベクトルの外積)を求める
        var rv:Number3D = Number3D.cross(toV, fromV);
        rv.normalize();
        if(rv.modulo == 0){
            rv.x = 1;
        }

        if(fromV.x == toV.x && fromV.y == toV.y && fromV.z == toV.z){
            // 元の姿勢のクォータニオン
            qSrc = Matrix3D.axis2quaternion(0, 1, 0, 0);

            // 法線ベクトル中心の回転を表すクォータニオンが最終的な姿勢のクォータニオン
            qDst = Matrix3D.axis2quaternion(toV.x, toV.y, toV.z, rad);
            qDst = Matrix3D.normalizeQuaternion(qDst);
        }else{
            // 法線ベクトルをあわせるための回転角(内積のarccos)を求める
            var r:Number = Math.acos(Number3D.dot(fromV, toV));

            // 元の姿勢のクォータニオン
            qSrc = Matrix3D.axis2quaternion(rv.x, rv.y, rv.z, 0);
            qSrc = Matrix3D.normalizeQuaternion(qSrc);

            // 法線ベクトルをあわせるための回転を表すクォータニオン
            var q1:Object= Matrix3D.axis2quaternion(rv.x, rv.y, rv.z, r);

            // 法線ベクトル中心の回転を表すクォータニオン
            var q2:Object= Matrix3D.axis2quaternion(toV.x, toV.y, toV.z, rad);

            // 二つのクォータニオンを合成したものが最終的な姿勢のクォータニオン
            qDst = Matrix3D.multiplyQuaternion(q2,q1);
            qDst = Matrix3D.normalizeQuaternion(qDst);
        }

        timer = new Timer(100);
        timer.addEventListener(TimerEvent.TIMER, animation);
        timer.start();
        cnt = 0;
    }

    private function animation(e:Event):void{
        if(cnt++ < time*10){
            var t:Number = cnt/(time*10);

            var t1:Number = Math.sin((1-t)*Math.PI/2) / Math.sin(Math.PI/2);
            var t2:Number = Math.sin(t*Math.PI/2) / Math.sin(Math.PI/2);
            var q:Object = {};
            q.x = t1*qSrc.x + t2*qDst.x;
            q.y = t1*qSrc.y + t2*qDst.y;
            q.z = t1*qSrc.z + t2*qDst.z;
            q.w = t1*qSrc.w + t2*qDst.w;

            q = Matrix3D.normalizeQuaternion(q);

            var pos:Number3D = new Number3D(fromPos.x + (toPos.x - fromPos.x)*t, 
                    fromPos.y + (toPos.y - fromPos.y)*t, 
                    fromPos.z + (toPos.z - fromPos.z)*t);

            if(callback != null)
                callback(q,pos);
        }else{
            timer.stop();
            timer.removeEventListener(TimerEvent.TIMER, animation);
            timer = null;
            if(onComplete != null)
                onComplete();
        }
    }
}

スポンサーサイト

Comment

Post a comment

Secret

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。