回転行列から回転軸と回転角を求める

(3x3の)回転行列Mが分かっている時の回転軸ベクトルは
( M.n32 - M.n23, M.n13 - M.n31, M.n21 - M.n12)

回転角は
arccos((Trace(M) - 1 ) / 2)
4x4の同次座標の場合、(n44成分が1なので)回転角は
arccos((Trace(M) - 2 ) / 2)
で求まる。

(PV3DのDisplayObject3D.transformは同次座標)
(Traceは対角成分の和、PV3DのMatrix3Dにもtraceというプロパティがある)

下のキューブはマウスドラッグで回転できます。
回転後に上のやり方で回転軸と回転角を求めています。(黒い線が回転軸)
ESCキーで元の姿勢に戻ります。(検証のためにもとに戻す時は回転軸を使って逆回転させています。)

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 Mat2axis 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;
            private var pose:Matrix3D;

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

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

            public function Mat2axis():void
            {
                tf = new TextField();
                addChild(tf);

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

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

                camera.y = -300;
                camera.z = -1100;
                camera.x = 0;
                camera.focus = 1100;
                camera.zoom = 1;

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

                renderer.renderScene(scene, camera, viewport);

                stage.addEventListener(MouseEvent.MOUSE_DOWN, function(e:*):void{
                        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{
                        mouseDown = false;
                        });
                stage.addEventListener( KeyboardEvent.KEY_DOWN, function(e:*):void{
                        switch (e.keyCode){
                        case 27 :// ESC
                        var m:Matrix3D = redCube.transform;
                        // 回転軸ベクトル
                        var rv:Number3D = new Number3D( m.n32 - m.n23, m.n13 - m.n31, m.n21 - m.n12);
                        rv.normalize();

                        // 回転角
                        var rad:Number =  Math.acos((m.trace - 2 ) / 2);
                        // 逆回転して元の位置に戻す回転行列
                        var r:Matrix3D = Matrix3D.rotationMatrix(rv.x, rv.y, rv.z, -rad);
                        redCube.transform = Matrix3D.multiply(r, redCube.transform);

                        renderer.renderScene(scene, camera, viewport);

                        showVector();

                        break;
                        }
                        });
            }

            private function rotate(e:*):void{
                if(mouseDown){
                    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{
                var m:Matrix3D = redCube.transform;
                // 回転軸ベクトル
                var rv:Number3D = new Number3D( m.n32 - m.n23, m.n13 - m.n31, m.n21 - m.n12);

                // (正規化する前に)回転軸を描画
                if(vl != null)
                    scene.removeChild(vl);

                vl = new Lines3D(new LineMaterial(0x0));
                scene.addChild(vl);

                vl.addNewLine(1, 0,0,0, rv.x*200, rv.y*200 , rv.z*200);

                rv.normalize();

                // 回転角
                var rad:Number = Math.acos((m.trace - 2 ) / 2);
                tf.text = rad.toString();

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

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

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

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

                renderer.renderScene(scene, camera, viewport);
            }
        }
}