Optimized Matrix3D.interpolate()

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

// forked from umhr's 【未完成】Matrix3D.interpolate()

package {
    import flash.display.Sprite;
    import flash.geom.Matrix3D;
    import flash.text.TextField;
    import flash.geom.Vector3D;
    import flash.system.Capabilities;
    import flash.utils.getTimer;
    public class FlashTest extends Sprite {
        public function FlashTest() {
            var tf:TextField = new TextField();
            tf.width=stage.stageWidth;
            tf.height=stage.stageHeight;
            tf.wordWrap=true;
            stage.addChild(tf);
            var txt:String="";
            
            var m1:Matrix3D = new Matrix3D;
            var m2:Matrix3D = new Matrix3D;
            var start:Number;
            var cnt:Number;
            const COUNT:uint = 50000;
            
            start = getTimer();
            for( cnt = COUNT; cnt; cnt-- ) {
                m1.rawData = Util.random16();
                m2.rawData = Util.random16();
                Mtrx3D.interpolate(m1, m2, Math.random());
            }
            txt += "interpolate() time: " + ( getTimer() - start ) + "ms\n";
            
            start = getTimer();
            for( cnt = COUNT; cnt; cnt-- ) {
                m1.rawData = Util.random16();
                m2.rawData = Util.random16();
                Mtrx3D.interpolate2(m1, m2, Math.random());
            }
            txt += "interpolate2() time: " + ( getTimer() - start ) + "ms\n";
            
            var r16:Vector.<Number> = Util.random16();
            m1.rawData = Util.random16();
            m2.rawData = Util.random16();
            
            txt += "Differences between results of interpolate() and interpolate2():\n";
            txt += Util.hikaku( Mtrx3D.interpolate(m1,m2,0.42).rawData,Mtrx3D.interpolate2(m1,m2,0.42).rawData );
            
            tf.text=txt;
        }
    }
}
import flash.display.Sprite;
class Mtrx3D extends Sprite {
    import flash.geom.Matrix3D;
    import flash.geom.Vector3D;
    public static function interpolate(thisMat:Matrix3D,toMat:Matrix3D,percent:Number):Matrix3D{
        var v0:Vector3D = thisMat.decompose("quaternion")[1];
        var v1:Vector3D = toMat.decompose("quaternion")[1];
        var cosOmega:Number = v0.w*v1.w + v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
        if(cosOmega < 0){
            v1.x = -v1.x;
            v1.y = -v1.y;
            v1.z = -v1.z;
            v1.w = -v1.w;
            cosOmega = -cosOmega;
        }
        var k0:Number;
        var k1:Number;
        if(cosOmega > 0.9999){
            k0 = 1 - percent;
            k1 = percent;
        }else{
            var sinOmega:Number = Math.sqrt(1 - cosOmega*cosOmega);
            var omega:Number = Math.atan2(sinOmega,cosOmega);
            var oneOverSinOmega:Number = 1/sinOmega;
            k0 = Math.sin((1-percent)*omega)*oneOverSinOmega;
            k1 = Math.sin(percent*omega)*oneOverSinOmega;
        }
        var scale_x:Number = thisMat.decompose("quaternion")[2].x*(1-percent) + toMat.decompose("quaternion")[2].x*percent;
        var scale_y:Number = thisMat.decompose("quaternion")[2].y*(1-percent) + toMat.decompose("quaternion")[2].y*percent;
        var scale_z:Number = thisMat.decompose("quaternion")[2].z*(1-percent) + toMat.decompose("quaternion")[2].z*percent;
        
        var tx:Number = thisMat.decompose("quaternion")[0].x*(1-percent) + toMat.decompose("quaternion")[0].x*percent;
        var ty:Number = thisMat.decompose("quaternion")[0].y*(1-percent) + toMat.decompose("quaternion")[0].y*percent;
        var tz:Number = thisMat.decompose("quaternion")[0].z*(1-percent) + toMat.decompose("quaternion")[0].z*percent;
        
        var x:Number = v0.x*k0+v1.x*k1;
        var y:Number = v0.y*k0+v1.y*k1;
        var z:Number = v0.z*k0+v1.z*k1;
        var w:Number = v0.w*k0+v1.w*k1;
        var _q:Vector.<Number> = new Vector.<Number>(16,true);
        _q[0] = (1-2*y*y-2*z*z)*scale_x;
        _q[1] = (2*x*y+2*w*z)*scale_x;
        _q[2] = (2*x*z-2*w*y)*scale_x;
        _q[3] = 0;
        _q[4] = (2*x*y-2*w*z)*scale_y;
        _q[5] = (1-2*x*x-2*z*z)*scale_y;
        _q[6] = (2*y*z+2*w*x)*scale_y;
        _q[7] = 0;
        _q[8] = (2*x*z+2*w*y)*scale_z;
        _q[9] = (2*y*z-2*w*x)*scale_z;
        _q[10] = (1-2*x*x-2*y*y)*scale_z;
        _q[11] = 0;
        _q[12] = tx;
        _q[13] = ty;
        _q[14] = tz;
        _q[15] = 1;
        //trace(_q)
        
        var v:Vector3D = new Vector3D(v0.x*k0+v1.x*k1,v0.y*k0+v1.y*k1,v0.z*k0+v1.z*k1,v0.w*k0+v1.w*k1);
        
        //var txyz:Vector3D = new Vector3D(tx,ty,tz);
        //var m:Matrix3D=new Matrix3D();
        //m.recompose(Vector.<Vector3D>([txyz,v,new Vector3D(scale_x,scale_y,scale_z)]),"quaternion");
        //trace(m.rawData);
        return new Matrix3D(_q);
    }

    public static function interpolate2(thisMat:Matrix3D,toMat:Matrix3D,percent:Number):Matrix3D{
        var thisDecomp:Vector.<Vector3D> = thisMat.decompose("quaternion");
        var toDecomp:Vector.<Vector3D> = toMat.decompose("quaternion");
        
        var v0:Vector3D = thisDecomp[1];
        var v1:Vector3D = toDecomp[1];
        var cosOmega:Number = v0.w*v1.w + v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
        if(cosOmega < 0){
            v1.x = -v1.x;
            v1.y = -v1.y;
            v1.z = -v1.z;
            v1.w = -v1.w;
            cosOmega = -cosOmega;
        }
        var k0:Number;
        var k1:Number;
        if(cosOmega > 0.9999){
            k0 = 1 - percent;
            k1 = percent;
        }else{
            var sinOmega:Number = Math.sqrt(1 - cosOmega*cosOmega);
            var omega:Number = Math.atan2(sinOmega,cosOmega);
            var oneOverSinOmega:Number = 1/sinOmega;
            k0 = Math.sin((1-percent)*omega)*oneOverSinOmega;
            k1 = Math.sin(percent*omega)*oneOverSinOmega;
        }
        var scale_x:Number = thisDecomp[2].x*(1-percent) + toDecomp[2].x*percent;
        var scale_y:Number = thisDecomp[2].y*(1-percent) + toDecomp[2].y*percent;
        var scale_z:Number = thisDecomp[2].z*(1-percent) + toDecomp[2].z*percent;
        
        var tx:Number = thisDecomp[0].x*(1-percent) + toDecomp[0].x*percent;
        var ty:Number = thisDecomp[0].y*(1-percent) + toDecomp[0].y*percent;
        var tz:Number = thisDecomp[0].z*(1-percent) + toDecomp[0].z*percent;
        
        // mxmlc optimizes away the duplicate multiplications, so there's no need to do it manually
        
        var x:Number = v0.x*k0+v1.x*k1;
        var y:Number = v0.y*k0+v1.y*k1;
        var z:Number = v0.z*k0+v1.z*k1;
        var w:Number = v0.w*k0+v1.w*k1;
        var _q:Vector.<Number> = new Vector.<Number>(16,true);
        _q[0] = (1-2*y*y-2*z*z)*scale_x;
        _q[1] = (2*x*y+2*w*z)*scale_x;
        _q[2] = (2*x*z-2*w*y)*scale_x;
        _q[3] = 0;
        _q[4] = (2*x*y-2*w*z)*scale_y;
        _q[5] = (1-2*x*x-2*z*z)*scale_y;
        _q[6] = (2*y*z+2*w*x)*scale_y;
        _q[7] = 0;
        _q[8] = (2*x*z+2*w*y)*scale_z;
        _q[9] = (2*y*z-2*w*x)*scale_z;
        _q[10] = (1-2*x*x-2*y*y)*scale_z;
        _q[11] = 0;
        _q[12] = tx;
        _q[13] = ty;
        _q[14] = tz;
        _q[15] = 1;
        
        return new Matrix3D(_q);
    }
}
class Util {
    import flash.geom.Matrix3D;
    import flash.geom.Vector3D;
    public static function hikaku(v0:Vector.<Number>,v1:Vector.<Number>):String {
        var _str:String="↓二つのMatrix3Dの要素毎の差\n";
        var _n:int=v0.length;
        for (var i:int=0; i<_n; i++) {
            _str += "["+i+"]:"+(v0[i]-v1[i])+"\n";
        }
        return _str;
    }

    public static function random16():Vector.<Number> {
        var _v:Vector.<Number>=new Vector.<Number>(16,true);
        for (var i:int=0; i<16; i++) {
            _v[i]=Math.random()*200-100;
        }
        return _v;
    }
    public static function random9():Vector.<Number>{
        var _mt:Matrix3D = new Matrix3D();
        var v:Vector.<Vector3D>=new Vector.<Vector3D>;
        v[0]=new Vector3D(200*Math.random()-100,200*Math.random()-100,200*Math.random()-100);//平行移動、
        v[1]=new Vector3D(10*Math.random()-5,10*Math.random()-5,10*Math.random()-5);//回転
        //v[2]=new Vector3D(100*Math.random(),100*Math.random(),100*Math.random());//拡大 / 縮小
        v[2]=new Vector3D(1,1,1);//拡大 / 縮小
        _mt.recompose(v);
        return _mt.rawData;
    }
}