Robot Arm

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

package {
    import flash.display.Sprite;
    import flash.events.Event;
    
    public class RobotArm extends Sprite {
        private var Mfwd:Array;
        private var Minv:Array;
        private var l0:Number;
        private var l1:Number;
        private var p0:Array;
        private var p1:Array;
        private var p:Array;
        private var canvas:Sprite;
        
        public function RobotArm() {
            graphics.beginFill(0, 1);
            graphics.drawRect(0, 0, 465, 465);
            graphics.endFill();
            
            canvas = new Sprite();
            addChild(canvas).x = 300;
            var i:int=0;
            Mfwd = new Array(3);
            for(i=0; i<3; i++) {
                Mfwd[i] = [0, 0, 0];
            }
            
            Minv = new Array(3);
            for(i=0; i<3; i++) {
                Minv[i] = [0, 0, 0];
            }
            
            l0 = 200;
            l1 = 200;
            
            addEventListener(Event.ENTER_FRAME, _loop);
        }
        
        protected function _loop(event:Event):void {
            canvas.graphics.clear();
            
            p0 = [100, 100, 0];
            p1 = [stage.mouseX-300, stage.mouseY, 0];
            p = new Array(3);
            var result:Boolean = solve(l0, l1, p1, p0, p);
            if(result) {
                _drawLine([0, 0, 0], p);
                _drawLine(p1, p);
                _drawPoint([0, 0, 0]);
                _drawPoint(p1);
                _drawPoint(p, 0x00FFFF);
            }
        }        
        
        public function _drawPoint(p:Array, color:uint=0xFF0000, radius:Number=5) : void {
            canvas.graphics.beginFill(color, 1);
            canvas.graphics.drawCircle(p[0], p[1], radius);
            canvas.graphics.endFill();
        }
        
        
        public function _drawLine(p0:Array, p1:Array) : void {
            canvas.graphics.lineStyle(1, 0xFFFFFF, 1);
            canvas.graphics.moveTo(p0[0], p0[1]);
            canvas.graphics.lineTo(p1[0], p1[1]);
            canvas.graphics.lineStyle();
        }
        
        
        public function solve(A:Number, B:Number, P:Array, D:Array, Q:Array) : Boolean {
            var R:Array = [0, 0, 0];
            defineM(P,D);
            rot(Minv,P,R);
            var d:Number = findD(A,B,norm(R));
            var e:Number = findE(A,d);
            var S:Array = [d,e,0];
            rot(Mfwd,S,Q);
            return d > 0 && d < A;
        }
        
        public function findD(a:Number, b:Number, c:Number) : Number {
            return Math.max(0, Math.min(a, (c + (a*a-b*b)/c) / 2));
        }
        
        public function findE(a:Number, d:Number) : Number {
            return Math.sqrt(a*a-d*d);
        }
        
        public function defineM(P:Array, D:Array) : void {
            var X:Array = Minv[0], Y:Array = Minv[1], Z:Array = Minv[2];
            
            var i:int;
            
            for(i=0; i<3; i++) {
                X[i] = P[i];
            }
            normalize(X);
            
            var dDOTx:Number = dot(D, X);
            for(i=0; i<3;i++) {
                Y[i] = D[i] - dDOTx * X[i];
            }
            normalize(Y);
            
            cross(X,Y,Z);
            
            
            for (i = 0 ; i < 3 ; i++) {
                Mfwd[i][0] = Minv[0][i];
                Mfwd[i][1] = Minv[1][i];
                Mfwd[i][2] = Minv[2][i];
            }
        }
        
        
        //    MATH STUFF
        public function dot(a:Array, b:Array) : Number {
            return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
        }
        
        public function cross(a:Array, b:Array, c:Array) : void {
            c[0] = a[1] * b[2] - a[2] * b[1];
            c[1] = a[2] * b[0] - a[0] * b[2];
            c[2] = a[0] * b[1] - a[1] * b[0];
        }
        
        public function norm(v:Array) : Number {
            return Math.sqrt( dot(v, v));
        }
        
        public function normalize(v:Array) : void {
            var norm:Number = norm(v);
            for (var i:int = 0 ; i < 3 ; i++) 
                v[i] /= norm;
        }
        
        public function rot(M:Array, src:Array, dst:Array) : void {
            for (var i:int = 0 ; i < 3 ; i++)
                dst[i] = dot(M[i],src);
        }
    }
}