RVO (on working)
A try to implement the "Recipropal Velocity Obstacle"
TODO
Adjust the velocity of the agent to proper value
source: http://gamma.cs.unc.edu/RVO/icra2008.pdf
/**
* Copyright wrotenodoc ( http://wonderfl.net/user/wrotenodoc )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/3HOY
*/
package {
import flash.display.Sprite
import flash.geom.Point
import flash.events.Event
public class RVO extends Sprite {
private var A:Agent, B:Agent, mark:Sprite
public function RVO() {
// write as3 code here..
addChild(debug)
mark = new Sprite
mark.graphics.lineStyle(2, 0x00ff00)
mark.graphics.moveTo(-10, 0)
mark.graphics.lineTo(10, 0)
mark.graphics.moveTo(0, -10)
mark.graphics.lineTo(0, 10)
addChild(mark)
A = new Agent(30)
A.x = A.y = 50
addChild(A)
mark.x = 400
mark.y = 200
A.setVelocity(mark.x - A.x, mark.y - A.y)
B = new Agent(40, 0x0000ff)
B.x = B.y = 200
addChild(B)
stage.addEventListener("mouseDown", setTarget)
addEventListener("enterFrame", loop)
}
private function setTarget(e:Event):void {
mark.x = mouseX
mark.y = mouseY
A.setVelocity(mark.x - A.x, mark.y - A.y)
}
private function loop(e:Event):void {
var dt:Number = 1 / 60
A.update(dt)
if(Point.distance(new Point(A.x, A.y), new Point(mark.x, mark.y)) < 20){
A.setVelocity(0, 0)
}
B.update(dt)
// y = m(x - x0) + y0
var VO:VelocityObstacle = A.calculateVelocityObstacle(B)
graphics.clear()
graphics.lineStyle(1, 0x000000)
graphics.moveTo(VO.x, VO.y)
graphics.lineTo(VO.x0, VO.y0)
graphics.moveTo(VO.x, VO.y)
graphics.lineTo(VO.x1, VO.y1)
graphics.lineStyle()
graphics.beginFill(0x00ff00, 0.5)
graphics.drawCircle(B.x, B.y, A.radius + B.radius)
// contact point
graphics.lineStyle(1)
graphics.beginFill(0xffff00)
graphics.drawCircle(VO.x0, VO.y0, 5)
graphics.drawCircle(VO.x1, VO.y1, 5)
if(VO.contains(A.velocity.x, A.velocity.y)){
log("they will collide ")
var px:Number = A.velocity.x, py:Number = A.velocity.y
var dx:Number = A.x + A.velocity.x - B.x
var dy:Number = A.y + A.velocity.y - B.y
var d:Number = (A.radius + A.radius + B.radius + B.radius) / Math.sqrt(dx*dx + dy*dy)
dx *= d ; dy *= d
A.setVelocity(B.x + dx - A.x, B.y + dy - A.y)
A.lockVelocity(10)
graphics.lineStyle(1, 0xff0000)
graphics.moveTo(A.x, A.y)
graphics.lineTo(B.x + dx, B.y + dy)
}else{
log("peaceful")
A.setVelocity(mark.x - A.x, mark.y - A.y)
}
}
}
}
import flash.text.TextField
var debug:TextField = new TextField
debug.autoSize = "left"
function log(...args):void {
var s:String = ""
for each(var arg:Object in args){
s += arg.toString() + "\n"
}
debug.text = s
}
import flash.display.Sprite
import flash.geom.Point
class Agent extends Sprite {
public var radius:Number, color:uint
public var velocity:Point, speed:Number = 100
public var velocityLock:int = 0
public function Agent(radius0:Number, color0:uint=0xff0000) {
radius = radius0
color = color0
velocity = new Point
render()
}
public function setVelocity(dirx:Number, diry:Number):void {
if(velocityLock > 0) return
velocity.x = dirx
velocity.y = diry
velocity.normalize(speed)
}
public function lockVelocity(delay:int):void {
if(velocityLock <= 0){
velocityLock = delay
}
}
public function update(dt:Number):void {
if(velocityLock > 0) velocityLock --
x += velocity.x * dt
y += velocity.y * dt
render()
}
public function render():void {
graphics.clear()
graphics.beginFill(color)
graphics.drawCircle(0, 0, radius)
graphics.endFill()
graphics.lineStyle(2, color, 0.6)
graphics.moveTo(0, 0)
graphics.lineTo(velocity.x, velocity.y)
}
public function calculateVelocityObstacle(obstacle:Agent):VelocityObstacle {
var dx:Number = this.x - obstacle.x
var dy:Number = this.y - obstacle.y
var r:Number = this.radius + obstacle.radius
var dx2:Number = dx * dx, dy2:Number = dy * dy
var r2:Number = r * r
var VO:VelocityObstacle =
new VelocityObstacle(this.x, this.y, this.radius, obstacle.x, obstacle.y, obstacle.radius)
var a:Number = r*Math.sqrt(r2*dx2 - (dx2+dy2)*(r2-dy2))
VO.x0 = (r2*dx + a) / (dx2 + dy2)
VO.y0 = (r2 - dx*VO.x0) / dy
VO.x1 = (r2*dx - a) / (dx2 + dy2)
VO.y1 = (r2 - dx*VO.x1) / dy
VO.x0 += obstacle.x ; VO.y0 += obstacle.y
VO.x1 += obstacle.x ; VO.y1 += obstacle.y
return VO
}
}
class VelocityObstacle {
public var x:Number, y:Number
public var cx:Number, cy:Number, r:Number
public var x0:Number, y0:Number // solution1
public var x1:Number, y1:Number // solution2
// (x0, y0) = agent position
public function VelocityObstacle(x0:Number, y0:Number, r0:Number, cx0:Number, cy0:Number, r1:Number) {
x = x0 ; y = y0
cx = cx0 ; cy = cy0 ; r = r0 + r1
}
// does this VO includes a velocity (vx, vy)?
public function contains(vx:Number, vy:Number):Boolean {
var speed:Number = Math.sqrt(vx*vx + vy*vy)
var dx:Number = cx - x, dy:Number = cy - y
var d:Number = Math.sqrt(dx*dx + dy*dy)
var close:Boolean = (d - r) <= speed
var f:Point = new Point(x1 - x, y1 - y)
var g:Point = new Point(x0 - x, y0 - y)
var v:Point = new Point(vx, vy)
f.normalize(1) ; g.normalize(1) ; v.normalize(1)
// 1. the order is (fx, fy) -> (vx, vy) -> (gx, gy)
// 2. the order is (fx, fy) <- (vx, vy) <- (gx, gy)
// If 1 or 2 holds, this VO contains (vx, vy)
var detF:Number = f.x*v.y - f.y*v.x
var detG:Number = v.x*g.y - v.y*g.x
var order:Boolean = ((detF >= 0 && detG >= 0) || (detF <= 0 && detG <= 0))
var angle:Boolean = f.x*v.x + f.y*v.y >= 0
return close && order && angle
}
}