public override Vector3 CalculateNeighborContribution(Vehicle other)
{
// accumulate sum of forces leading us towards neighbor's positions
var distance = other.Position - Vehicle.Position;
var sqrMag = distance.sqrMagnitude;
// Provide some contribution, but diminished by the distance to
// the vehicle.
distance *= 1 / sqrMag;
return distance;
}
public override Vector3 CalculateNeighborContribution(Vehicle other)
{
var steering = Vector3.zero;
// add in steering contribution
// (opposite of the offset direction, divided once by distance
// to normalize, divided another time to get 1/d falloff)
var offset = other.Position - Vehicle.Position;
var offsetSqrMag = offset.sqrMagnitude;
steering = (offset / -offsetSqrMag);
if (!Mathf.Approximately(_multiplierInsideComfortDistance, 1) && offsetSqrMag < _comfortDistanceSquared)
{
steering *= _multiplierInsideComfortDistance;
}
if (_vehicleRadiusImpact > 0)
{
steering *= (other.Radius + Vehicle.Radius) * _vehicleRadiusImpact;
}
return steering;
}
public override Vector3 CalculateNeighborContribution(Vehicle other)
{
// accumulate sum of neighbors' velocities
return other.Velocity;
}
/// <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);
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 = 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
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;
}
public abstract Vector3 CalculateNeighborContribution(Vehicle other);
public override Vector3 CalculateNeighborContribution(Vehicle other)
{
// accumulate sum of neighbor's heading
return other.Transform.forward;
}
/// <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(Vehicle other, float time,
ref Vector3 ourPosition,
ref Vector3 hisPosition,
float ourSpeed,
Vector3 ourForward)
{
var myTravel = ourForward * ourSpeed * time;
var otherTravel = other.Transform.forward * other.Speed * time;
ourPosition = Position + myTravel;
hisPosition = other.Position + otherTravel;
return Vector3.Distance(ourPosition, hisPosition);
}
/// <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(Vehicle other, float time,
ref Vector3 ourPosition,
ref Vector3 hisPosition)
{
return ComputeNearestApproachPositions(other, time, ref ourPosition, ref hisPosition, Speed,
Transform.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(Vehicle 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 = Position - other.Position;
var projection = Vector3.Dot(relTangent, relPosition);
return projection / relSpeed;
}
/// <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(Vehicle other)
{
var diff = Position - other.Position;
return diff.magnitude - Radius - other.Radius;
}
/// <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(Vehicle other, float minDistance, float maxDistance, float cosMaxAngle)
{
var result = false;
if (other != this)
{
var offset = other.Position - 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 = Vector3.Dot(Transform.forward, unitOffset);
result = forwardness > cosMaxAngle;
}
}
}
return result;
}
protected virtual void Awake()
{
_vehicle = GetComponent<Vehicle>();
ReportedArrival = true; // Default to true to avoid unnecessary notifications
}
