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); var offset = _menace.Position - position; var distance = offset.magnitude; var roughTime = distance / _menace.Speed; var predictionTime = ((roughTime > _predictionTime) ? _predictionTime : roughTime); //var target = _menace.PredictFuturePosition(predictionTime); var target = _menace.PredictFuturePosition(predictionTime) + _offset; // This was the totality of SteerToFlee var desiredVelocity = position - target; //Debug.DrawRay(Vehicle.Position, desiredVelocity, Color.blue); return(desiredVelocity - Vehicle.DesiredVelocity); }
/// <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"); var accumulator = Vector3.zero; var totalCount = 0; var now = Time.time; var futurePosition = Vehicle.PredictFutureDesiredPosition(_estimationTime); for (var 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); }
/// <summary> /// Calculates the force necessary to avoid the detected spherical obstacles /// </summary> /// <returns> /// Force necessary to avoid detected obstacles, 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() { var 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. */ var futurePosition = Vehicle.PredictFutureDesiredPosition(_estimationTime); #if ANNOTATE_AVOIDOBSTACLES Debug.DrawLine(Vehicle.Position, futurePosition, Color.cyan); #endif /* * Test all obstacles for intersection with the vehicle's future position. * If we find that we are going to intersect them, use their position * and distance to affect the avoidance - the further away the intersection * is, the less weight they'll carry. */ Profiler.BeginSample("Accumulate spherical obstacle influences"); for (var 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 } var next = FindNextIntersectionWithSphere(Vehicle, futurePosition, sphere); var avoidanceMultiplier = 0.1f; if (next.Intersect) { #if ANNOTATE_AVOIDOBSTACLES Debug.DrawRay(Vehicle.Position, Vehicle.DesiredVelocity.normalized * next.Distance, Color.yellow); #endif var timeToObstacle = next.Distance / Vehicle.Speed; avoidanceMultiplier = 2 * (_estimationTime / timeToObstacle); } var oppositeDirection = Vehicle.Position - sphere.Position; avoidance += avoidanceMultiplier * oppositeDirection; } 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); }