C# Vector3D.Length方法代码示例

        public void TestLength()
        {
            float x = -62, y = 5, z = 10;

            Vector3D v = new Vector3D(x, y, z);
            Assert.AreEqual((float) Math.Sqrt(x*x + y*y + z*z), v.Length(), "Testing v.Length()");
        }

        // Thanks to albmar @ http://forum.keenswh.com/threads/how-can-i-roll-my-ship-to-align-its-floor-with-the-floor-of-a-station.7382390/#post-1286963408
        // So I did take a university level analytical mechanics course at one point but it turns out I forgot everything so thanks for the save
        private bool rotate(Vector3D destOrient, Vector3D curOrient)
        {
            bool doneRotating = false;

            Echo("Rotating");

            // To be sure, we divide by the length of both vectors
            Vector3D axis = Vector3D.Cross(destOrient, curOrient) / (destOrient.Length() * curOrient.Length());
            // the Normalize method normalizes the axis and returns the length it had before
            double angle = Math.Asin(axis.Normalize());

            foreach (var gyro in gyros)
            {
                MatrixD worldToGyro = MatrixD.Invert(gyro.WorldMatrix.GetOrientation());
                Vector3D localAxis = Vector3D.Transform(axis, worldToGyro);

                // Stop if we reached threshold
                double value = Math.Log(angle + 1, 2);
                if (value < 0.0001)
                {
                    localAxis *= 0;
                    doneRotating = true;
                }
                else
                {
                    localAxis *= value;
                }

                gyro.SetValueBool("Override", true);
                gyro.SetValueFloat("Power", 1f);
                // Seems at some point KSH changed the signs on pitch/yaw/roll
                gyro.SetValue("Pitch", (float)-localAxis.X);
                gyro.SetValue("Yaw", (float)localAxis.Y);
                gyro.SetValue("Roll", (float)localAxis.Z);
            }

            if (doneRotating)
            {
                resetGyros();
            }

            timer.ApplyAction("TriggerNow");

            return doneRotating;
        }

        public override float GetVolume(ref Vector3D voxelPosition)
        {
            if (m_inverseIsDirty)
            {
                m_inverse = MatrixD.Invert(m_transformation);
                m_inverseIsDirty = false;
            }

            // Local voxel position
            voxelPosition = Vector3D.Transform(voxelPosition, m_inverse);
            // Local voxel position in unit sphere space
            Vector3 voxelInUnitSphere = Vector3.Transform(voxelPosition, m_scaleMatrixInverse);
            // Normalize the position so we have point on unit sphere.
            voxelInUnitSphere.Normalize();
            // Transform back to local system using scale matrix
            Vector3 localPointOnEllipsoid = Vector3.Transform(voxelInUnitSphere, m_scaleMatrix);

            float diff = (float)(voxelPosition.Length() - localPointOnEllipsoid.Length());
            return SignedDistanceToDensity(diff);
        }

        private void UpdateThrust(Vector3D delta, Vector3D perpDelta, double maxSpeed)
        {
            var thrustSystem = CubeGrid.Components.Get<MyEntityThrustComponent>();
	        if (thrustSystem == null)
		        return;

            thrustSystem.AutoPilotControlThrust = Vector3.Zero;

            // Planet-related stuff
            m_dbgDeltaH = Vector3.Zero;
            if (m_currentInfo.IsValid())
            {
                // Sample several points around the bottom of the ship to get a better estimation of the terrain underneath
                Vector3D shipCenter = CubeGrid.PositionComp.WorldVolume.Center + m_currentInfo.GravityWorld * CubeGrid.PositionComp.WorldVolume.Radius;

                // Limit max speed if too close to the ground.
                Vector3D samplePosition;
                m_thdPtr++;
                // A velocity-based sample
                if (m_thdPtr >= TERRAIN_HEIGHT_DETECTION_SAMPLES)
                {
                    m_thdPtr = 0;
                    samplePosition = shipCenter + new Vector3D(CubeGrid.Physics.LinearVelocity) * 5.0;
                }
                // Flight direction sample
                else if (m_thdPtr == 1)
                {
                    Vector3D direction = WorldMatrix.GetDirectionVector(m_currentDirection);
                    samplePosition = shipCenter + direction * CubeGrid.PositionComp.WorldVolume.Radius * 2.0;
                }
                // Random samples
                else
                {
                    Vector3D tangent = Vector3D.CalculatePerpendicularVector(m_currentInfo.GravityWorld);
                    Vector3D bitangent = Vector3D.Cross(tangent, m_currentInfo.GravityWorld);
                    samplePosition = MyUtils.GetRandomDiscPosition(ref shipCenter, CubeGrid.PositionComp.WorldVolume.Radius, ref tangent, ref bitangent);
                }

                if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_DRONES)
                {
                    MyRenderProxy.DebugDrawCapsule(samplePosition, samplePosition + m_currentInfo.GravityWorld * 50.0f, 1.0f, Color.Yellow, false);
                }

                if (m_currentInfo.IsValid())
                {
                    m_terrainHeightDetection[m_thdPtr] = (float)m_currentInfo.EstimateDistanceToGround(samplePosition);
                }
                else
                {
                    m_terrainHeightDetection[m_thdPtr] = 0.0f;
                }
                double distanceToGround = m_terrainHeightDetection[0];
                for (int i = 1; i < TERRAIN_HEIGHT_DETECTION_SAMPLES; ++i)
                {
                    distanceToGround = Math.Min(distanceToGround, m_terrainHeightDetection[i]);
                }

                if (distanceToGround < 0.0f) Debugger.Break();

                // Below 50m, the speed will be minimal, Above 150m, it will be maximal
                // coeff(50) = 0, coeff(150) = 1
                double coeff = (distanceToGround - 50.0) * 0.01;
                if (coeff < 0.05) coeff = 0.15;
                if (coeff > 1.0f) coeff = 1.0f;
                maxSpeed = maxSpeed * Math.Max(coeff, 0.05);

                double dot = m_currentInfo.PlanetVector.Dot(m_currentInfo.PlanetVector);
                double deltaH = m_currentInfo.Elevation - m_currentInfo.Elevation;
                double deltaPSq = perpDelta.LengthSquared();

                // Add height difference compensation on long distances (to avoid flying off the planet)
                if (dot < 0.99 && deltaPSq > 100.0)
                {
                    m_dbgDeltaH = -deltaH * m_currentInfo.GravityWorld;
                }

                delta += m_dbgDeltaH;

                //For now remove this (causes cubegrid to get stuck at a height)
                // If we are very close to the ground, just thrust upward to avoid crashing.
                // The coefficient is set-up that way that at 50m, this thrust will overcome the thrust to the target.
                //double groundAvoidanceCoeff = Math.Max(0.0, Math.Min(1.0, distanceToGround * 0.1));
                //delta = Vector3D.Lerp(-m_currentInfo.GravityWorld * delta.Length(), delta, groundAvoidanceCoeff);
            }

            m_dbgDelta = delta;

            Matrix invWorldRot = CubeGrid.PositionComp.WorldMatrixNormalizedInv.GetOrientation();

            Vector3D targetDirection = delta;
            targetDirection.Normalize();

            Vector3D velocity = CubeGrid.Physics.LinearVelocity;

            Vector3 localSpaceTargetDirection = Vector3.Transform(targetDirection, invWorldRot);
            Vector3 localSpaceVelocity = Vector3.Transform(velocity, invWorldRot);

            thrustSystem.AutoPilotControlThrust = Vector3.Zero;

            Vector3 brakeThrust = thrustSystem.GetAutoPilotThrustForDirection(Vector3.Zero);
//.........这里部分代码省略.........

        private Vector3D AvoidCollisions(Vector3D delta, ref float autopilotSpeedLimit)
        {
            if (m_collisionCtr <= 0)
            {
                m_collisionCtr = 0;
            }
            else
            {
                m_collisionCtr--;
                return m_oldCollisionDelta;
            }

            bool drawDebug = MyDebugDrawSettings.ENABLE_DEBUG_DRAW;// && MyDebugDrawSettings.DEBUG_DRAW_DRONES;

            Vector3D originalDelta = delta;

            Vector3D origin = this.CubeGrid.Physics.CenterOfMassWorld;
            double shipRadius = this.CubeGrid.PositionComp.WorldVolume.Radius * 1.3f;
            if (MyFakes.ENABLE_VR_DRONE_COLLISIONS) //TODO VR: this MyFake should be enabled in VR but disabled in SE
                shipRadius = this.CubeGrid.PositionComp.WorldVolume.Radius * 1f;

            Vector3D linVel = this.CubeGrid.Physics.LinearVelocity;

            double vel = linVel.Length();
            double detectionRadius = this.CubeGrid.PositionComp.WorldVolume.Radius * 10.0f + (vel * vel) * 0.05;
            if (MyFakes.ENABLE_VR_DRONE_COLLISIONS)
                detectionRadius = this.CubeGrid.PositionComp.WorldVolume.Radius + (vel * vel) * 0.05;
            BoundingSphereD sphere = new BoundingSphereD(origin, detectionRadius);

            Vector3D testPoint = sphere.Center + linVel * 2.0f;
            if (MyFakes.ENABLE_VR_DRONE_COLLISIONS)
                testPoint = sphere.Center + linVel;

            if (drawDebug)
            {
                MyRenderProxy.DebugDrawSphere(sphere.Center, (float)shipRadius, Color.HotPink, 1.0f, false);
                MyRenderProxy.DebugDrawSphere(sphere.Center + linVel, 1.0f, Color.HotPink, 1.0f, false);
                MyRenderProxy.DebugDrawSphere(sphere.Center, (float)detectionRadius, Color.White, 1.0f, false);
            }

            Vector3D steeringVector = Vector3D.Zero;
            Vector3D avoidanceVector = Vector3D.Zero;
            int n = 0;

            double maxAvCoeff = 0.0f;

            var entities = MyEntities.GetTopMostEntitiesInSphere(ref sphere);
            IMyGravityProvider well;
            if (MyGravityProviderSystem.GetStrongestNaturalGravityWell(origin, out well) > 0 && well is MyGravityProviderComponent)
            {
                MyEntity e = (MyEntity)((MyGravityProviderComponent)well).Entity;
                if (!entities.Contains(e)) entities.Add(e);
            }

            for (int i = 0; i < entities.Count; ++i)
            {
                var entity = entities[i];

                if (entity == this.Parent) continue;

                Vector3D steeringDelta = Vector3D.Zero;
                Vector3D avoidanceDelta = Vector3D.Zero;

                if ((entity is MyCubeGrid) || (entity is MyVoxelMap) || (entity is MySkinnedEntity))
                {
                    if (MyFakes.ENABLE_VR_DRONE_COLLISIONS && (entity is MyCubeGrid))
                    {
                        var grid = entity as MyCubeGrid;
                        if (grid.IsStatic)
                        {
                            continue;
                        }
                    }

                    if (entity is MyCubeGrid)
                    {
                        var grid = entity as MyCubeGrid;
                        if(MyCubeGridGroups.Static.Physical.GetGroup(CubeGrid) == MyCubeGridGroups.Static.Physical.GetGroup(grid))
                        {
                            continue;
                        }
                    }

                    var otherSphere = entity.PositionComp.WorldVolume;
                    otherSphere.Radius += shipRadius;
                    Vector3D offset = otherSphere.Center - sphere.Center;

                    if (this.CubeGrid.Physics.LinearVelocity.LengthSquared() > 5.0f && Vector3D.Dot(delta, this.CubeGrid.Physics.LinearVelocity) < 0) continue;

                    // Collision avoidance
                    double dist = offset.Length();
                    BoundingSphereD forbiddenSphere = new BoundingSphereD(otherSphere.Center + linVel, otherSphere.Radius + vel);
                    if (forbiddenSphere.Contains(testPoint) == ContainmentType.Contains)
                    {
                        autopilotSpeedLimit = 2.0f;
                        if (drawDebug)
                        {
                            MyRenderProxy.DebugDrawSphere(forbiddenSphere.Center, (float)forbiddenSphere.Radius, Color.Red, 1.0f, false);
                        }
                    }
//.........这里部分代码省略.........

        private static void SpawnEncounter(MyEncounterId encounterPosition, Vector3D placePosition, List<MySpawnGroupDefinition> candidates, int selectedEncounter)
        {
            var spawnGroup = candidates[selectedEncounter];

            long ownerId = 0; // 0 means that the owner won't be changed
            if (spawnGroup.IsPirate)
                ownerId = MyPirateAntennas.GetPiratesId();

            foreach (var selectedPrefab in spawnGroup.Prefabs)
            {
                List<MyCubeGrid> createdGrids = new List<MyCubeGrid>();
                Vector3D direction = Vector3D.Forward;
                Vector3D upVector = Vector3D.Up;

                var spawningOptions = spawnGroup.ReactorsOn ? VRage.Game.ModAPI.SpawningOptions.None : VRage.Game.ModAPI.SpawningOptions.TurnOffReactors;
                if (selectedPrefab.Speed > 0.0f)
                {
                    spawningOptions = VRage.Game.ModAPI.SpawningOptions.RotateFirstCockpitTowardsDirection |
                                     VRage.Game.ModAPI.SpawningOptions.SpawnRandomCargo |
                                     VRage.Game.ModAPI.SpawningOptions.DisableDampeners;

                    float centerArcRadius = (float)Math.Atan(MyNeutralShipSpawner.NEUTRAL_SHIP_FORBIDDEN_RADIUS / placePosition.Length());
                    direction = -Vector3D.Normalize(placePosition);
                    float theta = m_random.NextFloat(centerArcRadius, centerArcRadius + MyNeutralShipSpawner.NEUTRAL_SHIP_DIRECTION_SPREAD);
                    float phi = m_random.NextFloat(0, 2 * MathHelper.Pi);
                    Vector3D cosVec = Vector3D.CalculatePerpendicularVector(direction);
                    Vector3D sinVec = Vector3D.Cross(direction, cosVec);
                    cosVec *= (Math.Sin(theta) * Math.Cos(phi));
                    sinVec *= (Math.Sin(theta) * Math.Sin(phi));
                    direction = direction * Math.Cos(theta) + cosVec + sinVec;

                    upVector = Vector3D.CalculatePerpendicularVector(direction);
                }
                spawningOptions |= VRage.Game.ModAPI.SpawningOptions.DisableSave;

                if (selectedPrefab.PlaceToGridOrigin) spawningOptions |= SpawningOptions.UseGridOrigin;

                Stack<Action> callback = new Stack<Action>();
                callback.Push(delegate() { ProcessCreatedGrids(ref encounterPosition, selectedPrefab.Speed, createdGrids); });

                MyPrefabManager.Static.SpawnPrefab(
                   resultList: createdGrids,
                   prefabName: selectedPrefab.SubtypeId,
                   position: placePosition + selectedPrefab.Position,
                   forward: direction,
                   up:upVector,
                   beaconName: selectedPrefab.BeaconText,
                   initialLinearVelocity: direction * selectedPrefab.Speed,
                   spawningOptions: spawningOptions | SpawningOptions.UseGridOrigin,
                   ownerId: ownerId,
                   updateSync: true,
                   callbacks: callback);

            }
        }

        public static Color CalculateColor(Vector3D origin, Vector3D direction, uint depth)
        {
            /*
             * Najpierw sprawdzamy czy promien uderza w kule, jesli nie to musi
             * uderzac w plaszczyzne za kula (ze wzgledu na przyjeta specyfike
             * konstrukcji sceny. Przeciecie z plaszczyzna rowniez nalezy
             * sprawdzic, aby obliczyc kolor i stworzyc "szachownice"
             */
            Color returnColor = Color.FromArgb(0, 0, 0);
            Vector3D intersectionPoint = new Vector3D();
            float distance = float.MaxValue;
            bool inside = false;

            bool sphereHit = raySphereIntersection(origin, direction, out distance, out inside);

            // Jeśli kula nie zostala trafiona
            if (!sphereHit)
            {
                // Sprawdz przeciecie z plaszczyznami
                //float tmpDistance = -1;
                float distRight = -(planeR.d + planeR.vectorNormal.DotProduct(origin)) / (direction.DotProduct(planeR.vectorNormal));
                float distLeft = -(planeL.d + planeL.vectorNormal.DotProduct(origin)) / (direction.DotProduct(planeL.vectorNormal));
                float distDown  = -(planeD.d + planeD.vectorNormal.DotProduct(origin)) / (direction.DotProduct(planeD.vectorNormal));

                float dist = Math.Min(distLeft, distRight);
                dist = distDown > 0 ? Math.Min(distDown, dist) : dist;

                intersectionPoint = origin + direction * dist;

                bool hit = distRight > 0 || distLeft > 0 || distDown > 0;
                if (hit)
                {
                    if (distDown == dist)
                    {
                        returnColor = checkeredTexture(intersectionPoint.x, intersectionPoint.z);
                    }
                    else
                    {
                        returnColor = checkeredTexture(intersectionPoint.x, intersectionPoint.y);
                    }
                }

                return returnColor;
            }

            //if (inside) Console.WriteLine("INSIDE");

            Vector3D vectorObser = new Vector3D();
            Vector3D vectorLight = new Vector3D();
            Vector3D reflectedRay = new Vector3D();
            intersectionPoint = origin + direction * distance;

            // Ponieważ mamy tylko jedno światło nie trzeba robić pętli po wszystkich
            // Wektor od punktu przeciecia do swiatla
            vectorLight.x = (float)light.position.X - intersectionPoint.x;
            vectorLight.y = (float)light.position.Y - intersectionPoint.y;
            vectorLight.z = (float)light.position.Z - intersectionPoint.z;

            float lengthLight = vectorLight.Length();
            vectorLight /= lengthLight;

            // Wektor od punktu przeciecia do obserwatora

            Vector3D vecNorm = intersectionPoint - sphereCenter;
            vecNorm.Normalize();
            vectorObser = origin - intersectionPoint;
            vectorObser.Normalize();

            // Wektor odbicia
            reflectedRay = vecNorm * 2;
            reflectedRay *= vectorObser.DotProduct(vecNorm);
            reflectedRay -= vectorObser;
            reflectedRay.Normalize();

            int red = 0, green = 0, blue = 0;

            // Zalamanie
            if ((material.krcR > 0 || material.krcG > 0 || material.krcB > 0) && depth < maxDepth)
            {
                Vector3D newNorm = new Vector3D(vecNorm);
                Vector3D refractedRey = new Vector3D();
                float n = 0;
                float mult = 1;
                if (!inside)
                {
                    n = 1.0f / material.n;
                }
                else
                {
                    newNorm *= -1;
                    n = material.n / 1.0f;
                }

                float cosI = (newNorm.DotProduct(-direction));
                float cosT2 = 1.0f - n * n * (1.0f - cosI * cosI);

                if (cosT2 > 0)
                {
                    if (cosI > 0)
                        refractedRey = direction * n + newNorm * (n * cosI - (float)Math.Sqrt(cosT2));
//.........这里部分代码省略.........

            public bool set_oriented_gyros_dir(Vector3D tar, Vector3D dir = default(Vector3D))
            {
                tar.Normalize();
                Matrix orientation;
                remote.Orientation.GetMatrix(out orientation);
                MatrixD invMatrix = MatrixD.Invert(orientation);
                Vector3D localTar = Vector3D.Transform(tar, MatrixD.Invert(MatrixD.CreateFromDir(remote.WorldMatrix.Forward, remote.WorldMatrix.Up)) * orientation);
                Vector3D dirp = dir == default(Vector3D) ? (Vector3D)orientation.Forward : dir;


                double poop;
                Vector3D angsin;
                Vector3D ang;
                if (Vector3D.Dot(dirp, localTar) > 0.98)
                {


                    angsin = Vector3D.Cross(Vector3D.CalculatePerpendicularVector(dirp), localTar);

                }
                else
                {
                    angsin = Vector3D.Cross(dirp, localTar);

                }
                poop = angsin.GetDim(2);

                ang = new Vector3D(Math.Asin(angsin.GetDim(0)), Math.Asin(angsin.GetDim(1)), Math.Asin(poop));
                bool complete = false;
                if (ang.Length() < 0.005) { ang = new Vector3D(0, 0, 0); complete = true; }
                for (int i = 0; i < gyros.Count; i++)
                {
                    Matrix gyro_or;
                    gyros[i].Orientation.GetMatrix(out gyro_or);
                    MatrixD invGyroMatrix = MatrixD.Invert(gyro_or);//invMatrix *                                 
                    invGyroMatrix *= (new Matrix(-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1)); //Credit to Pennywise for this matrix                                       
                    Vector3D angle = Vector3D.Transform(ang, invGyroMatrix);
                    gyros[i].SetValueFloat("Pitch", (float)angle.GetDim(0));
                    gyros[i].SetValueFloat("Yaw", (float)angle.GetDim(1));
                    gyros[i].SetValueFloat("Roll", (float)angle.GetDim(2));
                }
                return complete;
            }

        public void RequestJump(string destinationName, Vector3D destination, long userId)
        {

            if (!Vector3.IsZero(MyGravityProviderSystem.CalculateNaturalGravityInPoint(m_grid.WorldMatrix.Translation)))
            {
                var notification = new MyHudNotification(MySpaceTexts.NotificationCannotJumpFromGravity, 1500);
                MyHud.Notifications.Add(notification);
                return;
            }
            if (!Vector3.IsZero(MyGravityProviderSystem.CalculateNaturalGravityInPoint(destination)))
            {
                var notification = new MyHudNotification(MySpaceTexts.NotificationCannotJumpIntoGravity, 1500);
                MyHud.Notifications.Add(notification);
                return;
            }

            if (!IsJumpValid(userId))
            {
                return;
            }

            if (MySession.Static.Settings.WorldSizeKm > 0 && destination.Length() > MySession.Static.Settings.WorldSizeKm * 500)
            {
                var notification = new MyHudNotification(MySpaceTexts.NotificationCannotJumpOutsideWorld, 1500);
                MyHud.Notifications.Add(notification);
                return;
            }

            m_selectedDestination = destination;
            double maxJumpDistance = GetMaxJumpDistance(userId);
            m_jumpDirection = destination - m_grid.WorldMatrix.Translation;
            double jumpDistance = m_jumpDirection.Length();
            double actualDistance = jumpDistance;
            if (jumpDistance > maxJumpDistance)
            {
                double ratio = maxJumpDistance / jumpDistance;
                actualDistance = maxJumpDistance;
                m_jumpDirection *= ratio;
            }

            //By Gregory: Check for obstacle not that fast but happens rarely(on Jump drive enable)
            //TODO: make compatible with GetMaxJumpDistance and refactor to much code checks for actual jump
            var direction = Vector3D.Normalize(destination - m_grid.WorldMatrix.Translation);
            var startPos = m_grid.WorldMatrix.Translation + m_grid.PositionComp.LocalAABB.Extents.Max() * direction;
            var line = new LineD(startPos, destination);


            var intersection = MyEntities.GetIntersectionWithLine(ref line, m_grid, null, ignoreObjectsWithoutPhysics: false);

            Vector3D newDestination = Vector3D.Zero;
            Vector3D newDirection = Vector3D.Zero;
            if (intersection.HasValue)
            {
                MyEntity MyEntity = intersection.Value.Entity as MyEntity;

                var targetPos = MyEntity.WorldMatrix.Translation;
                var obstaclePoint = MyUtils.GetClosestPointOnLine(ref startPos, ref destination, ref targetPos);

                MyPlanet MyEntityPlanet = intersection.Value.Entity as MyPlanet;
                if (MyEntityPlanet != null)
                {
                    var notification = new MyHudNotification(MySpaceTexts.NotificationCannotJumpIntoGravity, 1500);
                    MyHud.Notifications.Add(notification);
                    return;
                }

                //var Radius = MyEntityPlanet != null ? MyEntityPlanet.MaximumRadius : MyEntity.PositionComp.LocalAABB.Extents.Length();
                var Radius = MyEntity.PositionComp.LocalAABB.Extents.Length();

                destination = obstaclePoint - direction * (Radius + m_grid.PositionComp.LocalAABB.HalfExtents.Length());
                m_selectedDestination = destination;
                m_jumpDirection = m_selectedDestination - startPos;
                actualDistance = m_jumpDirection.Length();
            }

            if (actualDistance < MIN_JUMP_DISTANCE)
            {
                MyGuiSandbox.AddScreen(MyGuiSandbox.CreateMessageBox(
                    buttonType: MyMessageBoxButtonsType.OK,
                    messageText: GetWarningText(actualDistance, intersection.HasValue),
                    messageCaption: MyTexts.Get(MyCommonTexts.MessageBoxCaptionWarning)
                    ));
            }
            else
            {
                
                MyGuiSandbox.AddScreen(MyGuiSandbox.CreateMessageBox(
                    buttonType: MyMessageBoxButtonsType.YES_NO,
                    messageText: GetConfimationText(destinationName, jumpDistance, actualDistance, userId, intersection.HasValue),
                    messageCaption: MyTexts.Get(MyCommonTexts.MessageBoxCaptionPleaseConfirm),
                    size: new Vector2(0.839375f, 0.3675f), callback: delegate(MyGuiScreenMessageBox.ResultEnum result)
                    {
                        if (result == MyGuiScreenMessageBox.ResultEnum.YES && IsJumpValid(userId))
                        {
                            RequestJump(m_selectedDestination, userId);
                        }
                        else
                            AbortJump();
                    }
                    ));
//.........这里部分代码省略.........

            internal void Calculate(Vector3D worldPoint)
            {
                Clear();
                var planet = MyGravityProviderSystem.GetStrongestGravityWell(worldPoint);
                if (planet != null)
                {
                    var planetVector = worldPoint - planet.PositionComp.GetPosition();
                    if (planetVector.Length() > planet.GravityLimit)
                    {
                        return;
                    }

                    Planet = planet;
                    PlanetVector = planetVector;
                    if (!Vector3D.IsZero(PlanetVector))
                    {
                        GravityWorld = Vector3D.Normalize(Planet.GetWorldGravity(worldPoint));

                        Vector3 localPoint = (Vector3)(worldPoint - Planet.WorldMatrix.Translation);
                        Vector3D closestPoint = Planet.GetClosestSurfacePointLocal(ref localPoint);
                        Height = Vector3D.Distance(localPoint, closestPoint);

                        Elevation = PlanetVector.Length();
                        PlanetVector *= 1.0 / Elevation;
                    }
                }
            }

        private void UpdateThrust(Vector3D delta, Vector3D perpDelta)
        {
            var thrustSystem = CubeGrid.Components.Get<MyEntityThrustComponent>();
	        if (thrustSystem == null)
		        return;

            thrustSystem.AutoPilotControlThrust = Vector3.Zero;

            m_dbgDeltaH = Vector3.Zero;
            double maxSpeed = m_autopilotSpeedLimit;

            m_dbgDelta = delta;

            Matrix invWorldRot = CubeGrid.PositionComp.WorldMatrixNormalizedInv.GetOrientation();

            Vector3D targetDirection = delta;
            targetDirection.Normalize();

            Vector3D velocity = CubeGrid.Physics.LinearVelocity;

            Vector3 localSpaceTargetDirection = Vector3.Transform(targetDirection, invWorldRot);
            Vector3 localSpaceVelocity = Vector3.Transform(velocity, invWorldRot);

            thrustSystem.AutoPilotControlThrust = Vector3.Zero;

            Vector3 brakeThrust = thrustSystem.GetAutoPilotThrustForDirection(Vector3.Zero);

            if (velocity.Length() > 3.0f && velocity.Dot(ref targetDirection) < 0)
            {
                //Going the wrong way
                return;
            }

            Vector3D perpendicularToTarget1 = Vector3D.CalculatePerpendicularVector(targetDirection);
            Vector3D perpendicularToTarget2 = Vector3D.Cross(targetDirection, perpendicularToTarget1);

            Vector3D velocityToTarget = targetDirection * velocity.Dot(ref targetDirection);
            Vector3D velocity1 = perpendicularToTarget1 * velocity.Dot(ref perpendicularToTarget1);
            Vector3D velocity2 = perpendicularToTarget2 * velocity.Dot(ref perpendicularToTarget2);

            Vector3D velocityToCancel = velocity1 + velocity2;

            double timeToReachTarget = (delta.Length() / velocityToTarget.Length());
            double timeToStop = velocity.Length() * CubeGrid.Physics.Mass / brakeThrust.Length();

            if (m_dockingModeEnabled)
            {
                timeToStop *= 2.5f;
            }

            if ((double.IsInfinity(timeToReachTarget) || double.IsNaN(timeToStop) || timeToReachTarget > timeToStop) && velocity.LengthSquared() < (maxSpeed * maxSpeed))
            {
                thrustSystem.AutoPilotControlThrust = Vector3D.Transform(delta, invWorldRot) - Vector3D.Transform(velocityToCancel, invWorldRot);
                thrustSystem.AutoPilotControlThrust.Normalize();
            }
        }

        private Vector3D AvoidCollisions(Vector3D delta)
        {
            if (m_collisionCtr <= 0)
            {
                m_collisionCtr = 0;
            }
            else
            {
                m_collisionCtr--;
                return m_oldCollisionDelta;
            }

            bool drawDebug = MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_DRONES;

            Vector3D originalDelta = delta;

            Vector3D origin = this.CubeGrid.Physics.CenterOfMassWorld;
            double shipRadius = this.CubeGrid.PositionComp.WorldVolume.Radius * 1.3f;

            Vector3D linVel = this.CubeGrid.Physics.LinearVelocity;

            double vel = linVel.Length();
            double detectionRadius = this.CubeGrid.PositionComp.WorldVolume.Radius * 10.0f + (vel * vel) * 0.05;
            BoundingSphereD sphere = new BoundingSphereD(origin, detectionRadius);

            if (drawDebug)
            {
                MyRenderProxy.DebugDrawSphere(sphere.Center, (float)shipRadius, Color.HotPink, 1.0f, false);
                MyRenderProxy.DebugDrawSphere(sphere.Center + linVel, 1.0f, Color.HotPink, 1.0f, false);
                MyRenderProxy.DebugDrawSphere(sphere.Center, (float)detectionRadius, Color.White, 1.0f, false);
            }

            Vector3D steeringVector = Vector3D.Zero;
            Vector3D avoidanceVector = Vector3D.Zero;
            int n = 0;

            double maxAvCoeff = 0.0f;

            var entities = MyEntities.GetTopMostEntitiesInSphere(ref sphere);
            for (int i = 0; i < entities.Count; ++i)
            {
                var entity = entities[i];

                if (!(entity is MyCubeGrid) && !(entity is MyVoxelMap)) continue;
                if (entity == this.Parent) continue;

                var otherSphere = entity.PositionComp.WorldVolume;
                otherSphere.Radius += shipRadius;
                Vector3D offset = otherSphere.Center - sphere.Center;

                if (Vector3D.Dot(delta, this.CubeGrid.Physics.LinearVelocity) < 0) continue;

                // Collision avoidance
                double dist = offset.Length();
                BoundingSphereD forbiddenSphere = new BoundingSphereD(otherSphere.Center + linVel, otherSphere.Radius + vel);
                Vector3D testPoint = sphere.Center + linVel * 2.0f;
                if (forbiddenSphere.Contains(testPoint) == ContainmentType.Contains)
                {
                    m_autopilotSpeedLimit = 2.0f;
                    if (drawDebug)
                    {
                        MyRenderProxy.DebugDrawSphere(forbiddenSphere.Center, (float)forbiddenSphere.Radius, Color.Red, 1.0f, false);
                    }
                }
                else
                {
                    if (Vector3D.Dot(offset, linVel) < 0)
                    {
                        if (drawDebug)
                        {
                            MyRenderProxy.DebugDrawSphere(forbiddenSphere.Center, (float)forbiddenSphere.Radius, Color.Red, 1.0f, false);
                        }
                    }
                    else if (drawDebug)
                    {
                        MyRenderProxy.DebugDrawSphere(forbiddenSphere.Center, (float)forbiddenSphere.Radius, Color.DarkOrange, 1.0f, false);
                    }
                }

                // 0.693 is log(2), because we want svLength(otherSphere.radius) -> 1 and svLength(0) -> 2
                double exponent = -0.693 * dist / (otherSphere.Radius + this.CubeGrid.PositionComp.WorldVolume.Radius + vel);
                double svLength = 2 * Math.Exp(exponent);
                double avCoeff = Math.Min(1.0f, Math.Max(0.0f, -(forbiddenSphere.Center - sphere.Center).Length() / forbiddenSphere.Radius + 2));
                maxAvCoeff = Math.Max(maxAvCoeff, avCoeff);

                Vector3D normOffset = offset / dist;
                steeringVector -= normOffset * svLength;
                avoidanceVector -= normOffset * avCoeff;
                n++;
            }
            entities.Clear();

            /*if (minTmin < vel)
            {
                delta = origin + minTmin * vel;
            }*/

            if (n > 0)
            {
                double l = delta.Length();
//.........这里部分代码省略.........

 /// <summary>
 /// Returns the acute angle between two vectors.
 /// </summary>
 public static double AngleBetween(this Vector3D first, Vector3D second)
 {
     return Math.Acos(first.Dot(second) / (first.Length() * second.Length()));
 }

 public bool ThrustTowardsVector(Vector3D dir)
 {
     if (first || dir != lastDir)
     {
         startingVelocity = currentVelocity;
         lastDir = dir;
     }
     Vector3D vel = currentVelocity;
     Vector3D targVel = startingVelocity + dir;
     Vector3D remVel = targVel - vel;
     double mod = remVel.Length() / dir.Length();
     Vector3D localremVel = Vector3D.Transform(remVel, MatrixD.Invert(MatrixD.CreateFromDir(remote.WorldMatrix.Forward, remote.WorldMatrix.Up)));
     localremVel.Normalize();
     if (remVel.Length() < 0.1)
     {
         first = true;
         thrust(new Vector3D(0, 0, 0));
         return true;
     }
     // Me.Echo(mod.ToString() + remVel.Length().ToString());                       
     thrust(localremVel * mod);
     return false;
 }

        public void CartToKep(Vector3D pos, Vector3D vel)//This doesn't work, feel free to try and fix it.
        {
            this.pos = pos;
            this.vel = vel;
            en = vel.LengthSquared() / 2 - planet.u / pos.Length();
            a = Math.Abs(planet.u / (2 * en));
            h = Vector3D.Cross(pos, vel);
            E = Vector3D.Cross(vel, h) / planet.u - (pos / pos.Length());
            Console.WriteLine(E);
            e = E.Length();
            I = Math.Acos(h.GetDim(2) / h.Length());
            N = new Vector3D(-h.GetDim(1), h.GetDim(0), 0);
            if (N.GetDim(1) >= 0)
            {
                q = Math.Acos(N.GetDim(0) / N.Length());
                q = double.IsNaN(q) ? 0 : q;
            }
            else
            {
                q = 2 * Math.PI - Math.Acos(N.GetDim(0) / N.Length());
                q = double.IsNaN(q) ? 2*Math.PI : q;
            }
            
            n = Math.Sqrt(planet.u / Math.Pow(a, 3));
            if (q != 0)
            {
                w = E.GetDim(2) >= 0 ? Math.Acos(Vector3D.Dot(N, E) / (N.Length() * E.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(N, E) / (N.Length() * E.Length()));

            }
            else
            {
                double W=Math.Atan2(E.GetDim(1),E.GetDim(0));
                w = h.GetDim(2) < 0 ? 2*Math.PI-W:W;


            }

            if (double.IsNaN(w)) w = 0;
            if (w < 0) w += Math.PI * 2;
            epoch = DateTime.Now;
            if (E.Length() != 0)
            {
                v = Vector3D.Dot(pos, vel) >= 0 ? Math.Acos(Vector3D.Dot(E, pos) / (E.Length() * pos.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(E, pos) / (E.Length() * pos.Length()));
            }
            else
            {
                if (I != 0)
                {
                    v = Vector3D.Dot(N, vel) <= 0 ? Math.Acos(Vector3D.Dot(N, pos) / (N.Length() * pos.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(N, pos) / (N.Length() * pos.Length()));
                    Console.WriteLine(N);
                }
                else
                {
                    v = vel.GetDim(0) <= 0 ? Math.Acos(pos.GetDim(0) / (pos.Length())) : Math.PI * 2 - Math.Acos(pos.GetDim(0) / (pos.Length()));
                }
            }
            if (double.IsNaN(v)) v = 0;
            lastTa = v;
            double sinE = sin(a) * Math.Sqrt(1 - Math.Pow(e, 2)) / (1 + e * cos(a));
            double cosE = (e + cos(a)) / (1 + e * cos(a));
            double Ec = Math.Atan2(sinE, cosE);
            EC = 2 * Math.Atan(Math.Tan(v / 2) / Math.Sqrt(1 + e / 1 - e));
            EC = EC < 0 ? EC + Math.PI * 2 : EC;
            if (double.IsNaN(EC)) EC = 0;
            M = EC - e * Math.Sin(EC);
            lastM = M;
            Period = Math.PI * 2 * Math.Sqrt(Math.Pow(a,3)/planet.u);
        }