/// <summary> /// Calculates the force necessary to avoid the closest spherical obstacle /// </summary> /// <returns> /// Force necessary to avoid an obstacle, or Vector3.zero /// </returns> /// <remarks> /// This method will iterate through all detected spherical obstacles that /// are within MinTimeToCollision, and calculate a repulsion vector based /// on them. /// </remarks> protected override Vector3 CalculateForce() { Vector3 avoidance = Vector3.zero; if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any()) { return(avoidance); } /* * While we could just calculate movement as (Velocity * predictionTime) * and save ourselves the substraction, this allows other vehicles to * override PredictFuturePosition for their own ends. */ Vector3 futurePosition = Vehicle.PredictFutureDesiredPosition(_estimationTime); #if ANNOTATE_AVOIDOBSTACLES Debug.DrawLine(Vehicle.Position, futurePosition, Color.cyan); #endif // test all obstacles for intersection with my forward axis, // select the one whose point of intersection is nearest Profiler.BeginSample("Accumulate spherical obstacle influences"); for (int i = 0; i < Vehicle.Radar.Obstacles.Count; i++) { var sphere = Vehicle.Radar.Obstacles[i]; if (sphere == null || sphere.Equals(null)) { continue; // In case the object was destroyed since we cached it } PathIntersection next = FindNextIntersectionWithSphere(Vehicle, futurePosition, sphere); float avoidanceMultiplier = 1; if (next.Intersect) { #if ANNOTATE_AVOIDOBSTACLES Debug.DrawRay(Vehicle.Position, Vehicle.DesiredVelocity.normalized * next.Distance, Color.yellow); #endif avoidanceMultiplier = Vehicle.Radar.Obstacles.Count; } var distanceCurrent = Vehicle.Position - sphere.Position; var distanceFuture = futurePosition - sphere.Position; avoidance += avoidanceMultiplier * distanceCurrent / distanceFuture.sqrMagnitude; } Profiler.EndSample(); avoidance /= Vehicle.Radar.Obstacles.Count; var newDesired = Vector3.Reflect(Vehicle.DesiredVelocity, avoidance); #if ANNOTATE_AVOIDOBSTACLES Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.green); Debug.DrawLine(Vehicle.Position, futurePosition, Color.blue); Debug.DrawLine(Vehicle.Position, Vehicle.Position + newDesired, Color.white); #endif return(newDesired); }
protected override Vector3 CalculateForce() { Profiler.BeginSample("Accumulating repulsion"); Vector3 accumulator = Vector3.zero; int totalCount = 0; var now = Time.time; Vector3 futurePosition = Vehicle.PredictFutureDesiredPosition(_estimationTime); for (int i = 0; i < _maxEvents; i++) { var timeDelta = now - _eventTimes[i]; if (timeDelta > 0) { var posDelta = futurePosition - _eventLocations[i]; if (posDelta.sqrMagnitude < _minDistanceForFearSqr) { totalCount++; accumulator += posDelta / (timeDelta * _timeDeltaWeight); #if ANNOTATE_REPULSION Debug.DrawLine(futurePosition, _eventLocations[i], Color.red / (timeDelta * _timeDeltaWeight)); #endif } } } if (totalCount > 0) { accumulator.Normalize(); } #if ANNOTATE_REPULSION Debug.DrawLine(position, position + accumulator, Color.yellow); Debug.DrawLine(position, futurePosition, Color.blue); Debug.DrawLine(position + accumulator, futurePosition, Color.magenta); #endif Profiler.EndSample(); return(accumulator); }
protected override Vector3 CalculateForce() { if (_menace == null || (Vehicle.Position - _menace.Position).sqrMagnitude > _sqrSafetyDistance) { return(Vector3.zero); } // offset from this to menace, that distance, unit vector toward menace var position = Vehicle.PredictFutureDesiredPosition(_predictionTime); Vector3 offset = _menace.Position - position; float distance = offset.magnitude; float roughTime = distance / _menace.Speed; float predictionTime = ((roughTime > _predictionTime) ? _predictionTime : roughTime); Vector3 target = _menace.PredictFuturePosition(predictionTime); // This was the totality of SteerToFlee Vector3 desiredVelocity = position - target; return(desiredVelocity - Vehicle.DesiredVelocity); }