45 lines
1.1 KiB
Plaintext
45 lines
1.1 KiB
Plaintext
|
function LCA(vel_vec) {
|
||
|
|
var safety_radius = 3.0
|
||
|
|
var default_velocity = 2.0
|
||
|
|
collide = 0
|
||
|
|
|
||
|
|
var k_v = 5 # velocity gain
|
||
|
|
var k_w = 10 # angular velocity gain
|
||
|
|
|
||
|
|
cart = neighbors.map(
|
||
|
|
function(rid, data) {
|
||
|
|
var c = {}
|
||
|
|
c.distance = data.distance
|
||
|
|
c.azimuth = data.azimuth
|
||
|
|
if (c.distance < (safety_radius * 2.0) )
|
||
|
|
collide = 1
|
||
|
|
return c
|
||
|
|
})
|
||
|
|
if (collide) {
|
||
|
|
log("")
|
||
|
|
log("------> AVOIDING NEIGHBOR! <------")
|
||
|
|
log("")
|
||
|
|
result = cart.reduce(function(rid, data, accum) {
|
||
|
|
if(data.distance < accum.distance and data.distance > 0.0){
|
||
|
|
accum.distance = data.distance
|
||
|
|
accum.angle = data.azimuth
|
||
|
|
return accum
|
||
|
|
}
|
||
|
|
return accum
|
||
|
|
}, {.distance= safety_radius * 2.0, .angle= 0.0})
|
||
|
|
|
||
|
|
d_i = result.distance
|
||
|
|
data_alpha_i = result.angle
|
||
|
|
|
||
|
|
penalty = math.exp(d_i - safety_radius)
|
||
|
|
if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
|
||
|
|
penalty = math.exp(-(d_i - safety_radius))
|
||
|
|
}
|
||
|
|
|
||
|
|
V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
|
||
|
|
W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
|
||
|
|
|
||
|
|
return math.vec2.newp(V, W)
|
||
|
|
} else
|
||
|
|
return vel_vec
|
||
|
|
}
|