PV3D matrix interpolation test 2 (seems to work)

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

// forked from yonatan's PV3D matrix interpolation test 1
// forked from yonatan's Optimized Matrix3D.interpolate()
// forked from umhr's 【未完成】Matrix3D.interpolate()

package {
    import org.papervision3d.core.math.Matrix3D;
    import org.papervision3d.core.math.Number3D;
    import org.papervision3d.core.math.Quaternion;
    import org.papervision3d.objects.DisplayObject3D;
    import org.papervision3d.objects.primitives.PaperPlane;
    import org.papervision3d.materials.ColorMaterial;
    import org.papervision3d.view.BasicView;

    import flash.display.Sprite;
    import flash.text.TextField;
    import flash.utils.getTimer;
    import flash.events.Event;
    
    [SWF (backgroundColor="0x000000")]

    public class FlashTest extends BasicView {
        private var tf:TextField;
        
        private var r:DisplayObject3D;
        private var g:DisplayObject3D;
        private var b:DisplayObject3D;
        
        public function FlashTest() {
            tf = new TextField;
            tf.width = tf.height = 400;
            tf.textColor = 0xFFFFFF;
            addChild( tf );
            
            // setup 3D scene
            _camera.zoom = 100;
            
            r = scene.addChild( new PaperPlane( new ColorMaterial( 0xFF0000, 0.75 ) ) );
            g = scene.addChild( new PaperPlane( new ColorMaterial( 0x00FF00, 0.75 ) ) );
            b = scene.addChild( new PaperPlane( new ColorMaterial( 0x0000FF, 0.75 ) ) );
            
            r.material.doubleSided = true;
            g.material.doubleSided = true;
            b.material.doubleSided = true;
            
            r.scaleX = r.scaleZ = 1.5;
            r.x = r.y = -100;
            r.rotationX = 55;
            r.rotationY = 200;
            r.rotationZ = 155;
            
            b.scaleY = 3;
            b.x = b.y = 100;
            b.rotationZ = 45;
            b.rotationY = 45;

            addEventListener( Event.ENTER_FRAME, frame );
        }
        
        private function frame( e:Event ):void {
            var t:Number = Math.sin( getTimer()/1000 )/2+0.5;
            tf.text = "Interpolation: " + t;
            g.copyTransform( interpolateMatrix3D( r.transform, b.transform, t ) );
            _camera.x = Math.sin( getTimer()/3000 ) * 1000;
            _camera.z = Math.cos( getTimer()/3000 ) * 1500;

            /* If you uncomment these there will be some hiccups. I'm not sure if it's an implementation bug
               or an inherent mathematical thing.
            b.rotationZ++;
            r.rotationX++;
            r.rotationY++;
               */

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

        private function interpolateMatrix3D(thisMat:Matrix3D,toMat:Matrix3D,percent:Number):Matrix3D{
            // decomposition:
            
            // we need to give Quaternion.createFromMatrix rotation-only matrices (with normalized axii), 
            // otherwise we choke on scaling matrices
            
            var thisQuat:Quaternion = Quaternion.createFromMatrix( getRotationMatrix( thisMat ) );
            var toQuat:Quaternion = Quaternion.createFromMatrix( getRotationMatrix( toMat ) );
            
            var thisScale:Number3D = getScaleFromMatrix3D( thisMat );
            var toScale:Number3D = getScaleFromMatrix3D( toMat );
            
            var thisTranslation:Number3D = new Number3D( thisMat.n14, thisMat.n24, thisMat.n34 );
            var toTranslation:Number3D = new Number3D( toMat.n14, toMat.n24, toMat.n34 );
            
            // interpolation:
            
            var scale_x:Number = thisScale.x*(1-percent) + toScale.x*percent;
            var scale_y:Number = thisScale.y*(1-percent) + toScale.y*percent;
            var scale_z:Number = thisScale.z*(1-percent) + toScale.z*percent;
            
            var tx:Number = thisTranslation.x*(1-percent) + toTranslation.x*percent;
            var ty:Number = thisTranslation.y*(1-percent) + toTranslation.y*percent;
            var tz:Number = thisTranslation.z*(1-percent) + toTranslation.z*percent;

            var q:Quaternion = Quaternion.slerp( thisQuat, toQuat, percent );

            // recomposition:

            var x:Number = q.x;
            var y:Number = q.y;
            var z:Number = q.z;
            var w:Number = q.w;
            
            var ret:Matrix3D = new Matrix3D;

            // apparently mxmlc optimizes away the duplicate multiplications, 
            // so there's no need to do it manually
            ret.n11 = (1-2*y*y-2*z*z)*scale_x;
            ret.n21 = (2*x*y+2*w*z)*scale_x;
            ret.n31 = (2*x*z-2*w*y)*scale_x;
            ret.n41 = 0;
            ret.n12 = (2*x*y-2*w*z)*scale_y;
            ret.n22 = (1-2*x*x-2*z*z)*scale_y;
            ret.n32 = (2*y*z+2*w*x)*scale_y;
            ret.n42 = 0;
            ret.n13 = (2*x*z+2*w*y)*scale_z;
            ret.n23 = (2*y*z-2*w*x)*scale_z;
            ret.n33 = (1-2*x*x-2*y*y)*scale_z;
            ret.n43 = 0;
            ret.n14 = tx;
            ret.n24 = ty;
            ret.n34 = tz;
            ret.n44 = 1;
            
            return ret;
        }
        
        // scale values can't be negative!
        private function getScaleFromMatrix3D( m:Matrix3D ):Number3D {
            var x:Number = Math.sqrt( m.n11*m.n11 + m.n21*m.n21 + m.n31*m.n31 );
            var y:Number = Math.sqrt( m.n12*m.n12 + m.n22*m.n22 + m.n32*m.n32 );
            var z:Number = Math.sqrt( m.n13*m.n13 + m.n23*m.n23 + m.n33*m.n33 );
            return( new Number3D( x, y, z ) );
        }

        // returns a rotation-only matrix (no scale/translate)
        private function getRotationMatrix( m:Matrix3D ):Matrix3D {
            var scale:Number3D = getScaleFromMatrix3D( m );
            return( new Matrix3D ( [
                m.n11 / scale.x, m.n12 / scale.y, m.n13 / scale.z, 0,
                m.n21 / scale.x, m.n22 / scale.y, m.n23 / scale.z, 0,
                m.n31 / scale.x, m.n32 / scale.y, m.n33 / scale.z, 0,
                0, 0, 0, 1 
            ] ) );
        }
    }
}