Beispiel #1
0
		/// <summary>
		/// Test a path between current position and destination.
		/// </summary>
		private void TestPath(Vector3D destination, MyEntity destEntity, byte runId, bool isAlternate, bool tryAlternates, bool slowDown = false)
		{
			if (runId != m_runId)
			{
				m_logger.debugLog("destination changed, abort", "TestPath()", Logger.severity.DEBUG);
				return;
			}

			if (!lock_testPath.TryAcquireExclusive())
			{
				m_logger.debugLog("Already running, requeue (destination:" + destination + ", destEntity: " + destEntity + ", runId :" + runId
					+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")", "TestPath()");
				LockedQueue<Action> queue = isAlternate ? m_pathLow : m_pathHigh;
				queue.Enqueue(() => TestPath(destination, destEntity, runId, isAlternate, tryAlternates));
				return;
			}
			try
			{
				if (m_grid != m_navBlock.Grid)
				{
					m_logger.debugLog("Grid has changed, from " + m_grid.DisplayName + " to " + m_navBlock.Grid.DisplayName + ", nav block: " + m_navBlock.Block.getBestName(), "TestPath()", Logger.severity.WARNING);
					return;
				}
				m_logger.debugLog("Running, (destination:" + destination + ", destEntity: " + destEntity + ", runId :" + runId
						+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")", "TestPath()");
				
				MyEntity obstructing;
				Vector3? pointOfObstruction;

				if ((isAlternate || tryAlternates) && !m_pathChecker.GravityTest(new LineSegmentD(m_navBlock.WorldPosition, destination), m_destination, out obstructing, out pointOfObstruction))
				{
					m_pathState = PathState.Searching;
					m_logger.debugLog("blocked by gravity at " + pointOfObstruction, "TestGravity()", Logger.severity.DEBUG);
					if (tryAlternates)
					{
						Vector3D point = pointOfObstruction.Value;
						Vector3 direction = m_navBlock.WorldPosition - obstructing.GetCentre(); direction.Normalize();
						float distance = Vector3.Distance(m_navBlock.WorldPosition, pointOfObstruction.Value);
						float minDist = m_navSet.Settings_Current.DestinationRadius;
						if (minDist < 1000f)
						{
							minDist = 1500f;
							m_navSet.Settings_Current.DestinationRadius = 1000f;
						}
						else
							minDist *= 1.5f;

						m_pathHigh.Clear();
						FindAlternate_AroundObstruction(pointOfObstruction.Value, direction, distance, minDist * minDist, runId);
						m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
					}
					return;
				}

				if (m_pathChecker.TestFast(m_navBlock, destination, m_ignoreAsteroid, destEntity, m_landing))
				{
					m_logger.debugLog("path is clear (fast)", "TestPath()", Logger.severity.TRACE);
					PathClear(ref destination, runId, isAlternate, slowDown);
					return;
				}

				if (m_pathChecker.TestSlow(out obstructing, out pointOfObstruction))
				{
					m_logger.debugLog("path is clear (slow)", "TestPath()", Logger.severity.TRACE);
					PathClear(ref destination, runId, isAlternate, slowDown);
					return;
				}

				if (runId != m_runId)
				{
					m_logger.debugLog("destination changed, abort", "TestPath()", Logger.severity.DEBUG);
					return;
				}

				if (slowDown)
				{
					Vector3 displacement = pointOfObstruction.Value - m_navBlock.WorldPosition;
					m_navSet.Settings_Task_NavWay.SpeedMaxRelative = Vector3.Distance(pointOfObstruction.Value, m_navBlock.WorldPosition) / LookAheadSpeed_Seconds;
					m_logger.debugLog("Set MaxRelativeSpeed to " + m_navSet.Settings_Task_NavWay.SpeedMaxRelative, "TestPath()", Logger.severity.TRACE);
					return;
				}

				m_pathState = PathState.Searching;

				m_logger.debugLog("path is blocked by " + obstructing.getBestName() + " at " + pointOfObstruction + ", destEntity: " + destEntity.getBestName(), "TestPath()", isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
				m_logger.debugLog(obstructing is IMyCubeBlock, "grid: " + obstructing.GetTopMostParent().DisplayName, "TestPath()", isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);

				if (tryAlternates)
					TryAlternates(runId, pointOfObstruction.Value, obstructing);
			}
			finally
			{
				lock_testPath.ReleaseExclusive();
				RunItem();
			}
		}
Beispiel #2
0
        /// <summary>
        /// Test a path between current position and destination.
        /// </summary>
        private void TestPath(Vector3D destination, MyEntity ignoreEntity, byte runId, bool isAlternate, bool tryAlternates, bool slowDown = false)
        {
            m_logger.debugLog(m_navBlock == null, "m_navBlock == null", Logger.severity.FATAL);

            if (runId != m_runId)
            {
                m_logger.debugLog("destination changed, abort", Logger.severity.DEBUG);
                return;
            }

            if (!lock_testPath.TryAcquireExclusive())
            {
                m_logger.debugLog("Already running, requeue (destination:" + destination + ", ignoreEntity: " + ignoreEntity.getBestName() + ", runId :" + runId
                    + ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")");
                LockedQueue<Action> queue = isAlternate ? m_pathLow : m_pathHigh;
                queue.Enqueue(() => TestPath(destination, ignoreEntity, runId, isAlternate, tryAlternates));
                return;
            }
            try
            {
                if (m_grid != m_navBlock.Grid)
                {
                    m_logger.debugLog("Grid has changed, from " + m_grid.getBestName() + " to " + m_navBlock.Grid.getBestName() + ", nav block: " + m_navBlock.Block.getBestName(), Logger.severity.WARNING);
                    return;
                }
                m_logger.debugLog("Running, (destination:" + destination + ", ignoreEntity: " + ignoreEntity.getBestName() + ", runId :" + runId
                        + ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")");

                MyEntity obstructing;
                Vector3? pointOfObstruction;

                if (!isAlternate && !m_ignoreAsteroid)
                {
                    if (slowDown)
                    {
                        if ((m_planetCheckSpeed.CurrentState & PlanetChecker.State.Blocked) != 0)
                        {
                            float speed = Vector3.Distance(m_planetCheckSpeed.ObstructionPoint, m_navBlock.WorldPosition) * 0.1f;
                            if (speed < 1f)
                                speed = 1f;
                            m_navSet.Settings_Task_NavWay.SpeedTarget = speed;
                            m_logger.debugLog("Path blocked by planet, set SpeedTarget to " + speed + ", obstructed by planet", Logger.severity.TRACE);
                            return;
                        }
                    }
                    else
                    {
                        if ((m_planetCheckDest.CurrentState & PlanetChecker.State.Blocked) != 0 &&
                            // planet checker is using an older displacement so verify that obstruction is closer than destination
                                Vector3D.DistanceSquared(m_navBlock.WorldPosition, m_planetCheckDest.ObstructionPoint) < Vector3D.DistanceSquared(m_navBlock.WorldPosition, destination))
                        {
                            m_logger.debugLog("path blocked by planet, to destination: " + (destination - m_navBlock.WorldPosition) + ", to obstruction: " + (m_planetCheckDest.ObstructionPoint - m_navBlock.WorldPosition));

                            if (m_pathState < PathState.Searching)
                                m_pathState = PathState.Searching;
                            obstructing = m_planetCheckDest.ClosestPlanet;

                            Vector3 direction = Vector3.Normalize(m_navBlock.WorldPosition - obstructing.GetCentre());
                            pointOfObstruction = m_planetCheckDest.ObstructionPoint + direction * 1e3f;

                            float distance = Vector3.Distance(m_navBlock.WorldPosition, pointOfObstruction.Value);

                            MoveObstruction = obstructing;
                            m_pathHigh.Clear();
                            ClearAltPath();
                            if ((m_planetCheckDest.CurrentState & PlanetChecker.State.BlockedPath) != 0)
                                FindAlternate_AroundObstruction(pointOfObstruction.Value - m_navBlock.WorldPosition, new Vector3[] { direction }, 1e4f, runId);
                            else // blocked by gravity
                                FindAlternate_AroundObstruction(pointOfObstruction.Value - m_navBlock.WorldPosition, new Vector3[] { direction }, 1e6f, runId);
                            m_pathLow.Enqueue(() => {
                                if (m_altPath_AlternatesFound != 0)
                                    SetWaypoint();
                                RunItem();
                            });
                            m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);

                            return;
                        }
                    }
                }

                // for alternates, check that it can be better than current value
                if (isAlternate)
                {
                    float distToWaypointSquared = (float)Vector3D.DistanceSquared(m_navBlock.WorldPosition, destination);
                    if (distToWaypointSquared * WaypointDistanceBias * WaypointDistanceBias > m_altPath_PathValue * m_altPath_PathValue)
                    {
                        m_logger.debugLog("no point in checking alternate path, bias is too high", Logger.severity.TRACE);
                        m_logger.debugLog(m_altPath_AlternatesFound == 0, "no alternate, yet path value is set", Logger.severity.ERROR);
                        IncrementAlternatesFound();
                        return;
                    }
                }

                if (m_pathChecker.TestFast(m_navBlock, destination, m_ignoreAsteroid, ignoreEntity, m_landing))
                {
                    m_logger.debugLog("path is clear (fast)", Logger.severity.TRACE);
                    PathClear(ref destination, runId, isAlternate, slowDown);
                    return;
                }

                if (m_pathChecker.TestSlow(out obstructing, out pointOfObstruction))
                {
                    m_logger.debugLog("path is clear (slow)", Logger.severity.TRACE);
                    PathClear(ref destination, runId, isAlternate, slowDown);
                    return;
                }

                if (runId != m_runId)
                {
                    m_logger.debugLog("destination changed, abort", Logger.severity.DEBUG);
                    return;
                }

                if (slowDown)
                {
                    float speed = Vector3.Distance(pointOfObstruction.Value, m_navBlock.WorldPosition) * 0.1f;
                    if (speed < 1f)
                        speed = 1f;
                    IMyEntity destEntity = m_navSet.Settings_Current.DestinationEntity;
                    if (destEntity != null)
                        destEntity = destEntity.GetTopMostParent();
                    if (obstructing.GetTopMostParent() == destEntity)
                    {
                        m_navSet.Settings_Task_NavWay.SpeedMaxRelative = speed;
                        m_logger.debugLog("Set SpeedMaxRelative to " + speed + ", obstructing: " + obstructing.getBestName() + ", DestinationEntity: " + m_navSet.Settings_Current.DestinationEntity, Logger.severity.TRACE);
                    }
                    else
                    {
                        m_navSet.Settings_Task_NavWay.SpeedTarget = speed;
                        m_logger.debugLog("Set SpeedTarget to " + speed + ", obstructing: " + obstructing.getBestName() + ", DestinationEntity: " + m_navSet.Settings_Current.DestinationEntity, Logger.severity.TRACE);
                    }
                    return;
                }

                if (m_pathState < PathState.Searching)
                    m_pathState = PathState.Searching;

                m_logger.debugLog("path is blocked by " + obstructing.getBestName() + " at " + pointOfObstruction + ", ignoreEntity: " + ignoreEntity.getBestName(), isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
                m_logger.debugLog(obstructing is IMyCubeBlock, "grid: " + obstructing.GetTopMostParent().DisplayName, isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);

                if (isAlternate && m_altPath_AlternatesFound != 0)
                    IncrementAlternatesFound();

                if (tryAlternates)
                {
                    //float angle = m_navSet.Settings_Current.DistanceAngle;
                    //if (angle > 0.1f && CanRotate)
                    //{
                    //	m_logger.debugLog("wait for rotation", "TestPath()");
                    //	return;
                    //}

                    if (m_navSet.Settings_Task_NavEngage.NavigatorMover != m_navSet.Settings_Current.NavigatorMover)
                    {
                        m_logger.debugLog("obstructed while flying to a waypoint, throwing it out and starting over", Logger.severity.DEBUG);
                        m_navSet.OnTaskComplete_NavWay();
                        return;
                    }

                    ClearAltPath();
                    MoveObstruction = obstructing;
                    TryAlternates(runId, pointOfObstruction.Value, obstructing);
                }
            }
            finally
            {
                lock_testPath.ReleaseExclusive();
                RunItem();
            }
        }