スポンサーサイト

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

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

[ActionScript 3.0] クォータニオンを使った任意軸を中心とした回転

2007-08-14 | 21:59

軸のベクトルを(a,b,c)、 回転角をwとすると、この回転を表すクォータニオンQは
Q = (cos(w/2); a * sin(w/2), b * sin(w/2), c * sin(w/2))

共役クォータニオンRは
R = (cos(w/2); -a * sin(w/2), -b * sin(w/2), -c * sin(w/2))
となる。

(x,y,z)の座標の点Pをクォータニオンで表すと(0,x,y,z)となり、点Pを(a,b,c)の軸を中心にwだけ回転した後の座標は RPQ(RかけるPかけるQ)となる。

Rotate3DwithQuaternion.as
package{
    import flash.display.*;
    import flash.events.*;
    import flash.geom.*;
    import flash.ui.*;
    import flash.utils.*;
    [SWF(width="400", height="400",backgroundColor="0xffffff")]

        public class Rotate3DwithQuaternion extends Sprite{
            // camera position
            private var cameraX:Number = 100;
            private var cameraY:Number = 100;
            private var cameraZ:Number = -100;

            // view position
            private var viewX:Number = 0;
            private var viewY:Number = 0;
            private var viewZ:Number = 0;

            private var faceZ:Number = 100;

            private var rad:Number = Math.PI/20;

            private var ball1:Ball = new Ball(0xff0000,50,10,0);
            private var ball2:Ball = new Ball(0x00000f,10,10,10);

            private var rotateAxis:Vector3 = new Vector3(-1,1,-1);

            public function Rotate3DwithQuaternion(){
                addChild(ball1);
                addChild(ball2);

                graphics.lineStyle(1,0,0.3);
                var from:Point;
                var to:Point;
                // X axis
                from = view2screen(world2view(0,0,0));
                to = view2screen(world2view(200,0,0));
                graphics.moveTo(from.x, from.y);
                graphics.lineTo(to.x, to.y);
                // Y axis
                from = view2screen(world2view(0,0,0));
                to = view2screen(world2view(0,200,0));
                graphics.moveTo(from.x, from.y);
                graphics.lineTo(to.x, to.y);
                // Z axis
                from = view2screen(world2view(0,0,0));
                to = view2screen(world2view(0,0,200));
                graphics.moveTo(from.x, from.y);
                graphics.lineTo(to.x, to.y);

                rotateAxis.normalize();

                addEventListener(Event.ENTER_FRAME, loop);
            }

            private function rotate(ball:Ball):void{
                // 回転クォータニオン
                var q:Quaternion = Quaternion.rotateQuaternion(rad, rotateAxis);
                // 共役クォータニオン
                var qc:Quaternion = q.conjugation();
                // 位置クォータニオン
                var q2:Quaternion = new Quaternion();
                q2.w = 0;
                q2.x = ball.wx;
                q2.y = ball.wy;
                q2.z = ball.wz;

                var r:Quaternion = qc.multiply(q2);
                r = r.multiply(q);

                ball.wx = r.x;
                ball.wy = r.y;
                ball.wz = r.z;
                var view:Point = view2screen(world2view(ball.wx, ball.wy, ball.wz));
                ball.x = view.x;
                ball.y = view.y;

                ball.alpha = (ball.wx > 0)? 0.8 : 0.3;
            }

            private function loop(e:Event):void{
                rotate(ball1);
                rotate(ball2);
            }

            private function world2view(x:Number, y:Number, z:Number):Point{
                var xx:Number = (faceZ - cameraZ) * (x - cameraX) / (z - cameraZ) 
                    - (faceZ - cameraZ) * (viewX - cameraX) / (viewZ - cameraZ);
                var yy:Number =  (faceZ - cameraZ) * (y - cameraY) / (z - cameraZ)
                    -(faceZ - cameraZ)* (viewY - cameraY) / (viewZ - cameraZ);
                return new Point(xx,yy);
            }

            private function view2screen(p:Point):Point{
                return new Point(p.x + 200, 200 - p.y);
            }
        }
}

Quaternion.as
package{
    public class Quaternion{
        private var _w:Number;
        private var _x:Number;
        private var _y:Number;
        private var _z:Number;

        public function Quaternion(w:Number=0, v:Vector3=null){
            _w = w;
            if(v != null){
                _x = v.x;
                _y = v.y;
                _z = v.z;
            }
        }
        /*
         * 回転を表すクォータニオンを返す
         */
        public static function rotateQuaternion(r:Number, v:Vector3):Quaternion{
            var q:Quaternion = new Quaternion();
            var s:Number = Math.sin(r/2.0);
            q._w = Math.cos(r/2.0);
            q._x = v.x * s;
            q._y = v.y * s;
            q._z = v.z * s;
            return q;
        }
        /*
         * スカラー倍したクォータニオンを返す
         */
        public function multiplScalar(s:Number):Quaternion{
            trace("s=" + s);
            var q:Quaternion = new Quaternion();

            q._w = _w * s;
            q._x = _x * s;
            q._y = _y * s;
            q._z = _z * s;

            return q;
        }
        /*
         * 積を返す
         */
        public function multiply(o:Quaternion):Quaternion{
            var q:Quaternion = new Quaternion();

            q._w = _w * o._w - _x * o._x - _y * o._y - _z * o._z;
            q._x = _y * o._z - _z * o._y + _w * o._x + _x * o._w;
            q._y = _z * o._x - _x * o._z + _w * o._y + _y * o._w;
            q._z = _x * o._y - _y * o._x + _w * o._z + _z * o._w;

            return q;
        }
        /*
         * 
         */
        public function add(rhs:Quaternion):Quaternion{
            var q:Quaternion = new Quaternion();

            q._w = _w + rhs._w;
            q._x = _x + rhs._x;
            q._y = _y + rhs._y;
            q._z = _z + rhs._z;

            return q;
        }
        /*
         * 共役クォータニオンを返す 
         */
        public function conjugation():Quaternion{
            var q:Quaternion = new Quaternion();

            q._w = _w ;
            q._x = -_x ;
            q._y = -_y ;
            q._z = -_z ;

            return q;
        }

        public function get w():Number{
            return _w;
        }

        public function get x():Number{
            return _x;
        }

        public function get y():Number{
            return _y;
        }

        public function get z():Number{
            return _z;
        }

        public function set w(v:Number):void{
            _w = v;
        }

        public function set x(v:Number):void{
            _x = v;
        }

        public function set y(v:Number):void{
            _y = v;
        }

        public function set z(v:Number):void{
            _z = v;
        }
    }
}

Vector3.as
package{
    public class Vector3{
        private var _x:Number;
        private var _y:Number;
        private var _z:Number;

        public function Vector3( x:Number=0.0,y:Number=0.0,z:Number=0.0){
            _x = x ;
            _y = y ;
            _z = z ;
        }

        public function normalize():void{
            var lenv:Number = Math.sqrt(_x*_x + _y*_y + _z*_z);
            _x = _x / lenv;
            _y = _y / lenv;
            _z = _z / lenv;
        }

        public function length():Number{
            return Math.sqrt(_x*_x + _y*_y + _z*_z);
        }

        public function cross(rhs:Vector3):Number{
            return _y*rhs.z - _z*rhs.y + _z*rhs.x - _x*rhs.z + _x*rhs.y - _y*rhs.x;
        }

        public function get x():Number{
            return _x;
        }

        public function get y():Number{
            return _y;
        }

        public function get z():Number{
            return _z;
        }

        public function set x(v:Number):void{
            _x = v;
        }

        public function set y(v:Number):void{
            _y = v;
        }

        public function set z(v:Number):void{
            _z = v;
        }
    }
}

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