public BoidNode[] GetUnitNeighbours(BoidNode boid, bool radialNeighbourhood = false, float radius = 1)
{
List <BoidNode> neighbours = new List <BoidNode>();
int radiusInt = (int)Mathf.Ceil(radius);
for (int x = -radiusInt; x <= radiusInt; x++)
{
for (int y = -radiusInt; y <= radiusInt; y++)
{
if ((!radialNeighbourhood && new Vector2(x, y).magnitude > radius))
{
continue;
}
Node node = NodeFromWorldPoint(boid.position);
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
HashSet <BoidNode> neighboursXY = units[checkX, checkY];
foreach (BoidNode neighbour in neighboursXY)
{
if (boid.id != neighbour.id)
{
neighbours.Add(neighbour);
}
}
}
}
}
return(neighbours.ToArray());
}
void MoveTo(BoidNode currentWaypoint)
{
node = boid.node;
// Influence of next checkpoint
Vector3 steeringFromTarget = boid.VelocityTowards(currentWaypoint.position, 0, 0.5f);
neighbourUnits = Grid.instance.GetUnitNeighbours(boid, false, 3);
Vector3 steeringFromUnits = Flocker.ComputeSeparationForce(boid, neighbourUnits, radius, 1);
Vector3 steeringFromSurroundings = ComputeSteeringFromObstacles(node, 1, 10);
// Compose
Vector3 steering = Vector3.ClampMagnitude(steeringFromTarget + steeringFromUnits + steeringFromSurroundings, steeringFromTarget.magnitude);
steering = steering / boid.mass;
// Floor influence
float movementRatio = 1 - node.boidNode.penalty;
boid.velocity = Vector3.ClampMagnitude(boid.velocity + steering, boid.maxSpeed * movementRatio);
Grid.instance.RemoveUnit(boid);
boid.position = boid.predictPosition(Time.deltaTime);
Grid.instance.RegisterUnit(boid);
transform.position = boid.position;
}
public static Vector3 ComputeAlignmentForce(BoidNode boid, BoidNode[] neighbourUnits, float maxRadius, float maxForce)
{
Vector3 force = new Vector3();
Vector3 velocity = new Vector3();
int neighborCount = 0;
foreach (BoidNode b in neighbourUnits)
{
if (b == boid)
{
continue;
}
float distance = boid.DistanceBetweenBorders(b);
if (b != boid && distance < maxRadius)
{
velocity += b.velocity * (1 - distance / maxRadius) * boid.mass;
neighborCount++;
}
}
if (neighborCount != 0)
{
velocity = velocity / neighborCount;
force.Normalize();
force *= maxForce;
}
return(velocity);
}
public static Vector3 ComputeSeparationForce(BoidNode boid, BoidNode[] neighbourUnits, float maxRadius, float maxForce)
{
Vector3 force = new Vector3();
int neighborCount = 0;
foreach (BoidNode b in neighbourUnits)
{
if (b == boid)
{
continue;
}
float distance = boid.DistanceBetweenBorders(b);
if (b != boid && distance < maxRadius)
{
force += (boid.position - b.position) * (1 - distance / maxRadius) * boid.mass;
neighborCount++;
}
}
if (neighborCount != 0)
{
force = force / (neighborCount);
force.Normalize();
force *= maxForce;
}
// Assume all the neigbours will make an equivalent effort
return(force);
}
public static Vector3 ComputeCohesionForce(BoidNode boid, BoidNode[] neighbourUnits, float maxRadius, float maxForce)
{
Vector3 force = new Vector3();
Vector3 centroid = new Vector3();
int neighborCount = 0;
foreach (BoidNode b in neighbourUnits)
{
if (b == boid)
{
continue;
}
float distance = boid.DistanceBetweenBorders(b);
if (b != boid && distance < maxRadius)
{
centroid += b.position * boid.mass;
neighborCount++;
}
}
if (neighborCount != 0)
{
centroid = centroid / neighborCount;
float distance = boid.DistanceFromBorder(centroid);
force = (boid.position - centroid) * Mathf.Max(1, distance / maxRadius);
force.Normalize();
force *= maxRadius;
}
return(force);
}
public bool WillCollide(BoidNode other, out Vector3 avoidanceSpeed, out Vector3 collisionPoint, int subSamples = 3)
{
if (this != other)
{
aheadVector = velocity.normalized * maxSeeAhead * velocity.magnitude / maxSpeed * Time.deltaTime;
float distance = float.MaxValue;
for (int sample = 0; sample <= subSamples; sample++)
{
Vector3 aheadPosition = position + aheadVector * ((float)sample) / subSamples;
float currentDistance = other.DistanceFromBorder(aheadPosition) - this.hardRadius;
if (currentDistance <= 0)
{
distance = currentDistance;
collisionPoint = aheadPosition;
avoidanceSpeed = collisionPoint - other.position;
avoidanceSpeed = -avoidanceSpeed.normalized * maxAvoidForce;
return(true);
}
else if (currentDistance < distance)
{
distance = currentDistance;
}
}
}
avoidanceSpeed = Vector3.zero;
collisionPoint = Vector3.zero;
return(false);
}
public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY, int _penalty)
{
walkable = _walkable;
worldPosition = _worldPos;
gridX = _gridX;
gridY = _gridY;
movementPenalty = _penalty;
pathfindingNode = new PathfindingNode(this);
float normalizedPenalty = ((float)movementPenalty) / MaxMovementPenalty;
if (walkable)
{
boidNode = new BoidNode(this, normalizedPenalty);
}
else
{
float radius = 0.5f;
boidNode = new BoidNode(this, radius, normalizedPenalty, true);
//boidNode.softRadius = radius;
//boidNode.visibilityRadius = radius;
}
}
public Vector3 VelocityAwayFrom(BoidNode other)
{
float distance;
Vector3 otherPosition = InterceptPosition(other, out distance);
Vector3 desiredVelocity = MaxSpeed(-(otherPosition - this.position));
return(desiredVelocity);
}
public void RegisterUnit(BoidNode boid)
{
Node node = NodeFromWorldPoint(boid.position);
boid.node = node;
units[node.gridX, node.gridY].Add(boid);
//print ("Added Boid at ["+ node.gridX +"," + node.gridY+"] total:"+units[node.gridX,node.gridY].Count);
}
public Vector3 ComputeInfluence(BoidNode boid, BoidNode[] neighbourUnits, float separationFactor, float cohesionFactor, float alignmentFactor)
{
Vector3 separation = ComputeSeparationForce(boid, neighbourUnits, separationRadius, maxSeparationForce);
Vector3 cohesion = ComputeCohesionForce(boid, neighbourUnits, cohesionRadius, maxCohesionForce);
Vector3 alignment = ComputeAlignmentForce(boid, neighbourUnits, AlignmentRadius, maxAlignmentForce);
Vector3 unitForce = separation * separationFactor + cohesion * cohesionFactor + alignment * alignmentFactor;
return(unitForce);
}
public void RemoveUnit(BoidNode boid)
{
Node node = NodeFromWorldPoint(boid.position);
boid.node = null;
units[node.gridX, node.gridY].Remove(boid);
//print ("Removed Boid at ["+ node.gridX +"," + node.gridY+"] total:"+units[node.gridX,node.gridY].Count);
}
Vector3 InterceptPosition(BoidNode other, out float distance)
{
Vector3 otherPosition = other.position;
distance = DistanceFromBorder(other);
float currentSpeed = velocity.magnitude;
if (other.IsMoving() && currentSpeed > 0)
{
// See where the other will be when this one arrives to that point)
otherPosition = other.predictPosition(distance / currentSpeed);
// from now on, the target position is that one, so we need to update the distance
distance = DistanceFromBorder(otherPosition);
}
return(otherPosition);
}
public Vector3 VelocityTowards(BoidNode other)
{
float distance;
Vector3 otherPosition = InterceptPosition(other, out distance);
Vector3 desiredVelocity = MaxSpeed(otherPosition - this.position);
//Arrival
if (distance < other.softRadius && distance >= other.hardRadius)
{
desiredVelocity *= ((distance - other.hardRadius) / (other.softRadius - other.hardRadius));
}
if (distance < other.hardRadius)
{
desiredVelocity = -velocity;
}
return(desiredVelocity);
}
public override bool Equals(System.Object obj)
{
// If parameter is null return false.
if (obj == null)
{
return(false);
}
// If parameter cannot be cast to Point return false.
BoidNode b = obj as BoidNode;
if ((System.Object)b == null)
{
return(false);
}
// Return true if the fields match:
return(b.id == this.id);
}
protected override void Initialize()
{
boid = new BoidNode(transform.position, radius);
transform.localScale = Vector3.one * radius * 2;
flock = new Flocker();
}
public void chase(BoidNode targetBoid)
{
PathRequestManager.RequestPath(boid.position, targetBoid.position, OnPathFound);
}
public int GetHashCode(BoidNode n)
{
return(n.id);
}
public float DistanceBetweenBorders(BoidNode other)
{
float distance = Vector3.Distance(other.position, this.position);
return(distance - this.hardRadius - other.hardRadius);
}
void Start()
{
Grid.instance.RegisterUnit(boid);
targetBoid = target.GetComponent <Unit>().boid;
}
protected override void Initialize()
{
boid = new BoidNode(transform.position, radius);
}
