C# Vector3D.LengthSquared方法代码示例

        protected override void CalculatePositionDifference(ulong clientId, out bool positionValid, out bool correctServer, out Vector3D delta)
        {
            positionValid = true;
            correctServer = false;

            delta = Vector3D.Zero;

            ClientData clientData = m_additionalServerClientData[clientId];
            MatrixD worldMatrix = Entity.PositionComp.WorldMatrix;
     
            if (clientData.HasSupport)
            {
                if (m_support != null)
                {
                    Vector3D characterDelta = m_serverClientData[clientId].Transform.Position - m_character.PositionComp.GetPosition();
                    Vector3D supportDelta = clientData.SupportPosition - m_support.PositionComp.GetPosition();
                    delta = characterDelta - supportDelta;
                }
                else
                {
                    positionValid = false;
                    delta = m_serverClientData[clientId].Transform.Position - m_character.PositionComp.GetPosition();
                }
            }
            else
            {
                
                delta = m_serverClientData[clientId].Transform.Position - worldMatrix.Translation;
            }


            if (delta.LengthSquared() > 0.05 * 0.05)
            {
                if (m_commandsApplied.ContainsKey(clientId) == false || m_commandsApplied[clientId] == false)
                {
                    positionValid = false;
                }
                else
                {
                    correctServer = true;
                }
            }

            m_commandsApplied[clientId] = false;

        }

        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);
//.........这里部分代码省略.........

 public bool AddSupportPoint(ref Vector3D newPoint)
 {
     int index1 = (GjkD.BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[index1] = newPoint;
     this.yLengthSq[index1] = newPoint.LengthSquared();
     int num = GjkD.BitsToIndices[this.simplexBits];
     while (num != 0)
     {
         int index2 = (num & 7) - 1;
         Vector3D Vector3D = this.y[index2] - newPoint;
         this.edges[index2][index1] = Vector3D;
         this.edges[index1][index2] = -Vector3D;
         this.edgeLengthSq[index1][index2] = this.edgeLengthSq[index2][index1] = Vector3D.LengthSquared();
         num >>= 3;
     }
     this.UpdateDeterminant(index1);
     return this.UpdateSimplex(index1);
 }

        protected override void CalculatePositionDifference(ulong clientId, out bool isValid, out bool correctServer, out Vector3D delta)
        {
            delta = Vector3D.Zero;
            isValid = true;
            correctServer = false;

            ClientData clientData = m_additionalServerClientData[clientId];
            MatrixD worldMatrix = Entity.PositionComp.WorldMatrix;

            float maxCharacterSpeedRelativeToShip = Math.Max(m_character.Definition.MaxSprintSpeed, Math.Max(m_character.Definition.MaxRunSpeed, m_character.Definition.MaxBackrunSpeed));
            float maxSpeed= 1.04f * (MyGridPhysics.ShipMaxLinearVelocity() + maxCharacterSpeedRelativeToShip);

            if (m_haveJetpack == false)
            {
                maxSpeed = 1.04f * maxCharacterSpeedRelativeToShip;
            }

            float maxMoveDistance = (float)((maxSpeed*maxSpeed) / (60f * 60f));
            if (clientData.HasSupport)
            {
                MyEntity support = MySupportHelper.FindSupportForCharacterAABB(m_character);
                if (support != null)
                {
                    Vector3D characterDelta = m_serverClientData[clientId].Transform.Position - m_character.PositionComp.GetPosition();
                    Vector3D supportDelta = clientData.SupportPosition - support.PositionComp.GetPosition();
                    delta = characterDelta - supportDelta;
                }
                else
                {
                    isValid = false;
                }
            }
            else
            {
                correctServer = true;
                delta = m_serverClientData[clientId].Transform.Position - worldMatrix.Translation;
                return;
            }

            double deltaL = delta.LengthSquared();
            if (deltaL > 1.3f*(maxMoveDistance + 0.0001))
            {
                isValid = true;
                correctServer = true;
              //  delta = Vector3D.Zero;
            }
            else if (deltaL > 0.05*0.05)
            {
                if (m_commandsApplied.ContainsKey(clientId) == false || m_commandsApplied[clientId] == false)
                {
                    isValid = false;
                    delta = Vector3D.Zero;
                }
                else
                {
                    correctServer = true;
                }
            }
            m_commandsApplied[clientId] = false;
        }

 private void CalculateFaceNormals()
 {
     _editMesh.Faces.ParallelDo(
         face =>
         {
             Vector3D faceNormal = new Vector3D();
             if (face.Vertices.Count == 3)
             {
                 Vector3D v0 = face.Vertices[0].Position;
                 Vector3D v1 = face.Vertices[1].Position;
                 Vector3D v2 = face.Vertices[2].Position;
                 faceNormal = Vector3D.Cross(v1 - v0, v2 - v1);
             }
             if (faceNormal.LengthSquared() > 0.0f)
             {
                 faceNormal.Normalize();
             }
             foreach (var vert in face.Vertices)
             {
                 vert.Normal = faceNormal;
             }
             face.Normal = faceNormal;
         }, ParallelizationChunkSize, _threadPool);
 }

        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);
        }

        public void TestLengthSquared()
        {
            float x = -5, y = 25f, z = 7;

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