/// <summary>
/// Given the time until nearest approach (predictNearestApproachTime)
/// determine position of each vehicle at that time, and the distance
/// between them
/// </summary>
/// <returns>
/// Distance between positions
/// </returns>
/// <param name='other'>
/// Other vehicle to compare against
/// </param>
/// <param name='time'>
/// Time to estimate.
/// </param>
/// <param name='ourPosition'>
/// Our position.
/// </param>
/// <param name='hisPosition'>
/// The other vehicle's position.
/// </param>
public float ComputeNearestApproachPositions(Vehicle2D other, float time,
ref Vector2 ourPosition,
ref Vector2 hisPosition)
{
return(ComputeNearestApproachPositions(other, time, ref ourPosition, ref hisPosition, Speed,
Forward));
}
/// <summary>
/// Predicts the time until nearest approach between this and another vehicle
/// </summary>
/// <returns>
/// The nearest approach time.
/// </returns>
/// <param name='other'>
/// Other vehicle to compare against
/// </param>
public float PredictNearestApproachTime(Vehicle2D other)
{
// imagine we are at the origin with no velocity,
// compute the relative velocity of the other vehicle
var otherVelocity = other.Velocity;
var relVelocity = otherVelocity - Velocity;
var relSpeed = relVelocity.magnitude;
// for parallel paths, the vehicles will always be at the same distance,
// so return 0 (aka "now") since "there is no time like the present"
if (Mathf.Approximately(relSpeed, 0))
{
return(0);
}
// Now consider the path of the other vehicle in this relative
// space, a line defined by the relative position and velocity.
// The distance from the origin (our vehicle) to that line is
// the nearest approach.
// Take the unit tangent along the other vehicle's path
var relTangent = relVelocity / relSpeed;
// find distance from its path to origin (compute offset from
// other to us, find length of projection onto path)
var relPosition = (Vector2)transform.position - other.Position;
var projection = Vector2.Dot(relTangent, relPosition);
return(projection / relSpeed);
}
/// <summary>
/// Calculates if a vehicle is in the neighborhood of another
/// </summary>
/// <param name="other">
/// Another vehicle to check against<see cref="Vehicle"/>
/// </param>
/// <param name="minDistance">
/// Minimum distance <see cref="System.Single"/>
/// </param>
/// <param name="maxDistance">
/// Maximum distance <see cref="System.Single"/>
/// </param>
/// <param name="cosMaxAngle">
/// Cosine of the maximum angle between vehicles (for performance)<see cref="System.Single"/>
/// </param>
/// <returns>
/// True if the other vehicle can be considered to our neighbor, or false if otherwise<see cref="System.Boolean"/>
/// </returns>
/// <remarks>Originally SteerLibrary.inBoidNeighborhood</remarks>
public bool IsInNeighborhood(Vehicle2D other, float minDistance, float maxDistance, float cosMaxAngle)
{
var result = false;
if (other != this)
{
var offset = other.Position - (Vector2)transform.position;
var distanceSquared = offset.sqrMagnitude;
// definitely in neighborhood if inside minDistance sphere
if (distanceSquared < (minDistance * minDistance))
{
result = true;
}
else
{
// definitely not in neighborhood if outside maxDistance sphere
if (distanceSquared <= (maxDistance * maxDistance))
{
// otherwise, test angular offset from forward axis
var unitOffset = offset / Mathf.Sqrt(distanceSquared);
var forwardness = Vector2.Dot(Forward, unitOffset);
result = forwardness > cosMaxAngle;
}
}
}
return(result);
}
/// <summary>
/// Given the time until nearest approach (predictNearestApproachTime)
/// determine position of each vehicle at that time, and the distance
/// between them
/// </summary>
/// <returns>
/// Distance between positions
/// </returns>
/// <param name='other'>
/// Other vehicle to compare against
/// </param>
/// <param name='time'>
/// Time to estimate.
/// </param>
/// <param name='ourPosition'>
/// Our position.
/// </param>
/// <param name='hisPosition'>
/// The other vehicle's position.
/// </param>
/// <param name="ourSpeed">Our speed to use for the calculations</param>
/// <param name='ourForward'>
/// Forward vector to use instead of the vehicle's.
/// </param>
public float ComputeNearestApproachPositions(Vehicle2D other, float time,
ref Vector2 ourPosition,
ref Vector2 hisPosition,
float ourSpeed,
Vector2 ourForward)
{
var myTravel = ourForward * ourSpeed * time;
var otherTravel = other.Forward * other.Speed * time;
//The casts are to make sure they are both the same even when changing from 2D to 3D.
ourPosition = (Vector2)transform.position + myTravel;
hisPosition = other.Position + otherTravel;
return(Vector2.Distance(ourPosition, hisPosition));
}
/// <summary>
/// Returns the distance from this vehicle to another
/// </summary>
/// <returns>
/// The distance between both vehicles' positions. If negative, they are overlapping.
/// </returns>
/// <param name='other'>
/// Vehicle to compare against.
/// </param>
public float DistanceFromPerimeter(Vehicle2D other)
{
var diff = Position - other.Position;
return(diff.magnitude - Radius - other.Radius);
}
protected virtual void Awake()
{
_vehicle = GetComponent<Vehicle2D>();
ReportedArrival = true; // Default to true to avoid unnecessary notifications
}
/// <summary>
/// Finds a vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehicle">
/// The vehicle to evaluate.
/// </param>
/// <param name="futureVehiclePosition">
/// The position where we expect the vehicle to be soon
/// </param>
/// <param name="obstacle">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
/// <remarks>We could probably spin out this function to an independent tool class</remarks>
public static PathIntersection FindNextIntersectionWithSphere(Vehicle2D vehicle, Vector2 futureVehiclePosition,
DetectableObject2D obstacle)
{
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
var combinedRadius = vehicle.Radius + obstacle.Radius;
var movement = futureVehiclePosition - vehicle.Position;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehicle.Position;
// this is the length of vehicleToObstacle projected onto direction
var projectionLength = Vector2.Dot(direction, vehicleToObstacle);
// if the projected obstacle center lies further away than our movement + both radius, we're not going to collide
if (projectionLength > movement.magnitude + combinedRadius)
{
//print("no collision - 1");
return(intersection);
}
// the foot of the perpendicular
var projectedObstacleCenter = vehicle.Position + projectionLength * direction;
// distance of the obstacle to the pathe the vehicle is going to take
var obstacleDistanceToPath = (obstacle.Position - projectedObstacleCenter).magnitude;
//print("obstacleDistanceToPath: " + obstacleDistanceToPath);
// if the obstacle is further away from the movement, than both radius, there's no collision
if (obstacleDistanceToPath > combinedRadius)
{
//print("no collision - 2");
return(intersection);
}
// use pythagorean theorem to calculate distance out of the sphere (if you do it 2D, the line through the circle would be a chord and we need half of its length)
var halfChord = Mathf.Sqrt(combinedRadius * combinedRadius + obstacleDistanceToPath * obstacleDistanceToPath);
// if the projected obstacle center lies opposite to the movement direction (aka "behind")
if (projectionLength < 0)
{
// behind and further away than both radius -> no collision (we already passed)
if (vehicleToObstacle.magnitude > combinedRadius)
{
return(intersection);
}
var intersectionPoint = projectedObstacleCenter - direction * halfChord;
intersection.Intersect = true;
intersection.Distance = (intersectionPoint - vehicle.Position).magnitude;
return(intersection);
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
var intersectionPoint1Distance = (intersectionPoint1 - vehicle.Position).magnitude;
var intersectionPoint2Distance = (intersectionPoint2 - vehicle.Position).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return(intersection);
}
