クォータニオン

by tepe
♥0 | Line 238 | Modified 2012-03-16 13:26:20 | MIT License
play

ActionScript3 source code

/**
 * Copyright tepe ( http://wonderfl.net/user/tepe )
 * MIT License ( http://www.opensource.org/licenses/mit-license.php )
 * Downloaded from: http://wonderfl.net/c/m9Xk
 */

package {
    import flash.geom.Matrix3D;
    import flash.geom.Matrix;
    import flash.display.Sprite;
    import flash.text.*;
    import flash.events.*;
    public class FlashTest extends Sprite {
        private var tf:TextField = new TextField();
        
        public function FlashTest() {
            // write as3 code here..
            // TextFormat オブジェクトを作成する
            var format:TextFormat = new TextFormat();
            

            format.font = "impact";        // フォント名
            format.size = 30;        // 文字のポイントサイズ

            tf.defaultTextFormat = format;//デフォルト書式設定

            addChild(tf);
            tf.z=0;
            
            tf.text ="test";
            tf.type = "input";
            tf.x =200;tf.y=200;
            tf.width = 400;
            tf.multiline = true;
            addEventListener(Event.ENTER_FRAME,onEnter);
            
            
            //tf.transform.matrix = new Matrix();
            var mat3:Matrix3D = new Matrix3D();
            mat3.copyToMatrix3D(Q.QuatConvertToMatrix(q1)); 
            tf.transform.matrix3D = mat3;
            
        }
        private var q1:Quatanion = new Quatanion(10,10,10,10);
        private var Q:Quat = new Quat();
        
        private function onEnter(e:Event):void{
            //tf.rotation++;
            tf.rotationX++;
            tf.rotationY++;
            //tf.rotationZ++;
            q1.x++;
            q1.w++;
            q1.z++;
            var mat3:Matrix3D = new Matrix3D();
            mat3.copyToMatrix3D(Q.QuatConvertToMatrix(q1)); 
            tf.transform.matrix3D = mat3;
            
            
        }

    }
}
import flash.geom.Matrix;
import flash.geom.Matrix3D;
import flash.geom.Vector3D;
/* -----------------------------------------------------------------------------
    クォータニオン(4元数)関数集
    ファイル名    :    quat.as
    as.Ver        :    AS 1.0 Flash 5 以降
    製作        :    Hakuhin    (C)    2006/08/17
 ----------------------------------------------------------------------------- */

/* -----------------------------------------------------------------------------

    --------------------------------------
    クォータニオンの型
    --------------------------------------
    quat = {
        x : 0,     // 虚数
        y : 0,     // 虚数
        z : 0,     // 虚数
        w : 0    // 実数
    };
    --------------------------------------


    // クォータニオン用関数 ---------------------------------------------------
    function QuatCreate(x,y,z,w);                // クォータニオン生成
    function QuatClone(q);                        // クォータニオン複製
    function QuatIdentity(q);                    // 単位クォータニオン化
    function QuatAdd(q1,q2);                    // 足し算
    function QuatSub(q1,q2);                    // 引き算
    function QuatMul(q1,q2);                    // 掛け算
    function QuatScale(q,s);                    // 実数掛け算
    function QuatNorm(q);                        // 絶対値(ノルム) |q|
    function QuatNormSqr(q);                    // 絶対値の2乗   |q|~2

    // 回転クォータニオン -----------------------------------------------------
    function QuatRotate(vec,rot);                // 回転クォータニオン作成
    function QuatRotatePref(vec,rot,m);            // 回転クォータニオン * 指定クォータニオン
    function QuatRotatePost(vec,rot,m);            // 指定クォータニオン * 回転クォータニオン

    // 射影変換 ---------------------------------------------------------------
    function QuatTransformVector(vec,q);        // ベクトルの射影変換

    // コンバート関数 ---------------------------------------------------------
    function QuatConvertFromMatrix(m);            // 行列からクォータニオンに変換
    function QuatConvertToMatrix(q);            // クォータニオンから行列に変換
    function QuatRotateConvertToMatrix(q);        // クォータニオンから行列に変換 (回転成分のみ)

    // 補間 ---------------------------------------------------------
    function QuatSlerp(q1,q2,d);                // 球面線形補間

 ----------------------------------------------------------------------------- */
 class Quatanion{
     public var x:Number;
     public var y:Number;
     public var z:Number;
     public var w:Number;
     public function Quatanion(qx:Number=0,qy:Number=0,qz:Number=0,qw:Number=0){       
        x = qx;
        y = qy;
        z = qz;
        w = qw;
    }

 }

class Quat{
    
    public function Quat(x:Number=0,y:Number=0,z:Number=0,w:Number=0){
        
    }
    /* ------------------------------------
    クォータニオン生成
    ------------------------------------ */
    public function QuatCreate(x:Number=0,y:Number=0,z:Number=0,w:Number=0):Quatanion{
        var q:Quatanion;
        q.x = x;
        q.y = y;
        q.z = z;
        q.w = w;
        return q;
    }
    /* ------------------------------------
        クォータニオン複製
    ------------------------------------ */
    public function QuatClone(q:Quatanion):Quatanion{
        var q2:Quatanion = new Quatanion(q.x, q.y, q.z, q.w); 
        
        return q2;
    }
    
    /* ------------------------------------
        単位クォータニオン化
    ------------------------------------ */
    public function QuatIdentity(q:Quatanion):Quatanion{
        q.x = 0;
        q.y = 0;
        q.z = 0;
        q.w = 1;
        return q;
    }

    /* ------------------------------------
        クォータニオン同士の足し算
    ------------------------------------ */
    public function QuatAdd(q1:Quatanion,q2:Quatanion):Quatanion{
        var add:Quatanion;
            add.x = q1.x + q2.x;
            add.y = q1.y + q2.y;
            add.z = q1.z + q2.z;
            add.w = q1.w + q2.w;
        
        return add;
    }

    /* ------------------------------------
        クォータニオン同士の引き算
    ------------------------------------ */
    public function QuatSub(q1:Quatanion,q2:Quatanion):Quatanion{
        var sub:Quatanion;
            sub.x = q1.x - q2.x;
            sub.y = q1.y - q2.y;
            sub.z = q1.z - q2.z;
            sub.w = q1.w - q2.w;
        
        return sub;
    }
    
    /* ------------------------------------
        クォータニオン同士の掛け算
    ------------------------------------ */
    public function QuatMul(q1:Quatanion,q2:Quatanion):Quatanion{
        var mul:Quatanion;
            mul.x = (q1.y * q2.z) - (q1.z * q2.y) + (q1.w * q2.x) + (q1.x * q2.w);
            mul.y = (q1.z * q2.x) - (q1.x * q2.z) + (q1.w * q2.y) + (q1.y * q2.w);
            mul.z = (q1.x * q2.y) - (q1.y * q2.x) + (q1.w * q2.z) + (q1.z * q2.w);
            mul.w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z);
        return mul;        
    }
    
    /* ------------------------------------
        クォータニオン実数の掛け算
    ------------------------------------ */
    public function QuatScale(q:Quatanion,s:Number):Quatanion{
        var scale:Quatanion;
            scale.x = q.x * s;
            scale.y = q.y * s;
            scale.z = q.z * s;
            scale.w = q.w * s;
        return scale;
    }
    
    /* ------------------------------------
        クォータニオンの絶対値(ノルム) |q|
    ------------------------------------ */
    public function QuatNorm(q:Quatanion):Number{
        return Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    }
    
    /* ------------------------------------
        クォータニオンの絶対値の2乗 |q|*|q|
    ------------------------------------ */
    public function QuatNormSqr(q:Quatanion):Number{
        var n:Number = Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
        return n * n;
    }
    
    /* ------------------------------------
        回転クォータニオンを作成
    ------------------------------------ */
    public function QuatRotate(v:Vector3D,r:Number):Quatanion{    
        var rad:Number = r * (Math.PI / 180.0) / 2.0;
        var s:Number = Math.sin(rad);
        var rot:Quatanion;
            rot.x = v.x * s;
            rot.y = v.y * s;
            rot.z = v.z * s;
            rot.w = Math.cos(rad);
        return rot;
        
    }
    
    /* ------------------------------------
        (回転クォータニオン * 指定クォータニオン)
    ------------------------------------ */
    public function QuatRotatePref(v:Vector3D,r:Number,q:Quatanion):Quatanion{    
        var rad:Number = r * (Math.PI / 180.0) / 2.0;
        var s:Number = Math.sin(rad);
        var qw:Number = Math.cos(rad);
        var qx:Number = v.x * s;
        var qy:Number = v.y * s;
        var qz:Number = v.z * s;
        var rp:Quatanion;
            rp.x = q.y * qz - q.z * qy + q.w * qx + q.x * qw;
            rp.y = q.z * qx - q.x * qz + q.w * qy + q.y * qw;
            rp.z = q.x * qy - q.y * qx + q.w * qz + q.z * qw;
            rp.w = q.w * qw - q.x * qx - q.y * qy - q.z * qz;
        return rp;
            
    }


    /* ------------------------------------
        (指定クォータニオン * 回転クォータニオン)
    ------------------------------------ */
    public function QuatRotatePost(v:Vector3D,r:Number,q:Quatanion):Quatanion{    
        var rad:Number = r * (Math.PI / 180.0) / 2.0;
        var s:Number = Math.sin(rad);
        var qw:Number = Math.cos(rad);
        var qx:Number = v.x * s;
        var qy:Number = v.y * s;
        var qz:Number = v.z * s;
        var rp:Quatanion;
            rp.x = qy * q.z - qz * q.y + qw * q.x + qx * q.w;
            rp.y = qz * q.x - qx * q.z + qw * q.y + qy * q.w;
            rp.z = qx * q.y - qy * q.x + qw * q.z + qz * q.w;
            rp.w = qw * q.w - qx * q.x - qy * q.y - qz * q.z;
        return rp;
    }
    
    
    /* ------------------------------------
        ベクトルの射影変換
    ------------------------------------ */
    public function QuatTransformVector(v:Vector3D,q:Quatanion):Vector3D{
        var w:Number =-q.x * v.x - q.y * v.y - q.z * v.z;
        var x:Number = q.y * v.z - q.z * v.y + q.w * v.x;
        var y:Number = q.z * v.x - q.x * v.z + q.w * v.y;
        var z:Number = q.x * v.y - q.y * v.x + q.w * v.z;
        var vec:Vector3D;
            vec.x = y * -q.z + z * q.y - w * q.x + x * q.w;
            vec.y = z * -q.x + x * q.z - w * q.y + y * q.w;
            vec.z = x * -q.y + y * q.x - w * q.z + z * q.w;
        return vec;
    }
    
    
    /* ------------------------------------
        回転行列からクォータニオンに変換
    ------------------------------------ */
    public function QuatConvertFromMatrix(m:Matrix3D):Quatanion{
    
        var s:Number;
        var tr:Number = m.rawData[0] + m.rawData[5] + m.rawData[10] + 1.0;//m._00 + m._11 + m._22 + 1.0;
        var fm:Quatanion;
        if (tr >= 1.0) {
            s = 0.5 / Math.sqrt(tr);
            fm.x = (m.rawData[9] - m.rawData[6]) * s;//x : (m._12 - m._21) * s,
            fm.y = (m.rawData[2] - m.rawData[8]) * s;//y : (m._20 - m._02) * s,
            fm.z = (m.rawData[4] - m.rawData[1]) * s;//z : (m._01 - m._10) * s
            fm.w = 0.25 / s;//    w : 0.25 / s,
            return fm;
        }
        else {
            var max:Number;
            if(m.rawData[5] > m.rawData[10]) max = m.rawData[5];//if(m._11 > m._22)    max = m._11;
            else max = m.rawData[10];//max = m._22;
            
            if(max < m.rawData[0]){//if (max < m._00) {
                //s = Math.sqrt(m._00 - (m._11 + m._22) + 1.0);
                s = Math.sqrt(m.rawData[0] - (m.rawData[5]+m.rawData[10]) + 1.0);
                var x:Number = s * 0.5;
                s = 0.5 / s; 
                fm.x = x;//x : x,
                fm.y = (m.rawData[4] + m.rawData[1]) * s;//y : (m._01 + m._10) * s,
                fm.z = (m.rawData[2] + m.rawData[8]) * s;//z : (m._20 + m._02) * s,
                fm.w = (m.rawData[9] - m.rawData[6]) * s;//w : (m._12 - m._21) * s
                return fm;
            } else if(max == m.rawData[5]){ //(max == m._11) {
                //s = Math.sqrt(m._11 - (m._22 + m._00) + 1.0);
                s = Math.sqrt(m.rawData[5] - (m.rawData[10] + m.rawData[0]) + 1.0);
                var y:Number = s * 0.5;
                s = 0.5 / s;
                fm.x = (m.rawData[4] + m.rawData[1]) * s;//x : (m._01 + m._10) * s,
                fm.y = y;//y : y,
                fm.z = (m.rawData[9] + m.rawData[6]) * s;//z : (m._12 + m._21) * s,
                fm.w = (m.rawData[2] - m.rawData[8]) * s;//w : (m._20 - m._02) * s
                return fm;
            } else {
                //s = Math.sqrt(m._22 - (m._00 + m._11) + 1.0);
                s = Math.sqrt(m.rawData[10] - (m.rawData[0] + m.rawData[5]) + 1.0);
                var z:Number = s * 0.5;
                s = 0.5 / s;
                fm.x = (m.rawData[2] + m.rawData[8]) * s;//x : (m._20 + m._02) * s,
                fm.y = (m.rawData[9] + m.rawData[6]) * s;//y : (m._12 + m._21) * s,
                fm.z = z;//z : z,
                fm.w = (m.rawData[4] - m.rawData[1]) * s;//w : (m._01 - m._10) * s
                return fm;
            }
    
        }
    
    }
    
    
    /* ------------------------------------
        クォータニオンから行列に変換
    ------------------------------------ */
    public function QuatConvertToMatrix(q:Quatanion):Matrix3D{
        var sx:Number = q.x * q.x;
        var sy:Number = q.y * q.y;
        var sz:Number = q.z * q.z;
        var cx:Number = q.y * q.z;
        var cy:Number = q.x * q.z;
        var cz:Number = q.x * q.y;
        var wx:Number = q.w * q.x;
        var wy:Number = q.w * q.y;
        var wz:Number = q.w * q.z;
        var cm:Matrix3D;
        
        //_00 : 1.0 - 2.0 * (sy + sz),    _01 : 2.0 * (cz + wz),            _02 : 2.0 * (cy - wy),
        cm.rawData[0] = 1.0 - 2.0 *(sy+sz);
        cm.rawData[1] = 2.0 * (cz+wz);
        //cm.rawData[2] = 
        //cm.rawData[3]
        //_10 : 2.0 * (cz - wz),            _11 : 1.0 - 2.0 * (sx + sz),    _12 : 2.0 * (cx + wx),
        cm.rawData[4]
        cm.rawData[5]
        cm.rawData[6]
        cm.rawData[7]
        //_20 : 2.0 * (cy + wy),            _21 : 2.0 * (cx - wx),            _22 : 1.0 - 2.0 * (sx + sy),
        cm.rawData[8]
        cm.rawData[9]
        cm.rawData[10]
        cm.rawData[11]
        //_30 : 0.0,                        _31 : 0.0,                        _32 : 0.0
        cm.rawData[12]
        cm.rawData[13]
        cm.rawData[14]
        cm.rawData[15]
        
        
        
        
        return cm;
    }
    
    
    /* ------------------------------------
        クォータニオンから行列に変換(回線成分のみ)
    ------------------------------------ */
   /* function QuatRotateConvertToMatrix(q){
        var s = Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
        s = 2.0 / (s * s);
        var vx = q.x * s;
        var vy = q.y * s;
        var vz = q.z * s;
        var wx = vx * q.w;
        var wy = vy * q.w;
        var wz = vz * q.w;    
        var sx = q.x * vx;
        var sy = q.y * vy;
        var sz = q.z * vz;
        var cx = q.y * vz;
        var cy = q.z * vx;
        var cz = q.x * vy;
        
        return {
            _00 : 1.0 - sy - sz,    _01 : cz + wz,            _02 : cy - wy,
            _10 : cz - wz,            _11 : 1.0 - sx - sz,    _12 : cx + wx,
            _20 : cy + wy,            _21 : cx - wx,            _22 : 1.0 - sx - sy,
            _30 : 0.0,                _31 : 0.0,                _32 : 0.0
        };
    }
    
    
    /* ------------------------------------
        球面線形補間
    ------------------------------------ */
    /*function QuatSlerp(q1,q2,d){
        var qr = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
        var ss = 1.0 - (qr * qr);
        if (ss == 0.0){
            return {
                x : q1.x,
                y : q1.y,
                z : q1.z,
                w : q1.w
            };
        }else{
            var ph = Math.acos(qr);
            if(qr < 0.0 && ph > Math.PI / 2.0){
                var s1,s2;
                qr = - q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
                ph = Math.acos(qr);
                s1 = Math.sin(ph * (1.0 - d)) / Math.sin(ph);
                s2 = Math.sin(ph *  d       ) / Math.sin(ph);
                
                return {
                    x : q1.x * s1 - q2.x * s2,
                    y : q1.y * s1 - q2.y * s2,
                    z : q1.z * s1 - q2.z * s2,
                    w : q1.w * s1 - q2.w * s2
                };
            }else{
                var s1,s2;
                s1 = Math.sin(ph * (1.0 - d)) / Math.sin(ph);
                s2 = Math.sin(ph *  d       ) / Math.sin(ph);
                return {            
                    x : q1.x * s1 + q2.x * s2,
                    y : q1.y * s1 + q2.y * s2,
                    z : q1.z * s1 + q2.z * s2,
                    w : q1.w * s1 + q2.w * s2
                }
            }
        }
    }
    
*/

}//class