C# LineSegment.PointInCylinder方法代码示例

		/// <summary>
		/// Test if it is safe for the grid to rotate.
		/// </summary>
		/// <param name="axis">Normalized axis of rotation in world space.</param>
		/// <param name="ignoreAsteroids"></param>
		/// <returns>True iff the path is clear.</returns>
		public bool TestRotate(Vector3 axis, bool ignoreAsteroids, out IMyEntity obstruction)
		{
			Vector3 centreOfMass = m_grid.Physics.CenterOfMassWorld;
			float longestDim = m_grid.GetLongestDim();

			// calculate height
			Matrix toMyLocal = m_grid.WorldMatrixNormalizedInv;
			Vector3 myLocalCoM = Vector3.Transform(centreOfMass, toMyLocal);
			Vector3 myLocalAxis = Vector3.Transform(axis, toMyLocal.GetOrientation());
			Vector3 myLocalCentre = m_grid.LocalAABB.Center; // CoM may not be on ship (it now considers mass from attached grids)
			Ray upper = new Ray(myLocalCentre + myLocalAxis * longestDim * 2f, -myLocalAxis);
			float? upperBound = m_grid.LocalAABB.Intersects(upper);
			if (!upperBound.HasValue)
				m_logger.alwaysLog("Math fail, upperBound does not have a value", "TestRotate()", Logger.severity.FATAL);
			Ray lower = new Ray(myLocalCentre - myLocalAxis * longestDim * 2f, myLocalAxis);
			float? lowerBound = m_grid.LocalAABB.Intersects(lower);
			if (!lowerBound.HasValue)
				m_logger.alwaysLog("Math fail, lowerBound does not have a value", "TestRotate()", Logger.severity.FATAL);
			m_logger.debugLog("LocalAABB: " + m_grid.LocalAABB + ", centre: " + myLocalCentre + ", axis: " + myLocalAxis + ", longest dimension: " + longestDim + ", upper ray: " + upper + ", lower ray: " + lower, "TestRotate()");
			float height = longestDim * 4f - upperBound.Value - lowerBound.Value;

			float furthest = 0f;
			m_cells.ForEach(cell => {
				Vector3 rejection = Vector3.Reject(cell * m_grid.GridSize, myLocalAxis);
				float cellDistSquared = Vector3.DistanceSquared(myLocalCoM, rejection);
				if (cellDistSquared > furthest)
					furthest = cellDistSquared;
			});
			float length = (float)Math.Sqrt(furthest) + m_grid.GridSize * 0.5f;

			m_logger.debugLog("height: " + height + ", length: " + length, "TestRotate()");

			BoundingSphereD surroundingSphere = new BoundingSphereD(centreOfMass, Math.Max(length, height));
			m_obstructions.Clear();
			MyGamePruningStructure.GetAllTopMostEntitiesInSphere(ref surroundingSphere, m_obstructions);

			LineSegment axisSegment = new LineSegment();

			foreach (MyEntity entity in m_obstructions)
				if (PathChecker.collect_Entity(m_grid, entity))
				{
					if (entity is IMyVoxelBase)
					{
						if (ignoreAsteroids)
							continue;

						IMyVoxelMap voxel = entity as IMyVoxelMap;
						if (voxel != null)
						{
							if (voxel.GetIntersectionWithSphere(ref surroundingSphere))
							{
								m_logger.debugLog("Too close to " + voxel.getBestName() + ", CoM: " + centreOfMass.ToGpsTag("Centre of Mass") + ", required distance: " + surroundingSphere.Radius, "TestRotate()");
								obstruction = voxel;
								return false;
							}
							continue;
						}
						MyPlanet planet = entity as MyPlanet;
						if (planet != null && planet.Intersects(ref surroundingSphere))
						{
							obstruction = planet;
							return false;
						}
						continue;
					}

					IMyCubeGrid grid = entity as IMyCubeGrid;
					if (grid != null)
					{
						Matrix toLocal = grid.WorldMatrixNormalizedInv;
						Vector3 localAxis = Vector3.Transform(axis, toLocal.GetOrientation());
						Vector3 localCentre = Vector3.Transform(centreOfMass, toLocal);
						axisSegment.From = localCentre - localAxis * height;
						axisSegment.To = localCentre + localAxis * height;

						bool found = false;
						GridCellCache.GetCellCache(grid).ForEach(cell => {
							if (axisSegment.PointInCylinder(length, cell * grid.GridSize))
							{
								found = true;
								return true;
							}
							return false;
						});

						if (found)
						{
							obstruction = grid;
							return false;
						}
						continue;
					}

					m_logger.debugLog("No tests for object: " + entity.getBestName(), "TestRotate()", Logger.severity.INFO);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        //                if (Vector3D.DistanceSquared(planetCentre, closestPoint) < Vector3D.DistanceSquared(planetCentre, finalDestination))
        //                {
        //                    m_logger.debugLog("closest point is end, which is closer to planet than final dest is", "GravityTest()");
        //                    blockingPath = entity;
        //                    pointOfObstruction = closestPoint;
        //                    return false;
        //                }
        //                m_logger.debugLog("closest point is end", "GravityTest()");
        //                continue;
        //            }
        //            //float startGravity = planet.GetWorldGravityGrid( line.From).LengthSquared();
        //            //float closestGravity = planet.GetWorldGravityGrid(closestPoint).LengthSquared();
        //            //double toDistSq = Vector3D.DistanceSquared(planetCentre, line.To);
        //            //if (closestPoint == line.To && (line.To == finalDestination ||     ))
        //            //{
        //            //	m_logger.debugLog("path: " + line.From + " to " + line.To + ", closest point: " + closestPoint + ", planet @ " + planetCentre + " : " + planet.getBestName(), "GravityTest()");
        //            //	m_logger.debugLog("closest point is end", "GravityTest()");
        //            //	continue;
        //            //}
        //            //m_logger.debugLog("path: " + line.From + " to " + line.To + ", closest point: " + closestPoint + ", planet @ " + planetCentre + " : " + planet.getBestName(), "GravityTest()", Logger.severity.DEBUG);
        //            double closestDistSq = Vector3D.DistanceSquared(planetCentre, closestPoint) + 1f;
        //            if (closestDistSq < Vector3D.DistanceSquared(planetCentre, line.From) || closestDistSq < Vector3D.DistanceSquared(planetCentre, line.To))
        //            {
        //                m_logger.debugLog("path moves ship closer to planet. closestDistSq: " + closestDistSq + ", from dist sq: " + Vector3D.DistanceSquared(planetCentre, line.From) + ", to dist sq: " + Vector3D.DistanceSquared(planetCentre, line.To), "GravityTest()", Logger.severity.INFO);
        //                blockingPath = entity;
        //                pointOfObstruction = closestPoint;
        //                return false;
        //            }
        //        }
        //    }
        //    blockingPath = null;
        //    pointOfObstruction = null;
        //    return true;
        //}
        /// <summary>
        /// How far long the line would the ship be able to travel? Uses a capsule derived from previously calculated path.
        /// </summary>
        /// <param name="canTravel">Line along which navigation block would travel</param>
        /// <remarks>
        /// Capsule only test because the ship will not be oriented correctly
        /// </remarks>
        /// <returns>distance from the destination that can be reached</returns>
        public float distanceCanTravel(LineSegment canTravel)
        {
            BoundingBoxD atFrom = m_grid.WorldAABB.Translate(canTravel.From - m_grid.GetPosition());
            BoundingBoxD atTo = m_grid.WorldAABB.Translate(canTravel.To - m_grid.GetPosition());

            ICollection<MyEntity> offenders = EntitiesInLargeAABB(atFrom, atTo);
            if (offenders.Count == 0)
            {
                m_logger.debugLog("AABB is empty");
                return 0;
            }
            m_logger.debugLog("collected entities to test: " + offenders.Count);
            IOrderedEnumerable<MyEntity> ordered = offenders.OrderBy(entity => Vector3D.Distance(canTravel.From, entity.GetCentre()));

            Capsule path = new Capsule(canTravel.From, canTravel.To, m_path.Radius);
            Vector3? pointOfObstruction = null;
            foreach (MyEntity entity in ordered)
                if (path.IntersectsAABB(entity))
                {
                    MyVoxelBase voxel = entity as MyVoxelBase;
                    if (voxel != null)
                        if (!TestVoxel(voxel, path, out pointOfObstruction))
                        {
                            m_logger.debugLog("obstruction at " + pointOfObstruction + " distance from dest is " + Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction));
                            return Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction);
                        }

                    IMyCubeGrid grid = entity as IMyCubeGrid;
                    if (grid != null)
                    {
                        float minDistSq = (m_grid.GridSize + grid.GridSize) * (m_grid.GridSize + grid.GridSize);
                        GridCellCache cache = GridCellCache.GetCellCache(grid);
                        cache.ForEach(cell => {
                            Vector3 cellPos = grid.GridIntegerToWorld(cell);
                            if (canTravel.PointInCylinder(minDistSq, ref cellPos))
                            {
                                pointOfObstruction = cellPos;
                                return true;
                            }
                            return false;
                        });
                        if (pointOfObstruction.HasValue)
                            return Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction);
                    }

                    m_logger.debugLog("not a grid, testing bounds");
                    if (!m_path.IntersectsVolume(entity))
                        continue;

                    m_logger.debugLog("no more tests for non-grids are implemented", Logger.severity.DEBUG);
                    pointOfObstruction = m_path.get_Line().ClosestPoint(entity.GetCentre());
                    return Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction);
                }

            // no obstruction
            m_logger.debugLog("no obstruction");
            return 0f;
        }