C# IMyEntity.GetLinearVelocity方法代码示例

		private void TryAlternates(byte runId, Vector3 pointOfObstruction, IMyEntity obstructing)
		{
			try
			{
				m_pathHigh.Clear();
			}
			catch (IndexOutOfRangeException ioore)
			{
				m_logger.debugLog("Caught IndexOutOfRangeException", "TryAlternates()", Logger.severity.ERROR);
				m_logger.debugLog("Count: " + m_pathHigh.Count, "TryAlternates()", Logger.severity.ERROR);
				m_logger.debugLog("Exception: " + ioore, "TryAlternates()", Logger.severity.ERROR);

				throw ioore;
			}

			Vector3 displacement = pointOfObstruction - m_navBlock.WorldPosition;

			if (m_canChangeCourse)
			{
				FindAlternate_AroundObstruction(displacement, pointOfObstruction, obstructing.GetLinearVelocity(), runId);
				FindAlternate_JustMove(runId);
			}
			FindAlternate_HalfwayObstruction(displacement, runId);
			m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
		}

		private bool ProjectileIsThreat(IMyEntity projectile, TargetType tType)
		{
			if (projectile.Closed)
				return false;

			Vector3D projectilePosition = projectile.GetPosition();
			BoundingSphereD ignoreArea = new BoundingSphereD(ProjectilePosition(), Options.TargetingRange / 10f);
			if (ignoreArea.Contains(projectilePosition) == ContainmentType.Contains)
				return false;

			Vector3D weaponPosition = ProjectilePosition();
			Vector3D nextPosition = projectilePosition + projectile.GetLinearVelocity() / 60f;
			if (Vector3D.DistanceSquared(weaponPosition, nextPosition) < Vector3D.DistanceSquared(weaponPosition, projectilePosition))
			{
				myLogger.debugLog("projectile: " + projectile.getBestName() + ", is moving towards weapon. D0 = " + Vector3D.DistanceSquared(weaponPosition, nextPosition) + ", D1 = " + Vector3D.DistanceSquared(weaponPosition, projectilePosition), "ProjectileIsThreat()");
				return true;
			}
			else
			{
				myLogger.debugLog("projectile: " + projectile.getBestName() + ", is moving away from weapon. D0 = " + Vector3D.DistanceSquared(weaponPosition, nextPosition) + ", D1 = " + Vector3D.DistanceSquared(weaponPosition, projectilePosition), "ProjectileIsThreat()");
				return false;
			}
		}

        /// <summary>
        /// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
        /// </summary>
        /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
        /// <param name="Direction">The direction to face the localMatrix in.</param>
        private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
        {
            m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR);

            m_gyro.Update();
            float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
            if (minimumMoment <= 0f)
            {
                // == 0f, not calculated yet. < 0f, we have math failure
                StopRotate();
                m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL);
                return;
            }

            localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
            Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

            localMatrix = localMatrix.GetOrientation();
            inverted = inverted.GetOrientation();

            Vector3 localDirect = Direction.ToLocalNormalized();
            Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

            float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

            Vector3 rotaRight = localMatrix.Right;
            Vector3 rotaUp = localMatrix.Up;

            Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
            Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

            Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

            if (UpDirect != null)
            {
                Vector3 upLocal = UpDirect.ToLocalNormalized();
                Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
                upRotBlock.Z = 0f;
                upRotBlock.Normalize();
                float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));

                Vector3 rotaBackward = localMatrix.Backward;
                Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

                //m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");

                displacement += roll * NFR_backward;
            }

            m_lastMoveAttempt = Globals.UpdateCount;
            Pathfinder.TestRotate(displacement);

            switch (Pathfinder.m_rotateState)
            {
                case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running:
                    m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState);
                    m_lastMove = Globals.UpdateCount;
                    return;
                case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction:
                    break;
                default:
                    m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState);
                    return;
            }

            float distanceAngle = displacement.Length();
            if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle))
            {
                m_bestAngle = distanceAngle;
                m_lastMove = Globals.UpdateCount;
            }
            m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;

            //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");

            m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);

            // adjustment to face a moving entity
            if (targetEntity != null)
            {
                Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity;
                float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());

                //myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");

                float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
                float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
                float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
                float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);

                m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));

                m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
            }
            //m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");
//.........这里部分代码省略.........

        /// <summary>
        /// Enqueue searches for alternate paths.
        /// </summary>
        /// <param name="runId">Id of search</param>
        /// <param name="pointOfObstruction">The point on the path where an obstruction was encountered.</param>
        /// <param name="obstructing">The entity that is obstructing the path.</param>
        private void TryAlternates(byte runId, Vector3 pointOfObstruction, IMyEntity obstructing)
        {
            //try
            //{
            m_pathHigh.Clear();
            //}
            //catch (IndexOutOfRangeException ioore)
            //{
            //	m_logger.debugLog("Caught IndexOutOfRangeException", "TryAlternates()", Logger.severity.ERROR);
            //	m_logger.debugLog("Count: " + m_pathHigh.Count, "TryAlternates()", Logger.severity.ERROR);
            //	m_logger.debugLog("Exception: " + ioore, "TryAlternates()", Logger.severity.ERROR);

            //	throw ioore;
            //}

            Vector3 displacement = pointOfObstruction - m_navBlock.WorldPosition;

            FindAlternate_HalfwayObstruction(displacement, runId);
            if (m_canChangeCourse)
            {
                // using a halfway point works much better when the obstuction is near the destination
                FindAlternate_AroundObstruction(displacement * 0.5f, obstructing.GetLinearVelocity(), runId);
                //FindAlternate_AroundObstruction(displacement, pointOfObstruction, obstructing.GetLinearVelocity(), runId);
                FindAlternate_JustMove(runId);
            }
            m_pathLow.Enqueue(() => {
                if (m_altPath_AlternatesFound != 0)
                    SetWaypoint();
                RunItem();
            });
            m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
        }

//.........这里部分代码省略.........
			rotBlockDirect.Normalize();

			float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

			Vector3 rotaRight = localMatrix.Right;
			Vector3 rotaUp = localMatrix.Up;

			Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
			Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

			Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

			if (UpDirect != null)
			{
				Vector3 upLocal = UpDirect.ToLocal();
				Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
				float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);

				Vector3 rotaBackward = localMatrix.Backward;
				Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

				myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (roll * NFR_backward), "CalcRotate()");

				displacement += roll * NFR_backward;
			}

			NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();

			if (NavSet.Settings_Current.CollisionAvoidance)
			{
				myPathfinder.TestRotate(displacement);
				if (!myPathfinder.CanRotate)
				{
					// if cannot rotate and not calculating move, move away from obstruction
					if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove)
					{
						Vector3 position  = Block.CubeBlock.GetPosition();
						Vector3 away = position - myPathfinder.RotateObstruction.GetCentre();
						away.Normalize();
						myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO);
						new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true);
					}
					Logger.debugNotify("Cannot Rotate", 50);
					myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
					return;
				}
			}

			//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");

			if (myGyro.torqueAccelRatio == 0)
			{
				// do a test
				myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
				rotateForceRatio = new Vector3(0, 1f, 0);
				return;
			}

			Vector3 targetVelocity = MaxAngleVelocity(displacement, secondsSinceLast);

			if (targetEntity != null)
			{
				Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - Block.Physics.LinearVelocity;
				float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
				//Vector3 tangentialVelocity = Vector3.Reject(relativeLinearVelocity, targetEntity.GetCentre() - Block.CubeBlock.GetPosition());
				//Vector3 localTangVel = Vector3.Transform(tangentialVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

				//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "CalcRotate()");

				float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Up);
				float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Left);
				float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
				float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);

				myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch, "CalcRotate()");

				targetVelocity += new Vector3(angl_pitch, angl_yaw, 0f);
			}

			Vector3 accel = (targetVelocity - angularVelocity) / secondsSinceLast;

			rotateForceRatio = accel / (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce);

			myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + accel, "CalcRotate()");
			myLogger.debugLog("accel: " + accel + ", torque: " + (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");

			// dampeners
			for (int i = 0; i < 3; i++)
			{
				// if targetVelocity is close to 0, use dampeners

				float target = targetVelocity.GetDim(i);
				if (target > -0.01f && target < 0.01f)
				{
					//myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
					rotateForceRatio.SetDim(i, 0f);
					continue;
				}
			}
		}