逆運動学 (3D)

前回の逆運動学の3D版
↓のウィンドウ内でマウスを動かすと、連動してアーム(もどきのキューブ)が動きます。

package {
        import flash.display.*;
        import flash.events.*;
        
        import org.papervision3d.core.*;
        import org.papervision3d.core.math.*;
        import org.papervision3d.scenes.*;
        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.*;
        
        [SWF(width="600", height="400", backgroundColor="#ffffff")]
        public class Link3D 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:Camera3D = new Camera3D();
                private var arm1:Cube;
                private var arm2:Cube;
                
                private var ARM_LEN:int = 400;
                private var W:int = 600;
                private var H:int = 400;
                
                public function Link3D():void
                {
                        stage.align = StageAlign.TOP_LEFT;
                        stage.scaleMode = StageScaleMode.NO_SCALE;
                        
                        //viewport.y = 50;
                        addChild(viewport);
                        
                        camera.y = 0;
                        camera.z = -550;
                        camera.x = 0;
                        camera.focus = 1100;
                        camera.zoom = 1;
                        
                        var m:WireframeMaterial = new WireframeMaterial( 0xff0000 );
                        arm1 = new Cube(new MaterialsList({all:m}), 50, 50, ARM_LEN);
                        arm1.x = 0;
                        arm1.y = ARM_LEN/2;
                        scene.addChild(arm1);
                        
                        var m2:WireframeMaterial = new WireframeMaterial( 0xff00 );
                        arm2 = new Cube(new MaterialsList({all:m2}), 50, 50, ARM_LEN);
                        arm2.x = 0;
                        arm2.y = ARM_LEN/2*3;
                        scene.addChild(arm2);
                        
                        
                        //camera.target = arm1;
                        renderer.renderScene(scene, camera, viewport);
                        
                        stage.addEventListener( MouseEvent.MOUSE_MOVE, function(e:*):void{
                                if( stage.mouseY > 0 && stage.mouseY < H){
                                        moveArm((stage.mouseX - W/2) * 1.8, (H - stage.mouseY) * 1.6 , 300);
                                }
                        });
                }
                
                private function moveArm(x:int, y:int, z:int):void{
                        trace(x.toString() + " , " + y.toString() + " , " + z.toString());
                        
                        var txz:Number = (x == 0) ? 0 : Math.atan(z/x); // xz平面上の角度
                        var d:Number = Math.sqrt(z*z + x*x); // 原点と(x,y,z)を結んでできる三角形の底辺の長さ
                        
                        var l:Number = Math.sqrt(d*d + y*y);
                        var t2:Number = Math.acos((l*l - ARM_LEN*ARM_LEN - ARM_LEN*ARM_LEN) /
                        (2 * ARM_LEN * ARM_LEN));
                        
                        if(isNaN(t2))
                        t2 = 0;
                        
                        var t1:Number = Math.atan(d/y) -
                        Math.atan((ARM_LEN * Math.sin(t2)) /
                        (ARM_LEN + ARM_LEN * Math.cos(t2)));
                        
                        if(isNaN(t1))
                        t1 = 0;
                        
                        // arm1の端を原点に移動させる行列
                        var m1:Matrix3D = new Matrix3D();
                        m1.n24 = ARM_LEN /2;
                        
                        // Y軸の回転行列
                        var m2:Matrix3D = Matrix3D.rotationY(txz);
                        
                        // t1/t2に対する回転軸を求める
                        var v:Number3D = new Number3D();
                        v.x = x * Math.cos(Math.PI/2) + z * Math.sin(Math.PI/2);
                        v.z = -x * Math.sin(Math.PI/2) + z * Math.cos(Math.PI/2);
                        v.normalize();
                        
                        // t1だけ回転させる行列
                        var m3:Matrix3D = Matrix3D.rotationMatrix(v.x, 0, v.z, t1);
                        
                        // m1/m2/m3をarm1に適用
                        m3.calculateMultiply(m3, m2);
                        arm1.transform.calculateMultiply(m3,m1);
                        
                        // t1+t2だけ回転させる行列
                        m3 = Matrix3D.rotationMatrix(v.x, 0, v.z, t1+t2);
                        
                        // arm2の端をarm1の端に移動させる行列
                        var m4:Matrix3D = new Matrix3D();
                        m4.n24 = ARM_LEN * Math.cos(t1);
                        m4.n14 = ARM_LEN * Math.sin(t1) * Math.cos(txz);
                        if(txz < 0) m4.n14 *= -1;
                        m4.n34 = ARM_LEN * Math.sin(t1) * Math.abs(Math.sin(txz));
                        
                        
                        trace("t1 = " + t1.toString());
                        trace("t2 = " + t2.toString());
                        
                        // m1/m2/m3/m4をarm2に適用
                        m4.calculateMultiply(m4, m3);
                        m4.calculateMultiply (m4, m2);
                        arm2.transform.calculateMultiply(m4,m1);
                        
                        renderer.renderScene(scene, camera, viewport);
                }
        }
}