VehicleFlockTest

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

// forked from fakestar0826's VehiclePersueTest
// forked from fakestar0826's VehicleSeekTest
// forked from fakestar0826's VehicleTest
package {
    import flash.events.MouseEvent;
    import flash.events.Event;
    import flash.display.StageScaleMode;
    import flash.display.StageAlign;
    import flash.display.Sprite;
    public class SeekTest extends Sprite 
    {
        private var _target:SteeredVehicle;
        private var _seeker:SteeredVehicle;
        private var _persuer:SteeredVehicle;
        
        private var _circles:Array;
        private var _numCircles:int = 10;
        
        private var _path:Array;
        
        private var _vehicles:Array;
        private var _numVehicles:int = 10;
        
        public function SeekTest() 
        {
            // write as3 code here..
            stage.align = StageAlign.TOP_LEFT;
            stage.scaleMode = StageScaleMode.NO_SCALE;
            
            _vehicles = new Array();
            for(var i:int = 0;i < _numVehicles;i++)
            {
                var vehicle:SteeredVehicle = new SteeredVehicle();
                addChild(vehicle);
                vehicle.position = new Vector2D(Math.random() * stage.stageWidth, Math.random() * stage.stageHeight);
                vehicle.velocity = new Vector2D(Math.random() * 20 - 10, Math.random() * 20 - 10);
                vehicle.edgeBehavior = Vehicle.WRAP;
                _vehicles.push(vehicle);
            }

            addEventListener(Event.ENTER_FRAME, onEnterFrame);
        }
        
        private function onEnterFrame(e:Event):void
        {
            //少ないと正常に群行動する。（アルゴリズムのせいか、ステージの広さに対するオブジェクトの数なのか）多いと一定の間隔で数体が一箇所に固まる。
            //群行動時、大体楕円形のフォーメーションをとる。不思議。
            for(var i:int = 0;i < _numVehicles;i++)
            {
                _vehicles[i].flock(_vehicles);
                _vehicles[i].update();
            }

        }
        
        private function protRoute(e:MouseEvent):void
        {
            graphics.lineStyle(0, 0, 25);
            if(_path.length == 0)
            {
                graphics.moveTo(mouseX, mouseY);
            }
            graphics.lineTo(mouseX, mouseY);
            graphics.drawCircle(mouseX, mouseY, 10);
            graphics.moveTo(mouseX, mouseY);
            _path.push(new Vector2D(mouseX, mouseY));
        }

    }
}



import flash.display.Graphics;
    
class Vector2D
{
    public var _x:Number;
    public var _y:Number;
    
    public function Vector2D(x:Number = 0, y:Number = 0) 
    {
        _x = x;
        _y = y;
    }
    
    public function draw(graphics:Graphics, color:uint = 0):void
    {
        graphics.lineStyle(0, color);
        graphics.moveTo(0, 0);
        graphics.lineTo(_x, _y);
    }
    
    public function clone():Vector2D
    {
        return new Vector2D(x, y);
    }
    
    public function zero():Vector2D
    {
        _x = 0;
        _y = 0;
        return this;
    }
    
    public function isZero():Boolean
    {
        return _x == 0 && _y == 0;
    }
    
    public function set length(value:Number):void
    {
        var a:Number = angle;
        _x = Math.cos(a);
        _y = Math.sin(a);
    }
    
    public function get length():Number
    {
        return Math.sqrt(lengthSQ);
    }
    
    public function get lengthSQ():Number
    {
        return _x * _x + _y * _y;
    }

    public function set angle(value:Number):void
    {
        var len:Number = length;
        _x = Math.cos(value) * len;
        _y = Math.sin(value) * len;
    }
    
    public function get angle():Number
    {
        return Math.atan2(_y, _x);
    }
    
    public function normalize():Vector2D
    {
        if(length == 0)
        {
            _x = 1;
            return this;
        }
        
        var len:Number = length;
        _x /= len;
        _y /= len;
        return this;
    }
    
    public function truncate(max:Number):Vector2D
    {
       length = Math.min(max, length);
       return this;
    }
    
    public function reverse():Vector2D
    {
        _x = -_x;
        _y = -_y;
        return this;
    }
    
    public function isNormalized():Boolean
    {
        return length == 1.0;
    }
    
    public function dotProd(v2:Vector2D):Number
    {
        return _x * v2.x + _y * v2.y;
    }
    
    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.dotProd(v2));
    }
    
    public function sign(v2:Vector2D):int
    {
        return perp.dotProd(v2) < 0 ? -1 : 1;
    }
    
    public function get perp():Vector2D
    {
        return new Vector2D(-y, x);
    }
    
    public function dist(v2:Vector2D):Number
    {
        return Math.sqrt(distSQ(v2));
    }
    
    public function distSQ(v2:Vector2D):Number
    {
        var dx:Number = v2.x - x;
        var dy:Number = v2.y - y;
        return dx * dx + dy * dy;
    }
    
    public function add(v2:Vector2D):Vector2D
    {
        return new Vector2D(_x + v2.x, _y + v2.y);
    }
    
    public function subtract(v2:Vector2D):Vector2D
    {
        return new Vector2D(_x - v2.x, _y - v2.y);
    }
    
    public function multiply(value:Number):Vector2D
    {
        return new Vector2D(_x * value, _y * value);
    }
    
    public function divide(value:Number):Vector2D
    {
        return new Vector2D(_x / value, _y / value);
    }
    
    public function equals(v2:Vector2D):Boolean
    {
        return _x == v2.x && _y == v2.y;
    }
    
    public function set x(value:Number):void
    {
        _x = value;
    }

    public function get x():Number
    {
        return _x;
    }
    
    public function set y(value:Number):void
    {
        _y = value;
    }

    public function get y():Number
    {
        return _y;
    }
    
    public function toString():String
    {
        return "[Vector2D (x:" + _x + ", y:" + _y + ")]";
    }
}


import flash.display.Sprite;
class Vehicle extends Sprite
{
    protected var _edgeBehavior:String = WRAP;
    protected var _mass:Number = 1.0;
    protected var _maxSpeed:Number = 10;
    protected var _position:Vector2D;
    protected var _velocity:Vector2D;
    
    public static const WRAP:String = "wrap";
    public static const BOUNCE:String = "bounce";
        
    public function Vehicle():void
    {
        _position = new Vector2D();
        _velocity = new Vector2D();
        draw();
    }
    
    protected function draw():void
    {
        graphics.clear();
        graphics.lineStyle(0);
        graphics.moveTo(10, 0);
        graphics.lineTo(-10, 5);
        graphics.lineTo(-10, -5);
        graphics.lineTo(10, 0);
    }
    
    public function update():void
    {
        _velocity.truncate(_maxSpeed);
        
        _position = _position.add(_velocity);
        
        if(_edgeBehavior == WRAP)
        {
            wrap();
        }
        else if(_edgeBehavior == BOUNCE)
        {
            bounce();
        }
        
        x = _position.x;
        y = _position.y;
        
        rotation = _velocity.angle * 180 / Math.PI;

    }
    
    private function bounce():void
    {
        if(stage != null)
        {
            if(_position.x > stage.stageWidth)
            {
                _position.x = stage.stageWidth;
                _velocity.x *= -1;
            }
            else if(_position.x < 0)
            {
                _position.x = 0;
                _velocity.x *= -1;
            }

            if(_position.y > stage.stageHeight)
            {
                _position.y = stage.stageHeight;
                _velocity.y *= -1;
            }
            else if(_position.y < 0)
            {
                _position.y = 0;
                _velocity.y *= -1;
            }
        }

    }
    
    private function wrap():void
    {
        if(stage != null)
        {
            if(_position.x > stage.stageWidth) _position.x = 0;
            if(_position.x < 0) _position.x = stage.stageWidth;
            if(_position.y > stage.stageHeight) _position.y = 0;
            if(_position.y < 0) _position.y = stage.stageHeight;
        }

    }
    
    public function set edgeBehavior(value:String):void
    {
        _edgeBehavior = value;
    }
    public function get edgeBehavior():String
    {
        return _edgeBehavior;
    }
    
    public function set mass(value:Number):void
    {
        _mass = value;
    }
    public function get mass():Number
    {
        return _mass;
    }
    
    public function set maxSpeed(value:Number):void
    {
        _maxSpeed = value;
    }
    public function get maxSpeed():Number
    {
        return _maxSpeed;
    }
    
    public function set position(value:Vector2D):void
    {
        _position = value;
        x = _position.x;
        y = _position.y;
    }
    public function get position():Vector2D
    {
        return _position
    }
    
    public function set velocity(value:Vector2D):void
    {
        _velocity = value;
    }
    public function get velocity():Vector2D
    {
        return _velocity;
    }
    
    override public function set x(value:Number):void
    {
        super.x = value;
        _position.x = x;
    }
    override public function set y(value:Number):void
    {
        super.y = value;
        _position.y = y;
    }


}


import flash.display.Sprite;
class SteeredVehicle extends Vehicle
{
    private var _maxForce:Number = 1;
        private var _steeringForce:Vector2D;
        private var _arrivalThreshold:Number = 200;
        private var _wanderAngle:Number = 0;
        private var _wanderDistance:Number = 20;
        private var _wanderRadius:Number = 30;
        private var _wanderRange:Number = 50;
        private var _pathIndex:int = 0;
        private var _pathThreshold:Number = 20;
        private var _avoidDistance:Number = 300;
        private var _avoidBuffer:Number = 20;
        private var _inSightDist:Number = 200;
        private var _tooCloseDist:Number = 60;
        
        public function SteeredVehicle()
        {
            _steeringForce = new Vector2D();
            super();
        }
        
        public function set maxForce(value:Number):void
        {
            _maxForce = value;
        }
        public function get maxForce():Number
        {
            return _maxForce;
        }
        
        public function set arriveThreshold(value:Number):void
        {
            _arrivalThreshold = value;
        }
        public function get arriveThreshold():Number
        {
            return _arrivalThreshold;
        }
        
        public function set wanderDistance(value:Number):void
        {
            _wanderDistance = value;
        }
        public function get wanderDistance():Number
        {
            return _wanderDistance;
        }
        
        public function set wanderRadius(value:Number):void
        {
            _wanderRadius = value;
        }
        public function get wanderRadius():Number
        {
            return _wanderRadius;
        }
        
        public function set wanderRange(value:Number):void
        {
            _wanderRange = value;
        }
        public function get wanderRange():Number
        {
            return _wanderRange;
        }
        
        public function set pathIndex(value:int):void
        {
            _pathIndex = value;
        }
        public function get pathIndex():int
        {
            return _pathIndex;
        }
        
        public function set pathThreshold(value:Number):void
        {
            _pathThreshold = value;
        }
        public function get pathThreshold():Number
        {
            return _pathThreshold;
        }
        
        public function set avoidDistance(value:Number):void
        {
            _avoidDistance = value;
        }
        public function get avoidDistance():Number
        {
            return _avoidDistance;
        }
        
        public function set avoidBuffer(value:Number):void
        {
            _avoidBuffer = value;
        }
        public function get avoidBuffer():Number
        {
            return _avoidBuffer;
        }
        
        public function set inSightDist(value:Number):void
        {
            _inSightDist = value;
        }
        public function get inSightDist():Number
        {
            return _inSightDist;
        }
        
        public function set tooCloseDist(value:Number):void
        {
            _tooCloseDist = value;
        }
        public function get tooCloseDist():Number
        {
            return _tooCloseDist;
        }
        
        override public function update():void
        {
            _steeringForce.truncate(_maxForce);
            _steeringForce = _steeringForce.divide(_mass);
            _velocity = _velocity.add(_steeringForce);
            _steeringForce = new Vector2D();
            super.update();
        }
        
        //対象を探す。頭を対象に向ける。
        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.dist(target);
            if(dist > _arrivalThreshold)
            {//未到着？
                desiredVelocity = desiredVelocity.multiply(_maxSpeed)
            }
            else
            {//到着？
                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.dist(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.dist(target.position) / _maxSpeed;
            var predictedTarget:Vector2D = target.position.add(target.velocity.multiply(lookAheadTime));
            flee(predictedTarget);
        }
        
        public function wander():void
        {
            var center:Vector2D = velocity.clone().normalize().multiply(_wanderDistance);
            var offset:Vector2D = new Vector2D(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 flock(vehicles:Array):void
        {
            var averageVelocity:Vector2D = _velocity.clone();
            var averagePosition:Vector2D = new Vector2D();
            var inSightCount:int = 0;
            for (var i:int = 0; i < vehicles.length; i++)
            {
                var vehicle:Vehicle = vehicles[i] as Vehicle;
                //見えた！
                if (vehicle != this && inSight(vehicle))
                {
                    averageVelocity = averageVelocity.add(vehicle.velocity);
                    averagePosition = averagePosition.add(vehicle.position);
                    //近すぎ！
                    if (tooClose(vehicle)) flee(vehicle.position);
                    inSightCount++;
                }
            }
            if (inSightCount > 0)
            {
                averageVelocity = averageVelocity.divide(inSightCount);
                averagePosition = averagePosition.divide(inSightCount);
                seek(averagePosition);
                _steeringForce.add(averageVelocity.subtract(_velocity));
            }
        }
        
        public function inSight(vehicle:Vehicle):Boolean
        {
            if(_position.dist(vehicle.position) > _inSightDist) return false;
            
            var heading:Vector2D = _velocity.clone().normalize();//現在地からの進行方向
            var difference:Vector2D = vehicle.position.subtract(_position);//他のオブジェクトとの位置の差
            var dotProd:Number = difference.dotProd(heading);//位置の差からベクトルの内積をとる。
            
            if(dotProd < 0) return false;    //視野内に他のオブジェクトがいない場合
            return true;                    //視野内に他のオブジェクトがいた場合
        }
        
        public function tooClose(vehicle:Vehicle):Boolean
        {
            return _position.dist(vehicle.position) < _tooCloseDist;
        }
        
        public function avoid(circles:Array):void
        {
            for(var i:int = 0;i < circles.length; i++)
            {
                var circle:Circle = circles[i] as Circle;
                var heading:Vector2D = _velocity.clone().normalize();
                
                var difference:Vector2D = circle.position.subtract(_position);
                var dotProd:Number = difference.dotProd(heading);
                
                if(dotProd > 0)
                {
                    var feeler:Vector2D = heading.multiply(_avoidDistance);
                    var projection:Vector2D = heading.multiply(dotProd);
                    var dist:Number = projection.subtract(difference).length;
                    
                    if(dist < circle.radius + _avoidBuffer && projection.length < feeler.length)
                    {
                        var force:Vector2D = 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);
                    }

                }

            }

        }
        
        public function followPath(path:Array, loop:Boolean = false):void
        {
            var wayPoint:Vector2D = path[_pathIndex];
            if(wayPoint == null)
            {
                return;
            }
            
            if(_position.dist(wayPoint) < _pathThreshold)
            {
                if(_pathIndex >= path.length - 1)
                {
                    if(loop)
                    {
                        _pathIndex = 0;
                    }

                }
                else
                {
                    _pathIndex++;
                }

            }
            if(_pathIndex >= path.length - 1 && !loop)
            {
                arrive(wayPoint);
            }
            else
            {
                seek(wayPoint);
            }


        }

        
}

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);
    }
    
    public function get radius():Number
    {
        return _radius;
    }
    
    public function get position():Vector2D
    {
        return new Vector2D(x, y);
    }


}