// Create obstacle data from raycast hits
void UpdateObstacleData(RaycastHit[] hits)
{
// If no obstacles should be 'remembered' clear the obstacle data list
if (!useMemory)
{
obstacleDataList.Clear();
}
// Update the risk factors for all the obstacle data instances
for (int i = 0; i < obstacleDataList.Count; ++i)
{
UpdateRiskFactor(obstacleDataList[i], true);
}
// Add the new data to the list
for (int i = 0; i < hits.Length; ++i)
{
ObstacleData newData = new ObstacleData(hits[i], Time.time, hits[i].collider.attachedRigidbody);
UpdateRiskFactor(newData, false);
AddObstacleData(newData);
}
// Initialize the blackboard values
obstacleAvoidanceStrength = 0;
obstacleAvoidanceDirection = Vector3.forward;
obstacleMovingAwaySpeed = 0;
float totalRiskFactor = 0;
// Get the total risk factor for calculating the influence of this instance on the final calculated direction
// Also update the blackboard with the maximum collision risk value
for (int i = 0; i < obstacleDataList.Count; ++i)
{
totalRiskFactor += obstacleDataList[i].currentRiskFactor;
if (obstacleDataList[i].currentRiskFactor > obstacleAvoidanceStrength)
{
obstacleAvoidanceStrength = obstacleDataList[i].currentRiskFactor;
}
}
// Update blackboard data
if (totalRiskFactor > 0.0001f)
{
// Update the obstacle avoidance direction
for (int i = 0; i < obstacleDataList.Count; ++i)
{
obstacleAvoidanceDirection += (obstacleDataList[i].currentRiskFactor / totalRiskFactor) * obstacleDataList[i].currentAvoidanceDirection;
obstacleMovingAwaySpeed += (obstacleDataList[i].currentRiskFactor / totalRiskFactor) * obstacleDataList[i].movingAwaySpeed;
}
}
}
/// <summary>
/// Add another piece of data for a new obstacle.
/// </summary>
/// <param name="newData">The new obstacle data.</param>
public void AddObstacleData(ObstacleData newData)
{
Rigidbody rBody = newData.raycastHit.collider.attachedRigidbody;
bool hasRigidbody = rBody != null;
// Prevent obstacle avoidance of self
if (hasRigidbody && (rBody == vehicle.CachedRigidbody))
{
return;
}
// Merge obstacle data that are in close proximity
bool merged = false;
for (int i = 0; i < obstacleDataList.Count; ++i)
{
if (Vector3.Distance(newData.currentPos, obstacleDataList[i].currentPos) < obstacleMergeDistance)
{
obstacleDataList[i] = newData;
merged = true;
break;
}
}
// If obstacle has not been merged, replace any that have a lower risk
if (!merged)
{
if (obstacleDataList.Count < maxObstacleDataInstances)
{
obstacleDataList.Add(newData);
}
else
{
for (int i = 0; i < obstacleDataList.Count; ++i)
{
if (newData.currentRiskFactor > obstacleDataList[i].currentRiskFactor)
{
obstacleDataList[i] = newData;
break;
}
}
}
}
}
// Update the risk factors for the obstacles
void UpdateRiskFactor(ObstacleData obstacleData, bool show)
{
// Update the position
obstacleData.currentPos = obstacleData.raycastHit.point + obstacleData.obstacleVelocity *
(Time.time - obstacleData.raycastHitTime);
// Update the avoid target direction
Vector3 toObstacleVector = obstacleData.currentPos - vehicle.transform.position;
// Get the velocity of the collision point relative to this ship
Vector3 collisionRelVelocity = obstacleData.obstacleVelocity - vehicle.CachedRigidbody.velocity;
float closingVelocityAmount = Vector3.Dot(collisionRelVelocity.normalized, -toObstacleVector.normalized);
// Get the closest distance that the point obstacle will get to this ship
float tmp = Vector3.Dot(-toObstacleVector.normalized, collisionRelVelocity.normalized);
Vector3 nearestPointOnLine = obstacleData.currentPos + (tmp * Vector3.Magnitude(toObstacleVector) * collisionRelVelocity.normalized);
float timeToImpact = Vector3.Distance(obstacleData.currentPos, vehicle.transform.position) / Mathf.Max(closingVelocityAmount * collisionRelVelocity.magnitude, 0.0001f);
obstacleData.movingAwaySpeed = Vector3.Dot(obstacleData.obstacleVelocity.normalized, toObstacleVector.normalized) * obstacleData.obstacleVelocity.magnitude;
// Calculate the avoidance target position and the direction to it
Vector3 perpendicularAvoidDirection = (vehicle.transform.position - nearestPointOnLine).normalized;
Vector3 avoidTarget = obstacleData.currentPos + perpendicularAvoidDirection * obstacleAvoidanceMargin;
obstacleData.currentAvoidanceDirection = (avoidTarget - vehicle.transform.position).normalized;
float directionalityFactor = Mathf.Clamp(Vector3.Dot(vehicle.transform.forward, toObstacleVector.normalized), 0f, 1f);
float proximityFactor = timeToImpact < minMaxCollisionReactionTime.x ? 1 : 1 - Mathf.Clamp((timeToImpact - minMaxCollisionReactionTime.x) /
(minMaxCollisionReactionTime.y - minMaxCollisionReactionTime.x), 0f, 1f);
float nearestDist = Vector3.Distance(vehicle.transform.position, nearestPointOnLine);
proximityFactor *= nearestDist < minMaxObstaclePassDistance.x ? 1 : 1 - Mathf.Clamp((nearestDist - minMaxObstaclePassDistance.x) /
(minMaxObstaclePassDistance.y - minMaxObstaclePassDistance.x), 0f, 1f);
float timeFactor = Mathf.Clamp(1 - (Time.time - obstacleData.raycastHitTime) / (1 / obstacleImportanceFadeRate), 0f, 1f);
obstacleData.currentRiskFactor = directionalityFactor * timeFactor * proximityFactor;
}
