forked from: IK Test

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

package {
    import flash.display.StageScaleMode;
    import flash.display.Sprite;
    import flash.events.Event;
    import flash.geom.Point;

    public class IKWanderer extends Sprite {
        private var _list:SegmentList;
        private const colors:Array = [0xFF0000, 0xFFFF00, 0x00FF00, 0x00FFFF, 0x0000FF, 0xFF00FF];
        private var target:Vehicle;
        private var _circles:Array;
        
        public function IKWanderer() {
            initWanderer();
            init();
        }
        
        private function init():void {
            stage.scaleMode = StageScaleMode.NO_SCALE;
            _list = createSegmentList(1000, 3);
            addEventListener(Event.ENTER_FRAME, onEnterFrame);
        }
        private function initWanderer():void {
            var boundarySize:Number = 465;
            //stage.align = StageAlign.;
            stage.scaleMode = StageScaleMode.NO_SCALE;
            
            target = new Vehicle();
            target.position = new Vector2D(boundarySize * 0.5, boundarySize * 0.5);
            target.edgeBehavior = Vehicle.WRAP;
            target.maxSpeed = 4;
            target.wanderRange = 3;
            target.visible = true;
            addChild(target);
            var isBoundaryVisible:Boolean = true;
            var c:Circle;
            _circles = [];
            var cSize:Number=30;
            var _numCircles:int = Math.floor(boundarySize / (cSize * 2))+2;
            var number:Number = _numCircles * cSize * 2;
            var gap:Number = boundarySize-number;
            var number2:Number = cSize * 4;
            var boundaryPosition:Number=-2*cSize;
            for (var i:int = 0; i < _numCircles; i++) {
                c = new Circle(cSize,0xff);
                c.x = -cSize;
                boundaryPosition+=2*cSize+2;
                c.y = boundaryPosition;
                addChild(c);
                c.visible=isBoundaryVisible;
                _circles.push(c);
            }
            boundaryPosition=-2*cSize;
            for ( i= 0; i < _numCircles; i++) {
                c = new Circle(cSize);
                c.x = boundarySize+cSize+1;
                boundaryPosition+=2*cSize+2;
                c.y = boundaryPosition;
                addChild(c);
                c.visible=isBoundaryVisible;
                _circles.push(c);
            }
            boundaryPosition=-2*cSize;
            for ( i= 0; i < _numCircles; i++) {
                c = new Circle(cSize);
                c.y = boundarySize+cSize+1;
                boundaryPosition+=2*cSize+2;
                c.x = boundaryPosition;
                addChild(c);
                c.visible=isBoundaryVisible;
                _circles.push(c);
            }
            boundaryPosition=-2*cSize;
            for ( i= 0; i < _numCircles; i++) {
                c = new Circle(cSize);
                c.y = -cSize;
                boundaryPosition+=2*cSize+2;
                c.x = boundaryPosition;
                addChild(c);
                c.visible=isBoundaryVisible;
                _circles.push(c);
            }
            
        }
        
        private function createSegmentList(size:uint, segmentLength:Number):SegmentList {
            var sl:SegmentList = new SegmentList(new Point(0, 0));
            for (var i:uint = 1; i <= size; i++) {
                sl.addSegment(new Point(i % 2 ? segmentLength : 0,  0));
            }
            return sl;
        }
        
        private function onEnterFrame(evt:Event):void {
            wandererUpdate(evt);
            var count:uint = 0;
            _list.drag(new Point(target.x, target.y));
            graphics.clear();
            graphics.moveTo(_list.head.point0.x, _list.head.point0.y);
            for (var segment:Segment = _list.head; segment != null; segment = segment.next) {
                graphics.lineStyle(4, colors[count++ % colors.length]);
                graphics.lineTo(segment.point1.x, segment.point1.y);
            }
        }
        private function wandererUpdate(e:Event):void 
        {
            target.wander();
            target.avoid(_circles);
            target.update();
        }
    }
}


import flash.geom.Point;

class SegmentList {
    private var _head:Segment;
    private var _tail:Segment;
    private var _origin:Point;
    
    public function SegmentList(origin:Point) {
        _origin = origin;
    }
    
    public function get tail():Segment
    {
        return _tail;
    }

    public function get head():Segment
    {
        return _head;
    }

    public function addSegment(point:Point):Segment {
        if (_tail) {
            var segment:Segment = new Segment(_tail.point1, point);
            segment.prev = _tail;
            _tail.next = segment;
            _tail = segment;
        } else {
            _tail = new Segment(_origin, point);
            _head = _tail;
        }
        
        return _tail;
    }
    
    public function drag(target:Point):void {
        if (_tail == null) {
            return;
        }
        _tail.point1 = target;
        for (var segment:Segment = _tail; segment != null; segment = segment.prev) {
            segment.restore();
        }
    }
}

class Segment {
    public var point0:Point;
    public var point1:Point;
    public var next:Segment;
    public var prev:Segment;
    public var restoreRaito:Number = 1;
    
    private var _originalLength:Number;
    
    public function Segment(point0:Point, point1:Point) {
        this.point0 = point0;
        this.point1 = point1;
        _originalLength = this.length;
    }
    
    public function restore():void {
        var dx:Number = point1.x - point0.x;
        var dy:Number = point1.y - point0.y;
        var m:Number = .5 - _originalLength / Point.distance(point0, point1) * .5;
        
        point0.x += m * dx * restoreRaito * 2;
        point0.y += m * dy * restoreRaito * 2;
        point1.x -= m * dx * (1-restoreRaito) * 2;
        point1.y -= m * dy * (1-restoreRaito) * 2;
    }
    
    public function get length():Number {
        return Point.distance(point0, point1);
    }
}

import flash.display.Graphics;
import flash.display.Sprite;
class Vector2D {
    private var _x:Number;
    private var _y:Number;
    public function Vector2D(x:Number = 0, y:Number = 0) {
        _x = x;
        _y = y;
    }
    
    /**
     * draw 
     */
    public function draw(graphics:Graphics, color:uint):void {
        graphics.lineStyle(0, color);
        graphics.moveTo(0, 0);
        graphics.lineTo(_x, _y);
    }
    public function get x():Number { return _x; }
    public function set x(value:Number):void 
    {
        _x = value;
    }
    
    public function get y():Number { return _y; }
    public function set y(value:Number):void 
    {
        _y = value;
    }
    
    /**
     * 複製する
     * @return
     */
    public function clone():Vector2D {
        return new Vector2D(_x, _y);
    }
    /**
     * ベクトルをゼロに
     * @return
     */
    public function zero():Vector2D {
        _x = _y = 0;
        return this;
    }
    
    /**
     * ベクトルがゼロか？
     * @return
     */
    public function isZero():Boolean {
        return _x == 0 && _y == 0;
    }
    
    /**
     * ベクトルの大きさを指定したサイズに
     */
    public function set length(value:Number):void {
        var a:Number = angle;
        _x = Math.cos(a) * value;
        _y = Math.sin(a) * value;
    }
    
    /**
     * ベクトルの長さ
     */
    public function get length():Number {
        return Math.sqrt(lengthSQ);
    }
    
    /**
     * ベクトルの長さの2乗
     */
    public function get lengthSQ():Number {
        return _x * _x + _y * _y;
    }
    
    public function get angle():Number {
        return  Math.atan2(_y, _x);
    }
    
    public function set angle(value:Number):void 
    {
        var len:Number = length;
        _x = Math.cos(value) * len;
        _y = Math.sin(value) * len;
    }
    
    
    /**
     * ベクトルを正規化する
     * ベクトルが０の場合、結果を(1,0)とする
     * @return
     */
    public function normalize():Vector2D {
        if (length == 0) {
            _x = 1;
        }else {
            var len:Number = length;        
            _x /= len;
            _y /= len;
            
        }
        return this;
    }
    /**
     * ベクトルの大きさをmaxまでにカットする。
     * @param    max
     * @return
     */
    public function truncate(max:Number):Vector2D {
        var len:Number = length;
        if (len > max) {
            length = max;
        }
        return this;
    }
    /**
     * ベクトルの向きを逆に
     * @return
     */
    public function reverse():Vector2D {
        _x = -_x;
        _y = -_y;
        return this;
    }
    /**
     * ベクトルが正規化されているか？
     * @return
     */
    public function isNormalized():Boolean {
        return length == 1.0;
    }
    
    /**
     * ベクトル Vとの内積を求める
     * @param    v
     * @return
     */
    public function dotProduct(v:Vector2D):Number {
        return _x * v._x + _y * v._y;
    }
    
    /**
     * 内積からベクトルのなす角を求める　-PI/2 ～ PI/2
     * @param    v1
     * @param    v2
     * @return
     */
    public static function angleBetween(v1:Vector2D, v2:Vector2D):Number {
        if (!v1.isNormalized()) v1 = v1.clone().normalize();
        if (!v2.isNormalized()) v2 = v2.clone().normalize();
        
        return Math.acos(v1.dotProduct(v2));
    }
    /**
     * ベクトルvが右にあるか左にあるか、
     * @param    v
     * @return
     */
    public function sign(v:Vector2D):int {
        return this.perp.dotProduct(v) < 0 ? -1 : 1;
    }
    
    /**
     * 直交するベクトル
     */
    public function get perp():Vector2D {
        return new Vector2D( -y, x);
    }
    
    public function distance(v:Vector2D):Number {
        return Math.sqrt(distanceSQ(v));
    }
    public function distanceSQ(v:Vector2D):Number {
        var dx:Number = v._x - _x;
        var dy:Number = v._y - _y;
        return dx * dx + dy * dy;
    }
    
    public function add(v:Vector2D):Vector2D {
        return new Vector2D(_x + v._x, _y + v._y);
    }
    public function subtract(v:Vector2D):Vector2D {
        return new Vector2D(_x - v._x, _y - v._y);
    }
    
    public function multiply(value:Number):Vector2D {
        return new Vector2D(_x * value, _y * value);
    }
    
    public function divide(value:Number):Vector2D{
        if (value == 0) {}//後で考える
        return new Vector2D(_x / value, _y / value);
    }
    
    public function equals(v:Vector2D):Boolean {
        return _x == v._x && _y == v._y;
    }
    
    public function toString():String {
        return "[Vector2D( x:" + _x + ", y:" + _y + ", )]";
    }
}

class Vehicle extends Sprite {
    protected var _mass:Number = 1.0;
    protected var _maxSpeed:Number = 10;
    protected var _position:Vector2D;
    protected var _velocity:Vector2D;
    
    private var _edgeBehavior:Function;
    
    public static const WRAP:String = "wrap";
    public static const BOUNCE:String = "bounce";
    
    private var _maxForce:Number = 1;
    private var _steeringForce:Vector2D;
    private var _arrivalThreshold:Number = 100;

    private var _wanderAngle:Number = 0;
    private var _wanderDistance:Number = 3;
    private var _wanderRadius:Number = 30;
    private var _wanderRange:Number = 15;
    private var _avoidDistance:Number = 100;
    private var _avoidBuffer:Number = 10;

    public function Vehicle() {
        _steeringForce = new Vector2D();

        _position = new Vector2D();
        _velocity = new Vector2D();
        _edgeBehavior = wrap;
        draw();
        tabChildren = false;
    }
    
    protected function draw():void
    {
        var g:Graphics = graphics;
        g.clear();
        g.lineStyle(0);
        g.moveTo(10, 0);
        g.lineTo( -10, 5);
        g.lineTo( -10, -5);
        g.lineTo(10, 0);
    }
    public function update():void {
        _steeringForce.truncate(maxForce);
        _steeringForce = _steeringForce.divide(_mass);
        _velocity = _velocity.add(_steeringForce);
        
        _velocity.truncate(_maxSpeed);
        
        _steeringForce.x = _steeringForce.y = 0;
        _position = _position.add(_velocity);
        
        _edgeBehavior();
        x = position.x;
        y = position.y;
        
        rotation = _velocity.angle * 180 / Math.PI;
    }
    /**
     * 跳ね返る
     */
    private function bounce():void {
        if (stage != null) {
            var w:Number = stage.stageWidth;
            var h:Number = stage.stageHeight;
            
            if (position.x > w) {
                position.x = w;
                _velocity.x *= -1;
            }else if(position.x < 0){
                position.x = 0;
                _velocity.x *= -1;
            }
            
            if (position.y > h) {
                position.y = h;
                _velocity.y *= -1;
            }else if (position.y < 0) {
                position.y = 0
                _velocity.y *= -1;
            }
        }
    }
    /**
     * 反対側に移動する。
     */
    private function wrap():void {
        if (stage != null) {
            var w:Number = stage.stageWidth;
            var h:Number = stage.stageHeight;
            
            if (position.x > w) position.x = 0;
            if (position.x < 0) position.x = w;
            if ( position.y > h) position.y = 0;
            if ( position.y < 0) position.y = h;
            
        }
    }
    public function get edgeBehavior():String{
        if ( _edgeBehavior == bounce) return Vehicle.BOUNCE;
        if (_edgeBehavior == wrap) return Vehicle.WRAP;
        return "";
    }
    public function set edgeBehavior(value:String):void{
        switch(value) {
        case Vehicle.BOUNCE:
            _edgeBehavior = bounce;
            break;
        case Vehicle.WRAP:
        default:
            _edgeBehavior = wrap;
            break;
        }        
    }
    public function get mass():Number { return _mass; }
    public function set mass(value:Number):void 
    {
        _mass = value;
    }
    
    public function get maxSpeed():Number { return _maxSpeed; }
    public function set maxSpeed(value:Number):void 
    {
        _maxSpeed = value;
    }
    
    public function get position():Vector2D { return _position; }
    public function set position(value:Vector2D):void 
    {
        _position = value;
        x = _position.x;
        y = _position.y;
    }
    
    public function get velocity():Vector2D { return _velocity; }
    public function set velocity(value:Vector2D):void 
    {
        _velocity = value;
    }
    
    override public function set x(value:Number):void 
    {
        super.x = value;
        _position.x = value;
    }
    
    override public function set y(value:Number):void 
    {
        super.y = value;
        _position.y = value;
    }
    
    public function get maxForce():Number { return _maxForce; }
    
    public function set maxForce(value:Number):void 
    {
        _maxForce = value;
    }
    
    public function get wanderRange():Number 
    {
        return _wanderRange;
    }
    
    public function set wanderRange(value:Number):void 
    {
        _wanderRange = value;
    }
    
    /**
     * Seek behavior
     * @param    target
     */
    public function seek(target:Vector2D):void {
        var desiredVelocity:Vector2D = target.subtract(_position);
        desiredVelocity.normalize();
        desiredVelocity = desiredVelocity.multiply(_maxSpeed);
        var force:Vector2D = desiredVelocity.subtract(_velocity);
        
        _steeringForce = _steeringForce.add(force);
    }
    
    public function flee(target:Vector2D):void {
        var desiredVelocity:Vector2D = target.subtract(_position);
        desiredVelocity.normalize();
        desiredVelocity = desiredVelocity.multiply(_maxSpeed);
        var force:Vector2D = desiredVelocity.subtract(_velocity);
        
        _steeringForce = _steeringForce.subtract(force);        
    }
    public function arrive(target:Vector2D):void {
        var desiredVelocity:Vector2D = target.subtract(_position);
        desiredVelocity.normalize();
        
        
        var dist:Number = _position.distance(target);
        if (dist > _arrivalThreshold) {
            desiredVelocity = desiredVelocity.multiply(_maxSpeed);
        }else {
            //_arrivalThresholdまで近づいたらスピードダウン
            desiredVelocity = desiredVelocity.multiply(_maxSpeed * dist / _arrivalThreshold);
        }
        var force:Vector2D = desiredVelocity.subtract(_velocity);
        _steeringForce = _steeringForce.add(force);
    }
    
    public function pursue(target:Vehicle):void {
        //現在位置からターゲットまでかかる時間
        var lookAheadTime:Number = position.distance(target.position) / _maxSpeed;
        
        var predictedTarget:Vector2D = target.position.add(target.velocity.multiply(lookAheadTime));
        seek(predictedTarget);
    }
    
    public function evade(target:Vehicle):void {
        //現在位置からターゲットまでかかる時間
        var lookAheadTime:Number = position.distance(target.position) / _maxSpeed;
        
        var predictedTarget:Vector2D = target.position.subtract(target.velocity.multiply(lookAheadTime));
        flee(predictedTarget);
    }
    public function wander():void {
        //進行方向から_wanderDistance進んだ位置をcenter
        var center:Vector2D = velocity.clone().normalize().multiply(_wanderDistance);
        
        //centerからのずれ
        var offset:Vector2D = new Vector2D(0,0);
        offset.length = _wanderRadius;
        offset.angle = _wanderAngle;
        _wanderAngle += Math.random() * _wanderRange - _wanderRange * 0.5;
        
        var force:Vector2D = center.add(offset);
        _steeringForce = _steeringForce.add(force);
    }
    
    public function avoid(circles:Array):void {
        var i:int;
        var len:int = circles.length;
        var c:Circle;
        var heading:Vector2D;
        var feeler:Vector2D;
        var projection:Vector2D;
        var dist:Number;
        var force:Vector2D;
        
        for (i = 0; i < len; i++) {
            heading = _velocity.clone().normalize();
            c = circles[i] as Circle;
            
            var difference:Vector2D = c.position.subtract(_position);
            var dotProd:Number = difference.dotProduct(heading);
            
            //進行方向前方にCircleがあるか？
            if (dotProd > 0) {
                feeler = heading.multiply(_avoidDistance);
                projection = heading.multiply(dotProd);
                dist = projection.subtract(difference).length;
                
                //衝突するか？
                if (dist < c.radius + _avoidBuffer && projection.length < feeler.length) {
                    force = heading.multiply(_maxSpeed);
                    force.angle += difference.sign(_velocity) * Math.PI / 2;
                    
                    force = force.multiply(1.0 - projection.length / feeler.length);
                    
                    _steeringForce  = _steeringForce.add(force);
                    
                    //スピードを落とす
                    _velocity = _velocity.multiply(projection.length / feeler.length);
                }
                
            }
        }
    }
}

class Circle extends Sprite {
    private var _radius:Number;
    private var _color:uint;
    public function Circle(radius:Number, color:uint = 0x000000) {
        _radius = radius;
        _color = color;
        graphics.lineStyle(0, _color);
        graphics.drawCircle(0, 0, _radius);
        tabChildren=false;
    }
    
    public function get radius():Number { return _radius; }
    public function set radius(value:Number):void 
    {
        _radius = value;
    }
    public function get position():Vector2D {
        return new Vector2D(x, y);
    }
}