Commit 9816cfc9 authored by Chris Jewell's avatar Chris Jewell
Browse files

Initial commit of flocks!

parents
Package: flock
Type: Package
Title: What the Package Does (Title Case)
Version: 0.1.0
Author: Who wrote it
Maintainer: The package maintainer <yourself@somewhere.net>
Description: More about what it does (maybe more than one line)
Use four spaces when indenting paragraphs within the Description.
License: What license is it under?
Encoding: UTF-8
LazyData: true
RoxygenNote: 6.0.1
# Generated by roxygen2: do not edit by hand
export(flock)
# Flock algorithm
triangle = matrix(c(-0.5,0.5,-0.5,0.3,0,-0.3),ncol=2)
propagate = function(x,v,delta_t)
{
x = x + v*delta_t
x
}
magnitude = function(x)
{
sqrt(sum(x^2))
}
normalize = function(x)
{
x = as.matrix(x)
xprime = apply(x,1,function(xx) {
m = magnitude(xx)
if (m > 0) xx = xx / m
xx
})
t(xprime)
}
getNeighbours = function(x,r)
{
# Calculate mean of neighbours
D = dist(x)
neighbours = apply(as.matrix(D), 2, function(d) d > 0 & d < r)
}
cohere = function(x,v,r,max_speed=0.2)
{
neighbours = getNeighbours(x,r)
meanP = t(apply(neighbours, 2, function(n) apply(x[n,,drop=F],2,mean)))
# Calculate change in velocity
desired = ifelse(is.nan(meanP), c(0,0), meanP-x)
d = apply(desired, 1, function(dd) magnitude(dd))
desired = normalize(desired)
dv = t(sapply(1:length(d), function(i) {
if (d[i] == 0) c(0,0)
else if (d[i] < 1) max_speed * d[i] * desired[i,] - v[i,]
else max_speed*desired[i,] - v[i,]
}))
dv
}
alignment = function(x,v,r,max_force=0.01)
{
neighbours = getNeighbours(x,r)
meanV = t(apply(neighbours,2,function(n) apply(v[n,,drop=F],2,mean)))
meanV[is.nan(meanV)] = 0
if(any(is.nan(meanV))) browser()
pmin(meanV,max_force)
}
separation = function(x,r)
{
D = as.matrix(dist(x))
dv = matrix(nrow=ncol(D), ncol=2)
for(i in 1:ncol(D)) {
neighbours = D[,i] > 0 & D[,i] < r
xNeighbours = x[neighbours,,drop=F]
if(nrow(xNeighbours)==0) dv[i,] = 0
else {
dvv = apply(xNeighbours,1,function(xn) normalize(x[i,]-xn))
dv[i,] = apply(dvv,1,function(z) {
mean(z/D[neighbours,i])
})
}
}
dv
}
boundary = function(x,v,xmax,ymax,strength=5)
{
dv = v
for(i in 1:nrow(x)) {
xi = x[i,]
vi = v[i,]
boundaries = c(xmax,ymax)*(vi > 0)
t = (boundaries - xi) / vi
hitDim = which.min(t)
hitDist = magnitude(t[hitDim]*vi)
vReflect = vi
vReflect[hitDim] = vReflect[hitDim]*-1
dv[i,] = vReflect / hitDist^strength
}
dv
}
plotBoids = function(x,v,xmax,ymax,sz=xmax/50) {
dev.hold()
plot(c(0,xmax),c(0,ymax), type='n', ylab='',xlab='', asp=1, xaxt='n',yaxt='n')
v = normalize(v)
tri = triangle * sz
for(i in 1:nrow(x)) {
rotate = matrix(c(v[i,1],v[i,2],-v[i,2],v[i,1]),ncol=2)
pt = t(t(tri%*%t(rotate)) + x[i,])
polygon(pt,col=1)
}
dev.flush()
}
wraparound = function(x,xmax,ymax)
{
r = 0.0
x = apply(x, 1, function(xx) {
if(xx[1] < -r) xx[1] = xmax+r
if(xx[2] < -r) xx[2] = ymax+r
if(xx[1] > xmax+r) xx[1] = -r
if(xx[2] > ymax+r) xx[2] = -r
xx
})
t(x)
}
#' Simulate flocking
#'
#' This function simulates flocking using the algorithm of
#' Reynolds 1986.
#'
#' @param n the number of agents
#' @param the timestep
#' @param distance a vector of distances over which between-agent effects work c(alignment, separation, coherence).
#' @param weight a vector of weights in the same order as distance
#' @param delay a delay to impose in the simulation loop to allow graphics to catch up.
#'
#' @export
flock = function(n=100,delta_t=0.05,distance=c(1,0.3,1),weight=c(0.5,0.05,0.1),random=0,boundary=1, delay=0.0)
{
x = matrix(runif(n*2, min=2, max=8),ncol=2)
v = matrix(runif(n*2,min=-1,max=1),ncol=2)
xmax = 10
ymax = 10
dev.flush()
while(TRUE) {
dvAlign = weight[1] * alignment(x,v,distance[1],max_force=1)
dvSep = weight[2] * separation(x,distance[2])
dvCohere= weight[3] * cohere(x,v,distance[3])
dvBoundary = boundary(x,v,xmax,ymax)
nugget = runif(prod(dim(v)), min=-1,max=1)*random
v = v + dvAlign + dvSep + dvCohere + dvBoundary + nugget
v = normalize(v) # Normalise velocity
if(any(v==0)) browser()
x = propagate(x,v,delta_t)
x = wraparound(x,xmax,ymax)
plotBoids(x, v, xmax, ymax)
Sys.sleep(delay)
}
}
# Test arguments
# 1. Cohesion only
# weight=c(0.0, 0.00, 0.6)
#
# 2. Cohesion + alignment
# weight = c(0.3, 0.0, 0.6)
#
# 3. Cohesion + alignment + separation
# weight = c(0.3, 0.05, 0.6)
#
# 4. Cohesion + alignment + separation + random
# weight = c(0.3, 0.05, 0.6), random = 0.6
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment