C# Vector3.Length方法代码示例

        protected internal override IEnumerable<Star> Generate(Random random)
        {
            var density = Math.Max(0, random.NormallyDistributedSingle(_densityDeviation, _densityMean));
            var countMax = Math.Max(0, (int)(_size * _size * _size * density));
            if (countMax <= 0)
                yield break;

            var count = random.Next(countMax);

            for (int i = 0; i < count; i++)
            {
                var pos = new Vector3(
                    random.NormallyDistributedSingle(_deviationX * _size, 0),
                    random.NormallyDistributedSingle(_deviationY * _size, 0),
                    random.NormallyDistributedSingle(_deviationZ * _size, 0)
                );
                var d = pos.Length() / _size;
                var m = d * 2000 + (1 - d) * 15000;
                var t = random.NormallyDistributedSingle(4000, m, 1000, 40000);

                yield return new Star(
                    pos,
                    StarName.Generate(random),
                    t
                );
            }
        }

 public static float Angle(Vector3 a, Vector3 b)
 {
     float dotf = Vector3.Dot(a,b);
     dotf = dotf/(a.Length()*b.Length());
     float acos = (float)Math.Acos(dotf);
     return acos*180.0f/3.1415f;
 }

 /// <summary>
 /// Returns a Quaternion representing a rotation.
 /// </summary>
 /// <param name="axis">The axis to rotate around.</param>
 /// <param name="angle">The angle to rotate by.</param>
 /// <returns>A Quaternion representing the rotation.</returns>
 public static Quaternion Rotation(Vector3 axis, double angle)
 {
     double real = Math.Cos(angle / 2.0);
     Vector3 imaginary;
     //normalize first
     imaginary = axis.Multiply(1.0 / axis.Length());
     imaginary = imaginary.Multiply(Math.Sin(angle / 2.0));
     return new Quaternion(real, imaginary);
 }

 public Rectangle(Vector3 p, Vector3 _a, Vector3 _b, Vector3 _normal, string name)
     : base(name)
 {
     p0 = p;
     a = _a;
     b = _b;
     a_len_squared = a.LengthSquared();
     b_len_squared = b.LengthSquared();
     area = a.Length() * b.Length();
     inv_area = 1.0f / area;
     normal = _normal;
     Shadows = false;
 }

        // in local space!
        public override void GetClosestPoint(Vector3 point, ref Vector3 closestPoint, ref Vector3 normal, ref bool penetration, ref uint customData)
        {
            float dist = point.Length();
            penetration = (dist < m_Radius);

            //Handle case when point is in sphere origin
            if (dist == 0)
            {
                normal.X = 1.0f;
                normal.Y = 0.0f;
                normal.Z = 0.0f;
                closestPoint.X = m_Radius;
                closestPoint.Y = 0.0f;
                closestPoint.Z = 0.0f;
            }
            else
            {
                float d = dist;
                normal = point/d;
                closestPoint = point*(m_Radius / d);
            }

            customData = 0;
        }

        public override float GetDistanceToCenter(
                Vector3 particlePosition, Vector3 particleVelocity,
                out Vector3 alongAxis, out Vector3 aroundAxis, out Vector3 awayAxis)
        {
            // Along - following the main axis
            alongAxis = mainAxis;

            // Toward - tawards the main axis
            awayAxis = particlePosition - fieldPosition;
            awayAxis.Normalize();

            // Around - around the main axis, following the right hand rule
            aroundAxis = Vector3.Cross(alongAxis, awayAxis);

            particlePosition -= fieldPosition;
            inverseRotation.Rotate(ref particlePosition);
            particlePosition /= fieldSize;

            // Start of code for Sphere
            var maxDist = particlePosition.Length() / radius;
            // End of code for Sphere

            return maxDist;
        }

        public void Vector3LengthTest1()
        {
            Vector3 target = new Vector3();

            float expected = 0.0f;
            float actual = target.Length();
            Assert.IsTrue(MathHelper.Equal(expected, actual), "Vector3.Length did not return the expected value.");
        }

        public void Vector3LengthTest()
        {
            Vector2 a = new Vector2(1.0f, 2.0f);

            float z = 3.0f;

            Vector3 target = new Vector3(a, z);

            float expected = (float)System.Math.Sqrt(14.0f);
            float actual;

            actual = target.Length();
            Assert.IsTrue(MathHelper.Equal(expected, actual), "Vector3.Length did not return the expected value.");
        }

        public override bool IsPointInside(Vector3 particlePosition, out Vector3 surfacePoint, out Vector3 surfaceNormal)
        {
            particlePosition -= fieldPosition;
            inverseRotation.Rotate(ref particlePosition);
            particlePosition /= fieldSize;

            var maxDist = particlePosition.Length() / radius;
            if (maxDist <= MathUtil.ZeroTolerance)
            {
                surfacePoint = fieldPosition;
                surfaceNormal = new Vector3(0, 1, 0);
                return true;
            }

            surfaceNormal = particlePosition / maxDist;

            surfacePoint = surfaceNormal;
            surfacePoint *= fieldSize;
            fieldRotation.Rotate(ref surfacePoint);
            surfacePoint += fieldPosition;

            surfaceNormal /= fieldSize;
            fieldRotation.Rotate(ref surfaceNormal);
            surfaceNormal.Normalize();

            return (maxDist <= 1);
        }

    public Vector3 calculateDragForce(polygon poly, Vector3 linearVelocity, Matrix3 oldRotation, Vector3 angularVelocity, Vector3 objectPosition)
    {
        Normal = Vector3.Cross(poly.vertices[0] - poly.vertices[1], poly.vertices[0] - poly.vertices[2]);

        //if (Normal.Y > 0) Normal = -1.0f * Normal;

        Velocity = new Vector3(0.0f, 0.0f, 0.0f);
        if (usingLinearDrag)
        {
            Velocity -= linearVelocity;
        }
        if (usingAngularDrag)
        {
            Velocity -= Vector3.Cross(angularVelocity, oldRotation * poly.centerOfMass);
        }
        if (usingWind)
        {
            Velocity -= windSim.returnWind(oldRotation * poly.centerOfMass + objectPosition);
        }
        //Velocity = -linearVelocity - Vector3.Cross(angularVelocity, oldRotation * poly.centerOfMass) -windSim.returnWind(oldRotation * poly.centerOfMass + objectPosition);

        //Console.WriteLine("Centroid: " + poly.centerOfMass.Length());
        areaPercent = Math.Abs(Vector3.Dot(oldRotation * Normal, Velocity) / (Normal.Length() * Velocity.Length()));
        //Console.WriteLine("poly Area: " + poly.area);
        return (p * poly.area * areaPercent * 0.5f * Cd * Vector3.Multiply(Velocity, abs(Velocity)));
    }

 /// <summary>
 /// Calculates the angle, measured in radians, between two vectors.
 /// </summary>
 public static float AngleBetween(Vector3 value1, Vector3 value2)
 {
     float a = Vector3.Dot(value1, value2) / (value1.Length() * value2.Length());
     return (float)Math.Acos((double)MathHelper.Clamp(a, -1F, 1F));
 }

        public override void OnTick()
        {
            base.OnTick();

            if (KnownPosition.Y <= 0
                || (Velocity.Length() <= 0 && DespawnOnImpact)
                || (Velocity.Length() <= 0 && !DespawnOnImpact && Ttl <= 0))
            {
                DespawnEntity();
                return;
            }

            Ttl--;

            if (KnownPosition.Y <= 0 || Velocity.Length() <= 0) return;

            Entity entityCollided = CheckEntityCollide(KnownPosition.ToVector3(), Velocity);

            bool collided = false;
            if (entityCollided != null)
            {
                double speed = Math.Sqrt(Velocity.X*Velocity.X + Velocity.Y*Velocity.Y + Velocity.Z*Velocity.Z);
                double damage = Math.Ceiling(speed*Damage);
                if (IsCritical)
                {
                    damage += Level.Random.Next((int) (damage/2 + 2));

                    McpeAnimate animate = McpeAnimate.CreateObject();
                    animate.entityId = entityCollided.EntityId;
                    animate.actionId = 4;
                    Level.RelayBroadcast(animate);
                }

                Player player = entityCollided as Player;

                if (player != null)
                {
                    damage = player.CalculatePlayerDamage(player, damage);
                }

                entityCollided.HealthManager.TakeHit(this, (int) damage, DamageCause.Projectile);
                entityCollided.HealthManager.LastDamageSource = Shooter;
                collided = true;
            }
            else
            {
                var velocity2 = Velocity;
                velocity2 *= (float) (1.0d - Drag);
                velocity2 -= new Vector3(0, (float) Gravity, 0);
                double distance = velocity2.Length();
                velocity2 = Vector3.Normalize(velocity2)/2;

                for (int i = 0; i < Math.Ceiling(distance)*2; i++)
                {
                    PlayerLocation nextPos = (PlayerLocation) KnownPosition.Clone();
                    nextPos.X += (float) velocity2.X*i;
                    nextPos.Y += (float) velocity2.Y*i;
                    nextPos.Z += (float) velocity2.Z*i;

                    BlockCoordinates coord = new BlockCoordinates(nextPos);
                    Block block = Level.GetBlock(coord);
                    collided = block.Id != 0 && (block.GetBoundingBox()).Contains(nextPos.ToVector3());
                    if (collided)
                    {
                        SetIntersectLocation(block.GetBoundingBox(), KnownPosition);
                        break;
                    }
                }
            }

            if (collided)
            {
                Velocity = Vector3.Zero;
            }
            else
            {
                KnownPosition.X += (float) Velocity.X;
                KnownPosition.Y += (float) Velocity.Y;
                KnownPosition.Z += (float) Velocity.Z;

                Velocity *= (float) (1.0 - Drag);
                Velocity -= new Vector3(0, (float) Gravity, 0);

                KnownPosition.Yaw = (float) Velocity.GetYaw();
                KnownPosition.Pitch = (float) Velocity.GetPitch();
            }

            // For debugging of flight-path
            if (BroadcastMovement) BroadcastMoveAndMotion();
        }

	    //
	    // solve unilateral raint (equality, direct method)
	    //
        public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref Matrix world2A,
                            ref Matrix world2B,
                            ref Vector3 invInertiaADiag,
                            float invMassA,
                            ref Vector3 linvelA, ref Vector3 angvelA,
                            ref Vector3 rel_posA1,
                            ref Vector3 invInertiaBDiag,
                            float invMassB,
                            ref Vector3 linvelB, ref Vector3 angvelB,
                            ref Vector3 rel_posA2,
                            float depthA, ref Vector3 normalA,
                            ref Vector3 rel_posB1, ref Vector3 rel_posB2,
                            float depthB, ref Vector3 normalB,
                            ref float imp0, ref float imp1)
        {
            //(void)linvelA;
            //(void)linvelB;
            //(void)angvelB;
            //(void)angvelA;

	        imp0 = 0f;
	        imp1 = 0f;

	        float len = System.Math.Abs(normalA.Length()) - 1f;
	        if (System.Math.Abs(len) >= MathUtil.SIMD_EPSILON)
		        return;

	        Debug.Assert(len < MathUtil.SIMD_EPSILON);

	        //this jacobian entry could be re-used for all iterations
	        JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA,
		        ref invInertiaBDiag,invMassB);
	        JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA,
		        ref invInertiaBDiag,invMassB);
        	
	        // float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
	        // float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

	        float vel0 = Vector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
	        float vel1 = Vector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1)));

        //	float penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
	        float massTerm = 1f / (invMassA + invMassB);


	        // calculate rhs (or error) terms
	        float dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
	        float dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;


	        // dC/dv * dv = -C
        	
	        // jacobian * impulse = -error
	        //

	        //impulse = jacobianInverse * -error

	        // inverting 2x2 symmetric system (offdiagonal are equal!)
	        // 


	        float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB);
	        float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag );
        	
	        //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
	        //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;

	        imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
	        imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet;

	        //[a b]								  [d -c]
	        //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)

	        //[jA nD] * [imp0] = [dv0]
	        //[nD jB]   [imp1]   [dv1]
        }

        /// <summary>
        /// Returns a position randomly distributed inside a sphere of unit radius
        /// centered at the origin.  Orientation will be random and length will range
        /// between 0 and 1
        /// </summary>
        /// <returns></returns>
        public static Vector3 RandomVectorInUnitRadiusSphere()
        {
            Vector3 v = new Vector3();
            do
            {
                v.X = (RandomHelpers.Random() * 2) - 1;
                v.Y = (RandomHelpers.Random() * 2) - 1;
                v.Z = (RandomHelpers.Random() * 2) - 1;
            }
            while (v.Length() >= 1);

            return v;
        }

//.........这里部分代码省略.........
					{ // fixed. We'll need to add one more row to constraint
						if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z))))
						{
							m_solveSwingLimit = true;
							m_swingAxis = -Vector3.Cross(ivB, ivA);
						}
					}
					else
					{
						if (m_swingSpan1 < m_fixThresh)
						{ // hinge around Y axis
							if (!(MathUtil.FuzzyZero(y)))
							{
								m_solveSwingLimit = true;
								if (m_swingSpan2 >= m_fixThresh)
								{
									y = 0;
									float span2 = (float)System.Math.Atan2(z, x);
									if (span2 > m_swingSpan2)
									{
										x = (float)System.Math.Cos(m_swingSpan2);
										z = (float)System.Math.Sin(m_swingSpan2);
									}
									else if (span2 < -m_swingSpan2)
									{
										x = (float)System.Math.Cos(m_swingSpan2);
										z = -(float)System.Math.Sin(m_swingSpan2);
									}
								}
							}
						}
						else
						{ // hinge around Z axis
							if (!MathUtil.FuzzyZero(z))
							{
								m_solveSwingLimit = true;
								if (m_swingSpan1 >= m_fixThresh)
								{
									z = 0f;
									float span1 = (float)System.Math.Atan2(y, x);
									if (span1 > m_swingSpan1)
									{
										x = (float)System.Math.Cos(m_swingSpan1);
										y = (float)System.Math.Sin(m_swingSpan1);
									}
									else if (span1 < -m_swingSpan1)
									{
										x = (float)System.Math.Cos(m_swingSpan1);
										y = -(float)System.Math.Sin(m_swingSpan1);
									}
								}
							}
						}
						target.X = x * ivA.X + y * jvA.X + z * kvA.X;
						target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y;
						target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z;
						target.Normalize();
						m_swingAxis = -Vector3.Cross(ivB, target);
						m_swingCorrection = m_swingAxis.Length();
						m_swingAxis.Normalize();
					}
				}

				if (m_twistSpan >= 0f)
				{
					Vector3 twistAxis = Vector3.Zero;
					ComputeTwistLimitInfo(ref qABTwist, ref m_twistAngle, ref twistAxis);

					if (m_twistAngle > m_twistSpan * m_limitSoftness)
					{
						m_solveTwistLimit = true;

						m_twistLimitRatio = 1f;
						if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
						{
							m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
												(m_twistSpan - m_twistSpan * m_limitSoftness);
						}

						// twist correction tries to get back to soft limit
						m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);

						m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis);

						m_kTwist = 1f /
							(ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) +
							 ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB));
					}

					if (m_solveSwingLimit)
					{
						m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis);
					}
				}
				else
				{
					m_twistAngle = 0f;
				}
			}
		}