Path Following Behavior Test
...
@author Motoki Matsumoto
/**
* Copyright mtok ( http://wonderfl.net/user/mtok )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/n0V0
*/
// forked from mtok's Seek Behavior Test
package
{
import flash.display.Sprite;
import flash.events.Event;
import flash.display.StageAlign;
import flash.display.StageScaleMode;
import flash.events.MouseEvent;
/**
* ...
* @author Motoki Matsumoto
*/
public class PathFollowingBehaviorTest extends Sprite
{
private var _vehicle:Vehicle;
private var _path:Array;
public function PathFollowingBehaviorTest ()
{
addEventListener(Event.ADDED_TO_STAGE, addedToStageHandler);
}
private function addedToStageHandler(e:Event):void
{
removeEventListener(Event.ADDED_TO_STAGE, addedToStageHandler);
init();
}
private function init():void {
stage.align = StageAlign.TOP_LEFT;
stage.scaleMode = StageScaleMode.NO_SCALE;
_vehicle = new Vehicle();
addChild(_vehicle);
_path = [];
stage.addEventListener(MouseEvent.CLICK, clickHandler);
addEventListener(Event.ENTER_FRAME, enterFrameHandler);
}
private function clickHandler(e:MouseEvent):void
{
graphics.lineStyle(0, 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));
}
private function enterFrameHandler(e:Event):void
{
_vehicle.followPath(_path, true);
_vehicle.update();
}
}
}
import flash.display.Graphics;
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 + ", )]";
}
}
import flash.display.Sprite;
import flash.display.Graphics;
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 = 10;
private var _wanderRange:Number = 3;
private var _avoidDistance:Number = 100;
private var _avoidBuffer:Number = 10;
private var _pathIndex:int = 0;
private var _pathThreshold:Number = 20;
public function Vehicle() {
_steeringForce = new Vector2D();
_position = new Vector2D();
_velocity = new Vector2D();
_edgeBehavior = wrap;
draw();
}
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 pathIndex():int { return _pathIndex; }
public function set pathIndex(value:int):void
{
_pathIndex = value;
}
public function get pathThreshold():Number { return _pathThreshold; }
public function set pathThreshold(value:Number):void
{
_pathThreshold = 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);
}
}
}
}
public function followPath(path:Array, loop:Boolean = false):void {
var wayPoint:Vector2D = path[_pathIndex] as Vector2D;
if (wayPoint == null) return;
if (_position.distance(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 set radius(value:Number):void
{
_radius = value;
}
public function get position():Vector2D {
return new Vector2D(x, y);
}
}