Ejemplo n.º 1
0
        public static Vector3 SteerToAvoidObstacles(this IVehicle vehicle, float minTimeToCollision, IEnumerable <IObstacle> obstacles, IAnnotationService annotation = null)
        {
            PathIntersection?nearest = null;
            float            minDistanceToCollision = minTimeToCollision * vehicle.Speed;

            // test all obstacles for intersection with my forward axis,
            // select the one whose point of intersection is nearest
            foreach (var o in obstacles)
            {
                var next = o.NextIntersection(vehicle);
                if (!next.HasValue)
                {
                    continue;
                }

                if (!nearest.HasValue || (next.Value < nearest.Value.Distance))
                {
                    nearest = new PathIntersection {
                        Distance = next.Value, Obstacle = o
                    }
                }
                ;
            }

            if (nearest.HasValue)
            {
                if (annotation != null)
                {
                    annotation.AvoidObstacle(minDistanceToCollision);
                }

                return(nearest.Value.Obstacle.SteerToAvoid(vehicle, minTimeToCollision));
            }
            return(Vector3.Zero);
        }
Ejemplo n.º 2
0
    /// <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 steer to avoid the closest one to the
    /// vehicle.  It's not ideal, as that means the vehicle might crash into
    /// another obstacle while avoiding the closest one, but it'll do.
    /// </remarks>
    protected override Vector3 CalculateForce()
    {
        Vector3 avoidance = Vector3.zero;

        if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
        {
            return(avoidance);
        }

        PathIntersection nearest = new PathIntersection(null);

        /*
         * While we could just calculate line as (Velocity * predictionTime)
         * and save ourselves the substraction, this allows other vehicles to
         * override PredictFuturePosition for their own ends.
         */
        Vector3 futurePosition = Vehicle.PredictFuturePosition(_minTimeToCollision);
        Vector3 line           = (futurePosition - Vehicle.Position);

        // test all obstacles for intersection with my forward axis,
        // select the one whose point of intersection is nearest
        Profiler.BeginSample("Find nearest intersection");
        foreach (var o in Vehicle.Radar.Obstacles)
        {
            SphericalObstacle sphere = o as SphericalObstacle;
            PathIntersection  next   = FindNextIntersectionWithSphere(sphere, line);
            if (!nearest.intersect ||
                (next.intersect &&
                 next.distance < nearest.distance))
            {
                nearest = next;
            }
        }
        Profiler.EndSample();


        // when a nearest intersection was found
        Profiler.BeginSample("Calculate avoidance");
        if (nearest.intersect &&
            nearest.distance < line.magnitude)
        {
                        #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, nearest.obstacle.center, Color.red);
                        #endif

            // compute avoidance steering force: take offset from obstacle to me,
            // take the component of that which is lateral (perpendicular to my
            // forward direction), set length to maxForce, add a bit of forward
            // component (in capture the flag, we never want to slow down)
            Vector3 offset = Vehicle.Position - nearest.obstacle.center;
            avoidance = OpenSteerUtility.perpendicularComponent(offset, transform.forward);

            avoidance.Normalize();
            avoidance *= Vehicle.MaxForce;
            avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
        }
        Profiler.EndSample();

        return(avoidance);
    }
    /// <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.Position, 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);
    }
Ejemplo n.º 4
0
        // this version avoids all of the obstacles in an ObstacleGroup
        //
        // XXX 9-12-03: note this does NOT use the Obstacle::steerToAvoid protocol
        // XXX like the older steerToAvoidObstacle does/did.  It needs to be fixed


        public Vector3 steerToAvoidObstacles(float minTimeToCollision, ArrayList obstacles)
        {
            Vector3          avoidance = new Vector3();
            PathIntersection nearest, next;

            nearest = new PathIntersection();
            next    = new PathIntersection();

            float minDistanceToCollision = minTimeToCollision * speed();

            next.intersect    = 0; // false;
            nearest.intersect = 0; // false;

            // test all obstacles for intersection with my forward axis,
            // select the one whose point of intersection is nearest



            //for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); o++)
            for (int i = 0; i < obstacles.Count; i++)
            {
                SphericalObstacle o = (SphericalObstacle)obstacles[i];
                // xxx this should be a generic call on Obstacle, rather than
                // xxx this code which presumes the obstacle is spherical
                findNextIntersectionWithSphere(o, next);

                if ((nearest.intersect == 0) ||
                    ((next.intersect != 0) &&
                     (next.distance < nearest.distance)))
                {
                    nearest = next;
                }
            }

            // when a nearest intersection was found
            if ((nearest.intersect != 0) &&
                (nearest.distance < minDistanceToCollision))
            {
                // show the corridor that was checked for collisions
                annotateAvoidObstacle(minDistanceToCollision);

                // compute avoidance steering force: take offset from obstacle to me,
                // take the component of that which is lateral (perpendicular to my
                // forward direction), set length to maxForce, add a bit of forward
                // component (in capture the flag, we never want to slow down)
                Vector3 offset = Position - nearest.obstacle.center;
                //avoidance = offset.perpendicularComponent (forward());
                avoidance = OpenSteerUtility.perpendicularComponent(offset, forward());

                avoidance.Normalize();//.normalize ();
                avoidance *= maxForce();
                avoidance += forward() * maxForce() * 0.75f;
            }

            return(avoidance);
        }
Ejemplo n.º 5
0
        // ----------------------------------------------------------------------------
        // xxx experiment cwr 9-6-02



        public void findNextIntersectionWithSphere(SphericalObstacle obs, PathIntersection intersection)
        {
            // xxx"SphericalObstacle& obs" should be "const SphericalObstacle&
            // obs" but then it won't let me store a pointer to in inside the
            // PathIntersection

            // 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/

            float   b, c, d, p, q, s;
            Vector3 lc;

            // initialize pathIntersection object
            intersection.intersect = 0;
            intersection.obstacle  = obs;

            // find "local center" (lc) of sphere in boid's coordinate space
            lc = localizePosition(obs.center);

            // computer line-sphere intersection parameters
            b = -2 * lc.z;
            c = square(lc.x) + square(lc.y) + square(lc.z) -
                square(obs.radius + radius());
            d = (b * b) - (4 * c);

            // when the path does not intersect the sphere
            if (d < 0)
            {
                return;
            }

            // otherwise, the path intersects the sphere in two points with
            // parametric coordinates of "p" and "q".
            // (If "d" is zero the two points are coincident, the path is tangent)
            s = (float)System.Math.Sqrt(d);
            p = (-b + s) / 2;
            q = (-b - s) / 2;

            // both intersections are behind us, so no potential collisions
            if ((p < 0) && (q < 0))
            {
                return;
            }

            // at least one intersection is in front of us
            intersection.intersect = 0;
            intersection.distance  =
                ((p > 0) && (q > 0)) ?
                // both intersections are in front of us, find nearest one
                ((p < q) ? p : q) :
                // otherwise only one intersections is in front, select it
                ((p > 0) ? p : q);
            return;
        }
Ejemplo n.º 6
0
        // xxx experiment cwr 9-6-02
        protected void FindNextIntersectionWithSphere(SphericalObstacle obs, ref PathIntersection intersection)
        {
            // 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/

            float   b, c, d, p, q, s;
            Vector3 lc;

            // initialize pathIntersection object
            intersection.intersect = false;
            intersection.obstacle  = obs;

            // find "local center" (lc) of sphere in boid's coordinate space
            lc = this.LocalizePosition(obs.Center);

            // computer line-sphere intersection parameters
            b = -2 * lc.Z;
            c = lc.X * lc.X + lc.Y * lc.Y + lc.Z * lc.Z -
                (obs.Radius + this.Radius) * (obs.Radius + this.Radius);
            d = (b * b) - (4 * c);

            // when the path does not intersect the sphere
            if (d < 0)
            {
                return;
            }

            // otherwise, the path intersects the sphere in two points with
            // parametric coordinates of "p" and "q".
            // (If "d" is zero the two points are coincident, the path is tangent)
            s = (float)Math.Sqrt(d);
            p = (-b + s) / 2;
            q = (-b - s) / 2;

            // both intersections are behind us, so no potential collisions
            if ((p < 0) && (q < 0))
            {
                return;
            }

            // at least one intersection is in front of us
            intersection.intersect = true;
            intersection.distance  =
                ((p > 0) && (q > 0)) ?
                // both intersections are in front of us, find nearest one
                ((p < q) ? p : q) :
                // otherwise only one intersections is in front, select it
                ((p > 0) ? p : q);
        }
Ejemplo n.º 7
0
        // avoids all obstacles in an ObstacleGroup
        public Vector3 SteerToAvoidObstacles <Obstacle>(float minTimeToCollision, List <Obstacle> obstacles)
            where Obstacle : IObstacle
        {
            Vector3          avoidance = Vector3.Zero;
            PathIntersection nearest   = new PathIntersection();
            PathIntersection next      = new PathIntersection();
            float            minDistanceToCollision = minTimeToCollision * this.Speed;

            next.intersect    = false;
            nearest.intersect = false;

            // test all obstacles for intersection with my forward axis,
            // select the one whose point of intersection is nearest
            foreach (Obstacle o in obstacles)
            {
                //FIXME: this should be a generic call on Obstacle, rather than this code which presumes the obstacle is spherical
                FindNextIntersectionWithSphere(o as SphericalObstacle, ref next);

                if (nearest.intersect == false || (next.intersect != false && next.distance < nearest.distance))
                {
                    nearest = next;
                }
            }

            // when a nearest intersection was found
            if ((nearest.intersect != false) && (nearest.distance < minDistanceToCollision))
            {
                // show the corridor that was checked for collisions
                annotation.AvoidObstacle(minDistanceToCollision);

                // compute avoidance steering force: take offset from obstacle to me,
                // take the component of that which is lateral (perpendicular to my
                // forward direction), set length to maxForce, add a bit of forward
                // component (in capture the flag, we never want to slow down)
                Vector3 offset = this.Position - nearest.obstacle.Center;
                avoidance = VectorUtils.PerpendicularComponent(offset, this.Forward);
                avoidance.Normalize();
                avoidance *= this.MaxForce;
                avoidance += this.Forward * this.MaxForce * 0.75f;
            }

            return(avoidance);
        }
	/// <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 steer to avoid the closest one to the 
	/// vehicle.  It's not ideal, as that means the vehicle might crash into
	/// another obstacle while avoiding the closest one, but it'll do.
	/// </remarks>
	protected override Vector3 CalculateForce()
	{
		Vector3 avoidance = Vector3.zero;
		if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
		{
			return avoidance;
		}

		PathIntersection nearest = new PathIntersection(null);
		/*
		 * While we could just calculate line as (Velocity * predictionTime) 
		 * and save ourselves the substraction, this allows other vehicles to
		 * override PredictFuturePosition for their own ends.
		 */
		Vector3 futurePosition = Vehicle.PredictFuturePosition(_minTimeToCollision);
		Vector3 line = (futurePosition - Vehicle.Position);

		// test all obstacles for intersection with my forward axis,
		// select the one whose point of intersection is nearest
		Profiler.BeginSample("Find nearest intersection");
		foreach (var o in Vehicle.Radar.Obstacles)
		{
			SphericalObstacle sphere = o as SphericalObstacle;
			PathIntersection next = FindNextIntersectionWithSphere (sphere, line);
			if (!nearest.intersect ||
				(next.intersect &&
				 next.distance < nearest.distance))
			{
				nearest = next;
			}
		}
		Profiler.EndSample();


		// when a nearest intersection was found
		Profiler.BeginSample("Calculate avoidance");
		if (nearest.intersect &&
			nearest.distance < line.magnitude)
		{
			#if ANNOTATE_AVOIDOBSTACLES
			Debug.DrawLine(Vehicle.Position, nearest.obstacle.center, Color.red);
			#endif

			// compute avoidance steering force: take offset from obstacle to me,
			// take the component of that which is lateral (perpendicular to my
			// forward direction), set length to maxForce, add a bit of forward
			// component (in capture the flag, we never want to slow down)
			Vector3 offset = Vehicle.Position - nearest.obstacle.center;
			avoidance =	 OpenSteerUtility.perpendicularComponent(offset, transform.forward);

			avoidance.Normalize();
			avoidance *= Vehicle.MaxForce;
			avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
		}
		Profiler.EndSample();

		return avoidance;
	}
	/// <summary>
	/// Finds the vehicle's next intersection with a spherical obstacle
	/// </summary>
	/// <param name="obs">
	/// A spherical obstacle to check against <see cref="SphericalObstacle"/>
	/// </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 (SphericalObstacle 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.center;

		// initialize pathIntersection object
		var intersection = new PathIntersection(obs);
		
		#if ANNOTATE_AVOIDOBSTACLES
		obs.annotatePosition();
		Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
		#endif
		
		// computer line-sphere intersection parameters
		a = line.sqrMagnitude;
		b = 2 * Vector3.Dot(line, toCenter);
		c = obs.center.sqrMagnitude;
		c += Vehicle.Position.sqrMagnitude;
		c -= 2 * Vector3.Dot(obs.center, Vehicle.Position); 
		c -= Mathf.Pow(obs.radius + 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>
    /// 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 steer to avoid the closest one to the
    /// vehicle.  It's not ideal, as that means the vehicle might crash into
    /// another obstacle while avoiding the closest one, but it'll do.
    /// </remarks>
    protected override Vector3 CalculateForce()
    {
        Vector3 avoidance = Vector3.zero;

        if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any())
        {
            return(avoidance);
        }

        PathIntersection nearest = new PathIntersection(null);

        /*
         * 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(_minTimeToCollision);
        Vector3 movement       = futurePosition - Vehicle.Position;

                #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("Find nearest intersection");
        foreach (var o in Vehicle.Radar.Obstacles)
        {
            var sphere            = o as DetectableObject;
            PathIntersection next = FindNextIntersectionWithSphere(Vehicle.Position, futurePosition, sphere);
            if (!nearest.Intersect ||
                (next.Intersect &&
                 next.Distance < nearest.Distance))
            {
                nearest = next;
            }
        }
        Profiler.EndSample();


        // when a nearest intersection was found
        Profiler.BeginSample("Calculate avoidance");
        if (nearest.Intersect &&
            nearest.Distance < movement.magnitude)
        {
                        #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, nearest.Obstacle.Position, Color.red);
                        #endif

            // compute avoidance steering force: take offset from obstacle to me,
            // take the component of that which is lateral (perpendicular to my
            // movement direction),  add a bit of forward component
            Vector3 offset        = Vehicle.Position - nearest.Obstacle.Position;
            Vector3 moveDirection = movement.normalized;
            avoidance = OpenSteerUtility.perpendicularComponent(offset, moveDirection);

            avoidance.Normalize();

                        #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.white);
                        #endif

            avoidance += moveDirection * Vehicle.MaxForce * _avoidanceForceFactor;

                        #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.yellow);
                        #endif
        }
        Profiler.EndSample();

        return(avoidance);
    }
Ejemplo n.º 11
0
		// this version avoids all of the obstacles in an ObstacleGroup
		//
		// XXX 9-12-03: note this does NOT use the Obstacle::steerToAvoid protocol
		// XXX like the older steerToAvoidObstacle does/did.  It needs to be fixed
	   
		public Vector3 steerToAvoidObstacles (float minTimeToCollision, ArrayList obstacles)
		{
			Vector3 avoidance = Vector3.zero;

			if (obstacles == null || obstacles.Count == 0)
			{
				return avoidance;
			}

			PathIntersection nearest = new PathIntersection(null);

			float minDistanceToCollision = minTimeToCollision * Speed;

			// test all obstacles for intersection with my forward axis,
			// select the one whose point of intersection is nearest

			for (int i=0; i < obstacles.Count; i++)
			{
				SphericalObstacle o=(SphericalObstacle) obstacles[i];
				// xxx this should be a generic call on Obstacle, rather than
				// xxx this code which presumes the obstacle is spherical
				PathIntersection next = findNextIntersectionWithSphere (o);
				if (!nearest.intersect ||
					(next.intersect &&
					 next.distance < nearest.distance))
				{
					nearest = next;
				}
			}


			// when a nearest intersection was found
			if (nearest.intersect &&
				nearest.distance < minDistanceToCollision)
			{
				#if ANNOTATE_AVOIDOBSTACLES
				Debug.DrawLine(Position, nearest.obstacle.center, Color.red);
				#endif
				// show the corridor that was checked for collisions
				annotateAvoidObstacle (minDistanceToCollision);

				// compute avoidance steering force: take offset from obstacle to me,
				// take the component of that which is lateral (perpendicular to my
				// forward direction), set length to maxForce, add a bit of forward
				// component (in capture the flag, we never want to slow down)
				Vector3 offset = Position - nearest.obstacle.center;
				//avoidance = offset.perpendicularComponent (Forward);
				avoidance =	 OpenSteerUtility.perpendicularComponent( offset,Forward);

				avoidance.Normalize();
				avoidance *= MaxForce;
				avoidance += Forward * MaxForce * 0.75f;
			}

			return avoidance;
		}
    /// <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 steer to avoid the closest one to the 
    /// vehicle.  It's not ideal, as that means the vehicle might crash into
    /// another obstacle while avoiding the closest one, but it'll do.
    /// </remarks>
    protected override Vector3 CalculateForce()
    {
        Vector3 avoidance = Vector3.zero;
        if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any())
        {
            return avoidance;
        }

        PathIntersection nearest = new PathIntersection(null);
        /*
         * 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(_minTimeToCollision);
        Vector3 movement = futurePosition - Vehicle.Position;

        #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("Find nearest intersection");
        foreach (var o in Vehicle.Radar.Obstacles)
        {
            var sphere = o as DetectableObject;
            PathIntersection next = FindNextIntersectionWithSphere(Vehicle.Position, futurePosition, sphere);
            if (!nearest.Intersect ||
                (next.Intersect &&
                 next.Distance < nearest.Distance))
            {
                nearest = next;
            }
        }
        Profiler.EndSample();

        // when a nearest intersection was found
        Profiler.BeginSample("Calculate avoidance");
        if (nearest.Intersect &&
            nearest.Distance < movement.magnitude)
        {
            #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, nearest.Obstacle.Position, Color.red);
            #endif

            // compute avoidance steering force: take offset from obstacle to me,
            // take the component of that which is lateral (perpendicular to my
            // movement direction),  add a bit of forward component
            Vector3 offset = Vehicle.Position - nearest.Obstacle.Position;
            Vector3 moveDirection = movement.normalized;
            avoidance =	 OpenSteerUtility.perpendicularComponent(offset, moveDirection);

            avoidance.Normalize();

            #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.white);
            #endif

            avoidance += moveDirection * Vehicle.MaxForce * _avoidanceForceFactor;

            #if ANNOTATE_AVOIDOBSTACLES
            Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.yellow);
            #endif
        }
        Profiler.EndSample();

        return avoidance;
    }
Ejemplo n.º 13
0
        // ----------------------------------------------------------------------------
        // xxx experiment cwr 9-6-02
        public void findNextIntersectionWithSphere(SphericalObstacle obs, PathIntersection intersection)
        {
            // xxx"SphericalObstacle& obs" should be "const SphericalObstacle&
            // obs" but then it won't let me store a pointer to in inside the
            // PathIntersection

            // 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/

            float b, c, d, p, q, s;
            Vector3 lc;

            // initialize pathIntersection object
            intersection.intersect = 0;
            intersection.obstacle = obs;

            // find "local center" (lc) of sphere in boid's coordinate space
            lc = localizePosition (obs.center);

            // computer line-sphere intersection parameters
            b = -2 * lc.z;
            c = square (lc.x) + square (lc.y) + square (lc.z) -
                square (obs.radius + radius());
            d = (b * b) - (4 * c);

            // when the path does not intersect the sphere
            if (d < 0) return;

            // otherwise, the path intersects the sphere in two points with
            // parametric coordinates of "p" and "q".
            // (If "d" is zero the two points are coincident, the path is tangent)
            s = (float) System.Math.Sqrt(d);
            p = (-b + s) / 2;
            q = (-b - s) / 2;

            // both intersections are behind us, so no potential collisions
            if ((p < 0) && (q < 0)) return;

            // at least one intersection is in front of us
            intersection.intersect = 0;
            intersection.distance =
                ((p > 0) && (q > 0)) ?
                // both intersections are in front of us, find nearest one
                ((p < q) ? p : q) :
                // otherwise only one intersections is in front, select it
                ((p > 0) ? p : q);
            return;
        }
Ejemplo n.º 14
0
        // this version avoids all of the obstacles in an ObstacleGroup
        //
        // XXX 9-12-03: note this does NOT use the Obstacle::steerToAvoid protocol
        // XXX like the older steerToAvoidObstacle does/did.  It needs to be fixed
        public Vector3 steerToAvoidObstacles( float minTimeToCollision, ArrayList obstacles)
        {
            Vector3 avoidance = new Vector3() ;
            PathIntersection nearest, next;

            nearest = new PathIntersection();
            next = new PathIntersection();

             float minDistanceToCollision = minTimeToCollision * speed();

             next.intersect = 0; // false;
             nearest.intersect = 0;// false;

            // test all obstacles for intersection with my forward axis,
            // select the one whose point of intersection is nearest

            //for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); o++)
            for (int i=0;i<obstacles.Count;i++)
            {
                SphericalObstacle o=(SphericalObstacle) obstacles[i];
                // xxx this should be a generic call on Obstacle, rather than
                // xxx this code which presumes the obstacle is spherical
                findNextIntersectionWithSphere (o, next);

                if ((nearest.intersect == 0) ||
                    ((next.intersect != 0) &&
                     (next.distance < nearest.distance)))
                    nearest = next;
            }

            // when a nearest intersection was found
            if ((nearest.intersect != 0) &&
                (nearest.distance < minDistanceToCollision))
            {
                // show the corridor that was checked for collisions
                annotateAvoidObstacle (minDistanceToCollision);

                // compute avoidance steering force: take offset from obstacle to me,
                // take the component of that which is lateral (perpendicular to my
                // forward direction), set length to maxForce, add a bit of forward
                // component (in capture the flag, we never want to slow down)
                 Vector3 offset = Position - nearest.obstacle.center;
                //avoidance = offset.perpendicularComponent (forward());
                 avoidance =  OpenSteerUtility.perpendicularComponent( offset,forward());

                avoidance.Normalise();//.normalize ();
                avoidance *= maxForce ();
                avoidance += forward() * maxForce () * 0.75f;
            }

            return avoidance;
        }
Ejemplo n.º 15
0
    /// <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 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;
        }
	/// <summary>
	/// Finds the vehicle's next intersection with a spherical obstacle
	/// </summary>
	/// <param name="obs">
	/// A spherical obstacle to check against <see cref="SphericalObstacle"/>
	/// </param>
	/// <returns>
	/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
	/// </returns>
	public PathIntersection FindNextIntersectionWithSphere (SphericalObstacle obs)
	{
		// 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/

		float b, c, d, p, q, s;
		Vector3 lc;

		// initialize pathIntersection object
		PathIntersection intersection = new PathIntersection(obs);
		// find "local center" (lc) of sphere in the vehicle's coordinate space
		lc = transform.InverseTransformPoint(obs.center);
		
		#if ANNOTATE_AVOIDOBSTACLES
		obs.annotatePosition();
		#endif
		
		// computer line-sphere intersection parameters
		b = -2 * lc.z;
		c = Mathf.Pow(lc.x, 2) + Mathf.Pow(lc.y, 2) + Mathf.Pow(lc.z, 2) - 
			Mathf.Pow(obs.radius + Vehicle.Radius, 2);
		d = (b * b) - (4 * c);

		// when the path does not intersect the sphere
		if (d < 0) return intersection;

		// otherwise, the path intersects the sphere in two points with
		// parametric coordinates of "p" and "q".
		// (If "d" is zero the two points are coincident, the path is tangent)
		s = (float) System.Math.Sqrt(d);
		p = (-b + s) / 2;
		q = (-b - s) / 2;

		// both intersections are behind us, so no potential collisions
		if ((p < 0) && (q < 0)) return intersection; 

		// at least one intersection is in front of us
		intersection.intersect = true;
		intersection.distance =
			((p > 0) && (q > 0)) ?
			// both intersections are in front of us, find nearest one
			((p < q) ? p : q) :
			// otherwise only one intersections is in front, select it
			((p > 0) ? p : q);
		
		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);
    }
	/// <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 steer to avoid the closest one to the 
	/// vehicle.  It's not ideal, as that means the vehicle might crash into
	/// another obstacle while avoiding the closest one, but it'll do.
	/// </remarks>
	protected override Vector3 CalculateForce()
	{
		Vector3 avoidance = Vector3.zero;
		if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
		{
			return avoidance;
		}

		PathIntersection nearest = new PathIntersection(null);
		float minDistanceToCollision = _minTimeToCollision * Vehicle.Speed;

		// test all obstacles for intersection with my forward axis,
		// select the one whose point of intersection is nearest
		foreach (var o in Vehicle.Radar.Obstacles)
		{
			SphericalObstacle sphere = o as SphericalObstacle;
			PathIntersection next = FindNextIntersectionWithSphere (sphere);
			if (!nearest.intersect ||
				(next.intersect &&
				 next.distance < nearest.distance))
			{
				nearest = next;
			}
		}


		// when a nearest intersection was found
		if (nearest.intersect &&
			nearest.distance < minDistanceToCollision)
		{
			#if ANNOTATE_AVOIDOBSTACLES
			Debug.DrawLine(transform.position, nearest.obstacle.center, Color.red);
			#endif

			// compute avoidance steering force: take offset from obstacle to me,
			// take the component of that which is lateral (perpendicular to my
			// forward direction), set length to maxForce, add a bit of forward
			// component (in capture the flag, we never want to slow down)
			Vector3 offset = transform.position - nearest.obstacle.center;
			avoidance =	 OpenSteerUtility.perpendicularComponent(offset, transform.forward);

			avoidance.Normalize();
			avoidance *= Vehicle.MaxForce;
			avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
		}

		return avoidance;
	}