/// <summary>
/// Tells the radar to no longer ignore the detectable object when filtering the vehicles or objects detected
/// </summary>
/// <param name="o">
/// An object to remove from the ignore list<see cref="DetectableObject"/>
/// </param>
/// <returns>The radar</returns>
public Radar DontIgnore(DetectableObject o)
{
if (_ignoredObjects.Contains(o))
{
_ignoredObjects.Remove(o);
}
return(this);
}
/// <summary>
/// Tells the radar to ignore the detectable object when filtering the vehicles or objects detected
/// </summary>
/// <param name="o">
/// An object to be ignored<see cref="DetectableObject"/>
/// </param>
/// <returns>The radar</returns>
public Radar Ignore(DetectableObject o)
{
if (o != null)
{
_ignoredObjects.Add(o);
}
return(this);
}
public PathIntersection(DetectableObject obstacle)
{
_obstacle = obstacle;
_intersect = false;
_distance = float.MaxValue;
}
/// <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(Vehicle vehicle, Vector3 futureVehiclePosition, DetectableObject obstacle) {
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
float combinedRadius = vehicle.ScaledRadius + obstacle.ScaledRadius;
var movement = futureVehiclePosition - vehicle.Position;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehicle.Position;
// this is the length of vehicleToObstacle projected onto direction
float projectionLength = Vector3.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
float 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)
float 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
float intersectionPoint1Distance = (intersectionPoint1 - vehicle.Position).magnitude;
float intersectionPoint2Distance = (intersectionPoint2 - vehicle.Position).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return intersection;
}
/// <summary>
/// Tells the radar to ignore the detectable object when filtering the vehicles or objects detected
/// </summary>
/// <param name="o">
/// An object to be ignored<see cref="DetectableObject"/>
/// </param>
/// <returns>The radar</returns>
public Radar Ignore(DetectableObject o)
{
if (o != null)
{
_ignoredObjects.Add(o);
}
return this;
}
/// <summary>
/// Tells the radar to no longer ignore the detectable object when filtering the vehicles or objects detected
/// </summary>
/// <param name="o">
/// An object to remove from the ignore list<see cref="DetectableObject"/>
/// </param>
/// <returns>The radar</returns>
public Radar DontIgnore(DetectableObject o)
{
if (_ignoredObjects.Contains(o))
{
_ignoredObjects.Remove(o);
}
return this;
}
public PathIntersection(DetectableObject obstacle)
{
_obstacle = obstacle;
_intersect = false;
_distance = float.MaxValue;
}
/// <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(Vehicle vehicle, Vector3 futureVehiclePosition, DetectableObject obstacle)
{
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
float combinedRadius = vehicle.ScaledRadius + obstacle.ScaledRadius;
var movement = futureVehiclePosition - vehicle.Position;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehicle.Position;
// this is the length of vehicleToObstacle projected onto direction
float projectionLength = Vector3.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
float 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)
float 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
float intersectionPoint1Distance = (intersectionPoint1 - vehicle.Position).magnitude;
float intersectionPoint2Distance = (intersectionPoint2 - vehicle.Position).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return(intersection);
}
