/// <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 Intersection(DetectableObject obstacle)
{
_obstacle = obstacle;
_intersect = false;
_distance = float.MaxValue;
_point = Vector3.zero;
_normal = Vector3.zero;
}
public bool isVisible(DetectionZone detectionZone, DetectableObject target)
{
Collider[] overlaps = Physics.OverlapSphere(transform.position, detectionZone.radius);
foreach (Collider overlap in overlaps)
{
if (overlap != null)
{
if (overlap.transform == target.dObjectTransform)
{
if (detectionZone.FOVDependable)
{
Vector3 directionBetween = (target.dObjectTransform.position - transform.position).normalized;
directionBetween.y *= 0;
float angle = Vector3.Angle(transform.forward, directionBetween);
if (angle <= FOVAngle / 2)
{
Ray ray = new Ray(transform.position, target.dObjectTransform.position - transform.position);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, detectionZone.radius))
{
if (hit.transform == target.dObjectTransform)
{
detectionZone.zoneHit = true;
target.isDetected = true;
return(true);
}
}
}
}
else
{
Ray ray = new Ray(transform.position, target.dObjectTransform.position - transform.position);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, detectionZone.radius))
{
if (hit.transform == target.dObjectTransform)
{
detectionZone.zoneHit = true;
target.isDetected = true;
return(true);
}
}
}
}
}
}
target.isDetected = false;
return(false);
}
/// <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 the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehiclePosition">
/// The current position of the vehicle
/// </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>
public PathIntersection FindNextIntersectionWithSphere(Vector3 vehiclePosition, 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 - vehiclePosition;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehiclePosition;
// 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 = vehiclePosition + 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 - vehiclePosition).magnitude;
return(intersection);
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
float intersectionPoint1Distance = (intersectionPoint1 - vehiclePosition).magnitude;
float intersectionPoint2Distance = (intersectionPoint2 - vehiclePosition).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return(intersection);
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehiclePosition">
/// The current position of the vehicle
/// </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>
public PathIntersection FindNextIntersectionWithSphere(Vector3 vehiclePosition, 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 - vehiclePosition;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehiclePosition;
// 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 = vehiclePosition + 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 - vehiclePosition).magnitude;
return intersection;
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
float intersectionPoint1Distance = (intersectionPoint1 - vehiclePosition).magnitude;
float intersectionPoint2Distance = (intersectionPoint2 - vehiclePosition).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return intersection;
}
public PathIntersection(DetectableObject obstacle)
{
_obstacle = obstacle;
_intersect = false;
_distance = float.MaxValue;
}
protected bool DoesFutureDirectionIntersect(Vector3 vFutureDirection, out Intersection collisionInfo)
{
Vector3 vFutureMemoryPos = Vehicle.Position + (vFutureDirection * FlyingVehicle.SSpeed.Current * m_memory.TimeToLookAhead);
Vector3 vFuturePos = Vehicle.Position + (vFutureDirection * FlyingVehicle.SSpeed.Current * _estimationTime);
float fClosestSqrDist = 0;
Intersection closestInter = new Intersection(Vehicle);
//0. Check our memory for anything that needs to be checked so that we avoid looking odd.
for (int i = 0; i < m_memory.PrevHitObjs.Count; ++i)
{
DetectableObject obj = m_memory.PrevHitObjs[i].m_prevHit;
Intersection intersection = obj.FindNextIntersection(Vehicle.Position, vFutureMemoryPos, Vehicle.ScaledRadius);
if (intersection.Intersect)
{
float fTempSqrDist = (intersection.Point - Vehicle.Position).sqrMagnitude;
if (Mathf.Approximately(0, fClosestSqrDist) || (fTempSqrDist < fClosestSqrDist))
{
closestInter = intersection;
fClosestSqrDist = fTempSqrDist;
}
}
else
{
m_memory.UpdateDetectable(i);
}
}
//1. Check the radar for anything that needs to be ensured that we avoid
for (int nObsCount = 0; nObsCount < Vehicle.Radar.Obstacles.Count; ++nObsCount)
{
var obstacle = Vehicle.Radar.Obstacles[nObsCount];
if (null == obstacle || obstacle.Equals(null) || m_memory.DoesDetectableObjectExist(obstacle))
{
continue;
}
Intersection intersection = obstacle.FindNextIntersection(Vehicle.Position, vFuturePos, Vehicle.ScaledRadius);
if (intersection.Intersect)
{
float fTempSqrDist = (intersection.Point - Vehicle.Position).sqrMagnitude;
if (Mathf.Approximately(0, fClosestSqrDist) || (fTempSqrDist < fClosestSqrDist))
{
closestInter = intersection;
fClosestSqrDist = fTempSqrDist;
}
}
}
collisionInfo = closestInter;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, collisionInfo.Point, Color.red);
Debug.DrawLine(Vehicle.Position, vFuturePos, Color.white);
Debug.DrawLine(Vehicle.Position, vFutureMemoryPos, Color.blue);
#endif
return(closestInter.Intersect);
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <param name="line">
/// Line that we expect we'll follow to our future destination
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere (DetectableObject obs, Vector3 line)
{
/*
* This routine is based on the Paul Bourke's derivation in:
* Intersection of a Line and a Sphere (or circle)
* http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/
*
* Retaining the same variable values used in that description.
*
*/
float a, b, c, bb4ac;
var toCenter = Vehicle.Position - obs.Position;
// initialize pathIntersection object
var intersection = new PathIntersection(obs);
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
#endif
// computer line-sphere intersection parameters
a = line.magnitude;
b = 2 * Vector3.Dot(line, toCenter);
c = obs.Position.magnitude;
c += Vehicle.Position.magnitude;
c -= 2 * Vector3.Dot(obs.Position, Vehicle.Position);
c -= Mathf.Pow(obs.ScaledRadius + Vehicle.ScaledRadius, 2);
bb4ac = b * b - 4 * a * c;
if (bb4ac >= 0) {
intersection.Intersect = true;
Vector3 closest = Vector3.zero;
if (bb4ac == 0) {
// Only one intersection
var mu = -b / (2*a);
closest = mu * line;
}
else {
// More than one intersection
var mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2*a);
var mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2*a);
/*
* If the results are negative, the obstacle is behind us.
*
* If one result is negative and the other one positive,
* that would indicate that one intersection is behind us while
* the other one ahead of us, which would mean that we're
* just overlapping the obstacle, so we should still avoid.
*/
if (mu1 < 0 && mu2 < 0)
intersection.Intersect = false;
else
closest = (Mathf.Abs(mu1) < Mathf.Abs (mu2)) ? mu1 * line : mu2 * line;
}
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawRay(Vehicle.Position, closest, Color.red);
#endif
intersection.Distance = closest.magnitude;
}
return intersection;
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <param name="line">
/// Line that we expect we'll follow to our future destination
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere(DetectableObject obs, Vector3 line)
{
/*
* This routine is based on the Paul Bourke's derivation in:
* Intersection of a Line and a Sphere (or circle)
* http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/
*
* Retaining the same variable values used in that description.
*
*/
float a, b, c, bb4ac;
var toCenter = Vehicle.Position - obs.Position;
// initialize pathIntersection object
var intersection = new PathIntersection(obs);
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
#endif
// computer line-sphere intersection parameters
a = line.magnitude;
b = 2 * Vector3.Dot(line, toCenter);
c = obs.Position.magnitude;
c += Vehicle.Position.magnitude;
c -= 2 * Vector3.Dot(obs.Position, Vehicle.Position);
c -= Mathf.Pow(obs.ScaledRadius + Vehicle.ScaledRadius, 2);
bb4ac = b * b - 4 * a * c;
if (bb4ac >= 0)
{
intersection.Intersect = true;
Vector3 closest = Vector3.zero;
if (bb4ac == 0)
{
// Only one intersection
var mu = -b / (2 * a);
closest = mu * line;
}
else
{
// More than one intersection
var mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2 * a);
var mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2 * a);
/*
* If the results are negative, the obstacle is behind us.
*
* If one result is negative and the other one positive,
* that would indicate that one intersection is behind us while
* the other one ahead of us, which would mean that we're
* just overlapping the obstacle, so we should still avoid.
*/
if (mu1 < 0 && mu2 < 0)
{
intersection.Intersect = false;
}
else
{
closest = (Mathf.Abs(mu1) < Mathf.Abs(mu2)) ? mu1 * line : mu2 * line;
}
}
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawRay(Vehicle.Position, closest, Color.red);
#endif
intersection.Distance = closest.magnitude;
}
return(intersection);
}
