public static Vector2 GetVelocity(SteeringAgent2D agent, Vector2 targetPoint, float slowingDistance = 1f)
{
if (slowingDistance < 0)
{
throw new InvalidOperationException("The slowing distance can't be negative.");
}
//var toTarget = targetPoint - agent.Rigidbody2D.position;
//var distance = toTarget.magnitude;
//if (distance <= 0) {
// return new Vector2(0, 0);
//} else {
// var rampedSpeed = agent.MaxSpeed * (distance / slowingDistance);
// var clippedSpeed = Mathf.Min(rampedSpeed, agent.MaxSpeed);
// Vector2 desiredVelocity = (clippedSpeed / distance) * toTarget;
// return (desiredVelocity - agent.Rigidbody2D.velocity);
//}
var toTarget = targetPoint - agent.Rigidbody2D.position;
if (toTarget.magnitude <= 0)
{
return(new Vector2(0, 0));
}
else
{
return((Mathf.Min(agent.MaxSpeed * (toTarget.magnitude / slowingDistance), agent.MaxSpeed) / toTarget.magnitude) * toTarget - agent.Rigidbody2D.velocity);
}
}
public static Vector2 GetVelocity(SteeringAgent2D agent, float feelerLength)
{
var feelers = CreateFeelers(agent, feelerLength);
Vector2 steeringForce = Vector2.zero;
foreach (var feeler in feelers)
{
int currentWallIndex = 0;
int closestWallIndex = -1;
float distToThisIP = 0f;
float distToClosestIP = float.MaxValue;
Vector2 point = Vector2.zero;
Vector2 closestIP = Vector2.zero;
Vector2 feelerSteeringForce = Vector2.zero;
foreach (var wall in World2D.Instance.Walls)
{
try {
Vector2 feelerFrom = agent.transform.position;
Vector2 feelerTo = feeler;
//if (MathfExtensions.SegmentIntersection2D(feelerFrom, feelerTo, wall.From, wall.To, out distToThisIP, out point)) {
if (MathfExtensions.LineSegementsIntersect(feelerFrom, feelerTo, wall.From, wall.To, out point))
{
distToThisIP = (feelerFrom - point).magnitude;
if (distToThisIP < distToClosestIP)
{
distToClosestIP = distToThisIP;
closestWallIndex = currentWallIndex;
closestIP = point;
}
}
currentWallIndex++;
} catch {
//Do nothing
}
} //Next wall
if (closestWallIndex >= 0)
{
Vector2 overShoot = (feeler - closestIP) * agent.Rigidbody2D.velocity.magnitude;
var closestWall = World2D.Instance.Walls[closestWallIndex];
var feelerDirection = feeler - agent.Rigidbody2D.position;
if (Vector2.Angle(feelerDirection, closestWall.Normal) > 90)
{
feelerSteeringForce = closestWall.Normal * overShoot.magnitude;
}
else
{
feelerSteeringForce = closestWall.InverseNormal * overShoot.magnitude;
}
}
steeringForce += feelerSteeringForce;
} //Next feeler
return(steeringForce);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, IEnumerable <SteeringAgent2D> neighbors)
{
Vector2 centerOfMass = Vector2.zero, steeringForce = Vector2.zero;
foreach (var neighbor in neighbors)
{
if (neighbor == agent)
{
continue;
}
if (agent.TargetAgents.Contains(neighbor))
{
continue; //Ignore the target of evasion or pursue
}
centerOfMass += neighbor.Rigidbody2D.position;
}
int neighborCount = neighbors.Count();
if (neighborCount > 0)
{
centerOfMass /= (float)neighborCount;
steeringForce = Seek2D.GetVelocity(agent, centerOfMass);
}
return(steeringForce.normalized);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, SteeringAgent2D target, float hidingDistanceFromObstacle, float slowingDistance = 1f, float panicDistance = -1)
{
if (panicDistance > 0)
{
var sqrDistance = (agent.Rigidbody2D.position - target.GetComponent <Rigidbody2D>().position).magnitude;
if (sqrDistance > panicDistance)
{
return(Vector2.zero);
}
}
float distToClosest = float.MaxValue;
Vector2 bestHidingSpot = Vector2.zero;
foreach (var obstacle in World2D.Instance.Obstacles)
{
var hidingSpot = GetHidingPosition(obstacle.transform.position, obstacle.radius, target.GetComponent <Rigidbody2D>().position, hidingDistanceFromObstacle);
var dist = (hidingSpot - agent.Rigidbody2D.position).magnitude;
if (dist < distToClosest)
{
distToClosest = dist;
bestHidingSpot = hidingSpot;
}
}
if (distToClosest == float.MaxValue)
{
return(Evade2D.GetVelocity(agent, target));
}
return(Arrive2D.GetVelocity(agent, bestHidingSpot, slowingDistance));
}
public static float TurnaroundTime(SteeringAgent2D agent, Vector2 targetPos, float turnCoefficient)
{
Vector2 toTarget = (targetPos - agent.Rigidbody2D.position).normalized;
var dot = Vector2.Dot(agent.Heading, toTarget);
return((dot - 1) * -turnCoefficient);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, SteeringAgent2D evader, float turnCoefficient = 0f)
{
if (evader == null)
{
return(Vector2.zero);
}
var toEvader = evader.GetComponent <Rigidbody2D>().position - agent.Rigidbody2D.position;
float relativeHeading = Vector2.Dot(agent.Heading, evader.transform.up);
if ((Vector2.Dot(toEvader, agent.transform.up) > 0) &&
relativeHeading < -0.95) //acos(0.95) = 18 degs
{
return(Seek2D.GetVelocity(agent, evader.GetComponent <Rigidbody2D>().position));
}
//Not considered ahead so we predict where the evader will be
float lookAheadTime = toEvader.magnitude / (agent.MaxSpeed + evader.GetComponent <Rigidbody2D>().velocity.magnitude);
if (turnCoefficient != 0)
{
lookAheadTime += TurnaroundTime(agent, evader.GetComponent <Rigidbody2D>().position, turnCoefficient);
}
return(Seek2D.GetVelocity(agent, evader.GetComponent <Rigidbody2D>().position + evader.GetComponent <Rigidbody2D>().velocity *lookAheadTime));
}
public static Vector2 GetVelocity(SteeringAgent2D agent, SteeringAgent2D target, Vector2 offset, float slowingDistance = 1f)
{
Vector2 worldOffsetPos = target.transform.TransformPoint(offset);
Vector2 toOffset = worldOffsetPos - agent.Rigidbody2D.position;
float lookAheadTime = toOffset.magnitude / (agent.MaxSpeed + target.GetComponent <Rigidbody2D>().velocity.magnitude);
return(Arrive2D.GetVelocity(agent, worldOffsetPos + target.GetComponent <Rigidbody2D>().velocity *lookAheadTime, slowingDistance));
}
static Vector2[] CreateFeelers(SteeringAgent2D agent, float feelerLength)
{
var feelers = new Vector2[3];
feelers[0] = agent.transform.TransformPoint(Vector2.up * feelerLength);
feelers[1] = agent.transform.TransformPoint(Quaternion.AngleAxis(-SIDE_FEELER_ANGLE, Vector3.back) * Vector2.up * feelerLength * SIDE_FEELER_PROPORTION);
feelers[2] = agent.transform.TransformPoint(Quaternion.AngleAxis(SIDE_FEELER_ANGLE, Vector3.back) * Vector2.up * feelerLength * SIDE_FEELER_PROPORTION);
return(feelers);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, Vector2 targetPoint, float panicDistance = -1)
{
if (panicDistance > 0 && (targetPoint - agent.Rigidbody2D.position).sqrMagnitude > panicDistance * panicDistance)
{
return(new Vector2(0, 0));
}
Vector2 desiredVelocity = (agent.Rigidbody2D.position - targetPoint).normalized * agent.MaxSpeed;
return(desiredVelocity - agent.Rigidbody2D.velocity);
}
public void UpdateEntity(SteeringAgent2D entity, Vector2 previousPosition)
{
Vector2 currentPosition = entity.GetComponent <Rigidbody2D>().position;
int prevIndex = IndexFromPosition(previousPosition);
int curIndex = IndexFromPosition(currentPosition);
if (prevIndex != curIndex)
{
Cells[prevIndex].Members.Remove(entity);
Cells[curIndex].Members.Add(entity);
}
}
public static Vector2 GetVelocity(SteeringAgent2D agent, SteeringAgent2D pursuer, float panicDistance = -1)
{
if (pursuer == null)
{
return(Vector2.zero);
}
var toPursuer = pursuer.GetComponent <Rigidbody2D>().position - agent.Rigidbody2D.position;
if (panicDistance != -1 && toPursuer.sqrMagnitude > panicDistance * panicDistance)
{
return(Vector2.zero);
}
//Not considered ahead so we predict where the evader will be
float lookAheadTime = toPursuer.magnitude / (agent.MaxSpeed + pursuer.GetComponent <Rigidbody2D>().velocity.magnitude);
return(Flee2D.GetVelocity(agent, pursuer.GetComponent <Rigidbody2D>().position + pursuer.GetComponent <Rigidbody2D>().velocity *lookAheadTime));
}
public static Vector2 GetVelocity(SteeringAgent2D agent, IEnumerable <SteeringAgent2D> neighbors)
{
Vector2 steeringForce = Vector2.zero;
foreach (var neighbor in neighbors)
{
if (neighbor == agent)
{
continue;
}
if (agent.TargetAgents.Contains(neighbor))
{
continue; //Ignore the target of evasion or pursue
}
var toAgent = agent.Rigidbody2D.position - neighbor.Rigidbody2D.position;
steeringForce += toAgent.normalized * (agent.NeighborRadius / 8) / toAgent.magnitude;
}
return(steeringForce);
}
public static Vector2 GetVelocity(SteeringAgent2D thisAgent, SteeringAgent2D agentA, SteeringAgent2D agentB, float slowingDistance = 1f)
{
if (agentA == null || agentB == null)
{
return(Vector2.zero);
}
if (slowingDistance < 0)
{
throw new InvalidOperationException("The slowing distance can't be negative.");
}
Vector2 midPoint = (agentA.GetComponent <Rigidbody2D>().position + agentB.GetComponent <Rigidbody2D>().position) / 2;
float timeToReachMidPoint = (thisAgent.Rigidbody2D.position - midPoint).magnitude / thisAgent.MaxSpeed;
//Position in the future
Vector2 AFuturePos = agentA.GetComponent <Rigidbody2D>().position + agentA.GetComponent <Rigidbody2D>().velocity *timeToReachMidPoint;
Vector2 BFuturePos = agentB.GetComponent <Rigidbody2D>().position + agentB.GetComponent <Rigidbody2D>().velocity *timeToReachMidPoint;
midPoint = (AFuturePos + BFuturePos) / 2;
return(Arrive2D.GetVelocity(thisAgent, midPoint, slowingDistance));
}
public static Vector2 GetVelocity(SteeringAgent2D agent, NavMesh2D navMesh, float waypointSeekDistance = 0.5f)
{
if (navMesh == null || !navMesh.CurrentWaypoint.HasValue)
{
return(Vector2.zero);
}
var currentWaypoint = navMesh.CurrentWaypoint.Value;
if ((agent.Rigidbody2D.position - currentWaypoint).magnitude < waypointSeekDistance)
{
navMesh.SetNextWaypoint();
}
if (!navMesh.IsFinished())
{
return(Seek2D.GetVelocity(agent, navMesh.CurrentWaypoint.Value));
}
else
{
return(Arrive2D.GetVelocity(agent, navMesh.CurrentWaypoint.Value, 2f));
}
}
public static Vector2 GetVelocity(SteeringAgent2D agent, IEnumerable <SteeringAgent2D> neighbors)
{
Vector2 avgHeading = Vector2.zero;
foreach (var neighbor in neighbors)
{
if (neighbor == agent)
{
continue; //Ignore the same agent
}
if (agent.TargetAgents.Contains(neighbor))
{
continue; //Ignore the target of evasion or pursue
}
avgHeading += neighbor.Heading;
}
if (neighbors.Count() > 0)
{
avgHeading = avgHeading / (float)agent.Neighbors.Count() - agent.Heading;
}
return(avgHeading);
}
void Awake()
{
agent = GetComponent <SteeringAgent2D>();
}
public void AddEntity(SteeringAgent2D entity)
{
int index = IndexFromPosition(entity.transform.position);
Cells[index].Members.Add(entity);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, List <CircleCollider2D> obstacles, float minDetectionBoxLength = 1)
{
var boxLength = minDetectionBoxLength +
(agent.Rigidbody2D.velocity.magnitude / agent.MaxSpeed) *
minDetectionBoxLength;
var candidates = obstacles.Where(o => (o.transform.position - agent.transform.position).magnitude < boxLength);
CircleCollider2D closestIntersectingObstacle = null;
float distanceToClosestIO = float.MaxValue;
Vector2 localPosOfClosestObstacle = Vector2.zero;
foreach (var obstacle in candidates)
{
Vector2 localPos = agent.transform.InverseTransformPoint(obstacle.transform.position);
//If y is negative, then they are behind us, and not important
if (localPos.y >= 0)
{
float expandedRadius = obstacle.radius + agent.Radius;
if (Mathf.Abs(localPos.x) < expandedRadius)
{
float cX = localPos.x;
float cY = localPos.y;
float sqrtPart = Mathf.Sqrt(expandedRadius * expandedRadius - cX * cX);
float ip = cY - sqrtPart;
if (ip <= 0)
{
ip = cY + sqrtPart;
}
if (ip < distanceToClosestIO)
{
distanceToClosestIO = ip;
closestIntersectingObstacle = obstacle;
localPosOfClosestObstacle = localPos;
}
}
}
}
Vector2 steering = Vector2.zero;
if (closestIntersectingObstacle != null)
{
//float multiplier = 1f + (boxLength - localPosOfClosestObstacle.y) / boxLength;
//steering.x = (closestIntersectingObstacle.radius - Mathf.Abs(localPosOfClosestObstacle.x)) * multiplier * -Mathf.Sign(localPosOfClosestObstacle.x);
//const float BRAKING_WEIGHT = 0.2f;
//steering.y = (closestIntersectingObstacle.radius - localPosOfClosestObstacle.y) * BRAKING_WEIGHT;
//steering = agent.transform.TransformDirection(steering);
//var ahead = agent.rigidbody2D.velocity.normalized * distanceToClosestIO;
//var avoidance = ahead - (Vector2)(closestIntersectingObstacle.transform.position - agent.transform.position);
//steering = avoidance.normalized * agent.MaxForce;
//TODO -> Implement better version
steering = -new Vector2(localPosOfClosestObstacle.x * 4, localPosOfClosestObstacle.y / 2);
if (localPosOfClosestObstacle.magnitude > closestIntersectingObstacle.radius)
{
steering = steering.normalized * agent.MaxForce / distanceToClosestIO;
}
steering = agent.transform.TransformDirection(steering);
Debug.DrawLine(closestIntersectingObstacle.transform.position, (Vector2)closestIntersectingObstacle.transform.position + steering, Color.yellow);
}
return(steering);
}
public static Vector2 GetVelocity(SteeringAgent2D agent, Vector2 targetPoint)
{
//Vector2 desiredVelocity = (targetPoint - agent.Rigidbody2D.position).normalized * agent.MaxSpeed;
//return (desiredVelocity - agent.Rigidbody2D.velocity);
return((targetPoint - agent.Rigidbody2D.position).normalized * agent.MaxSpeed - agent.Rigidbody2D.velocity);
}
