スポンサーサイト

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

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

[PV3D] クォータニオンを使った3次元での中間姿勢の補間

2008-01-29 | 00:00

前回3次元での姿勢の変換について書いたが、同じように変換前の姿勢と変換後の姿勢が分かっている場合に、中間の姿勢をクォータニオンを使って補間する事を考える。
移動前のクォータニオンをP1、最終的なクォータニオンをP2、回転角をwとすると途中時刻t(0~1)でのクォータニオンは
p(t) = (sin((1-t)*w) / sin(w))*P1 + (sin(t*w)/sin(w))*P2
で表される。
この式を利用するために最初のクォータニオンと最終的なクォータニオンを求める必要がある。
最初のクォータニオンは前回説明した法線ベクトルをあわせるための回転軸と回転角から作る事ができる。
最終的なクォータニオンは、法線ベクトルをあわせるためのクォータニオンと法線ベクトルを中心とした回転のクォータニオンを合成(積)したものである。
(行列の積の時と同じようにクォータニオン同士の積の場合も掛ける順番が重要。先に適用する方を後に持ってくる。)

マウスクリックで下のキューブの姿勢変更の様子がアニメーション表示されます。(上で説明したやり方で中間姿勢を求めています。)


PaperVision3D 2.0 GreatWhiteにはクォータニオンを表すクラスそのものはないが、Matrix3Dクラスのstaticメソッドとしてクォータニオン関連の演算が用意されている。
追記:クォータニオンクラスが無いというのは間違いです。core.math.Quaternionというのがありました。
下のソースはその事を知らずに書いたのでMatrix3Dクラス内のメソッドを使ってクォータニオン操作をしています。
package {
    import flash.display.*;
    import flash.events.*;
    import flash.utils.*;

    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.shadematerials.* ;
    import org.papervision3d.materials.utils.*;
    import org.papervision3d.view.*;
    import org.papervision3d.render.*;
    import org.papervision3d.lights.*;
    import org.papervision3d.materials.special.LineMaterial ;

    [SWF(width="600", height="400", frameRate="10", backgroundColor="#ffffff")]
        public class QuatVec 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 lines:Array = null;

            private var cube:Cube;

            private var timer:Timer=null;
            private var cnt:int = 0;
            // 元の姿勢のクォータニオン
            private var qSrc:Object;
            // 移動後の姿勢のクォータニオン
            private var qDst:Object;
            private var rr:Number;

            private var fromPos:Number3D;
            private var toPos:Number3D;

            public function QuatVec():void
            {
                stage.align = StageAlign.TOP_LEFT;
                stage.scaleMode = StageScaleMode.NO_SCALE;

                addChild(viewport);

                camera.y = 100;
                camera.z = -1100;
                camera.x = 200;
                camera.focus = 1100;
                camera.zoom = 1;
                camera.tilt(20);

                var greenwire:WireframeMaterial = new WireframeMaterial( 0xff00 );
                cube= new Cube(new MaterialsList({all:greenwire}), 100, 200, 300);
                scene.addChild(cube);

                showVector(cube);

                renderer.renderScene(scene, camera, viewport);

                stage.addEventListener(MouseEvent.MOUSE_DOWN, onClick);
            }

            private function onClick(e:*):void{

                var toV:Number3D;
                var pos:Number3D;
                var rad:Number;

                toV = new Number3D(1,1,1);
                pos = new Number3D(300,200,0);
                rad = Math.PI*2/3;

                cube.transform = new Matrix3D();
                renderer.renderScene(scene, camera, viewport);
                showVector(cube);
                
                pose(cube, toV, rad, pos);
                showVector(cube);
            }

            private function pose(o3d:DisplayObject3D, toV:Number3D, rad:Number, pos:Number3D):void{
                fromPos = new Number3D(o3d.x, o3d.y, o3d.z);
                toPos = pos;

                var fromV:Number3D = new Number3D(o3d.transform.n12, o3d.transform.n22, o3d.transform.n32);
                fromV.normalize();
                toV.normalize();

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

                // 法線ベクトルをあわせるための回転角(内積のarccos)を求める
                rr = 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, rr);

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

                // 二つのクォータニオンを合成したものが最終的な姿勢のクォータニオン
                qDst = Matrix3D.multiplyQuaternion(q2,q1);
                qDst = Matrix3D.normalizeQuaternion(qDst);
 
                if(timer != null){
                    timer.stop();
                    timer.removeEventListener(TimerEvent.TIMER, animation);
                    timer = null;
                }
                cnt = 0;
                timer = new Timer(50);
                timer.addEventListener(TimerEvent.TIMER, animation);
                timer.start();
            }

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

                    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);

                    cube.transform = Matrix3D.quaternion2matrix(q.x, q.y, q.z, q.w);

                    cube.x = fromPos.x + toPos.x*t;
                    cube.y = fromPos.y + toPos.y*t;
                    cube.z = fromPos.z + toPos.z*t;

                    showVector(cube);
                }else{
                    timer.stop();
                    timer.removeEventListener(TimerEvent.TIMER, animation);
                    timer = null;
                }
            }

            private function showVector(cube:Cube):void{
                if(lines != null){
                    for each(var l:Lines3D in lines){
                        scene.removeChild(l);
                    }
                }
                lines = [];

                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);
            }
        }
}



スポンサーサイト

Comment

Post a comment

Secret

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