C# Vector3.LengthSquared方法代码示例

        ///bilateral constraint between two dynamic objects
        ///positive distance = separation, negative distance = penetration
        public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1,
                              RigidBody body2, ref Vector3 pos2,
                              float distance, ref Vector3 normal, ref float impulse, float timeStep)
        {
	        float normalLenSqr = normal.LengthSquared();
	        Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f);
	        if (normalLenSqr > 1.1f)
	        {
		        impulse = 0f;
		        return;
	        }
	        Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); 
	        Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
	        //this jacobian entry could be re-used for all iterations
        	
	        Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
	        Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
	        Vector3 vel = vel1 - vel2;

            Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform());
            Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform());


            JacobianEntry jac = new JacobianEntry(m1,m2,rel_pos1,rel_pos2,normal,
                body1.GetInvInertiaDiagLocal(),body1.GetInvMass(),
		        body2.GetInvInertiaDiagLocal(),body2.GetInvMass());

	        float jacDiagAB = jac.GetDiagonal();
	        float jacDiagABInv = 1f / jacDiagAB;

            
            float rel_vel = jac.GetRelativeVelocity(
                body1.GetLinearVelocity(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1),
                body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2));
	        float a = jacDiagABInv;

            rel_vel = Vector3.Dot(normal,vel);
        	
	        //todo: move this into proper structure
	        float contactDamping = 0.2f;

            if(ONLY_USE_LINEAR_MASS)
            {
	            float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
	            impulse = - contactDamping * rel_vel * massTerm;
            }
            else	
            {
	            float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
	            impulse = velocityImpulse;
            }
        }

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

 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     int index = (BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[index] = newPoint;
     this.yLengthSq[index] = newPoint.LengthSquared();
     for (int i = BitsToIndices[this.simplexBits]; i != 0; i = i >> 3)
     {
         int num2 = (i & 7) - 1;
         Vector3 vector = this.y[num2] - newPoint;
         this.edges[num2][index] = vector;
         this.edges[index][num2] = Vector3.One - vector;
         this.edgeLengthSq[index][num2] = this.edgeLengthSq[num2][index] = vector.LengthSquared();
     }
     this.UpdateDeterminant(index);
     return this.UpdateSimplex(index);
 }

        static TestCollidableBEPU[] GetRandomLeavesBEPU(int leafCount, BoundingBox bounds, Vector3 minimumSize, Vector3 maximumSize, float sizePower, VelocityDescription velocityDescription)
        {
            var leaves = new TestCollidableBEPU[leafCount];
            Random random = new Random(5);

            var range = bounds.Max - bounds.Min;
            var sizeRange = maximumSize - minimumSize;
            for (int i = 0; i < leafCount; ++i)
            {
                var halfLeafSize = 0.5f * (minimumSize + new Vector3((float)Math.Pow(random.NextDouble(), sizePower), (float)Math.Pow(random.NextDouble(), sizePower), (float)Math.Pow(random.NextDouble(), sizePower)) * sizeRange);
                var position = bounds.Min + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * range;

                leaves[i] = new TestCollidableBEPU();
                leaves[i].Position = new BEPUutilities.Vector3(position.X, position.Y, position.Z);
                leaves[i].HalfSize = new BEPUutilities.Vector3(halfLeafSize.X, halfLeafSize.Y, halfLeafSize.Z);
                leaves[i].UpdateBoundingBox();
            }

            for (int i = 0; i < leaves.Length * velocityDescription.PortionOfMovingLeaves; ++i)
            {
                var speed = (float)(velocityDescription.MinVelocity + (velocityDescription.MaxVelocity - velocityDescription.MinVelocity) * Math.Pow(random.NextDouble(), velocityDescription.VelocityDistributionPower));
                var direction = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * 2 - Vector3.One;
                var lengthSquared = direction.LengthSquared();
                if (lengthSquared < 1e-9f)
                {
                    direction = new Vector3(0, 1, 0);
                }
                else
                {
                    direction /= (float)Math.Sqrt(lengthSquared);
                }
                var velocity = speed * direction;
                leaves[i].Velocity = new BEPUutilities.Vector3(velocity.X, velocity.Y, velocity.Z);
            }
            return leaves;

        }

 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     int num = (Gjk.BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[num] = newPoint;
     this.yLengthSq[num] = newPoint.LengthSquared();
     for (int num2 = Gjk.BitsToIndices[this.simplexBits]; num2 != 0; num2 >>= 3)
     {
         int num3 = (num2 & 7) - 1;
         Vector3 vector = this.y[num3] - newPoint;
         this.edges[num3][num] = vector;
         this.edges[num][num3] = -vector;
         this.edgeLengthSq[num][num3] = (this.edgeLengthSq[num3][num] = vector.LengthSquared());
     }
     this.UpdateDeterminant(num);
     return this.UpdateSimplex(num);
 }

//.........这里部分代码省略.........
				       	        NewFace(simplex.c[0],simplex.c[2],simplex.c[3],true)};
				if(m_hull.Count==4)
				{
					sFace best=FindBest();
					sFace outer = best;
					uint pass=0;
					uint iterations=0;
					Bind(tetra[0],0,tetra[1],0);
					Bind(tetra[0],1,tetra[2],0);
					Bind(tetra[0],2,tetra[3],0);
					Bind(tetra[1],1,tetra[3],2);
					Bind(tetra[1],2,tetra[2],1);
					Bind(tetra[2],2,tetra[3],1);
					m_status=eStatus.Valid;
					for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
					{
						if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
						{
							sHorizon horizon = new sHorizon() ;
							sSV	w = m_sv_store[m_nextsv++];
							bool valid = true;					
							best.pass =	(uint)(++pass);
							gjk.GetSupport(ref best.n,ref w);
							float wdist=Vector3.Dot(best.n,w.w)-best.d;
							if (wdist > GjkEpaSolver2.EPA_ACCURACY)
							{
								for(int j=0;(j<3)&&valid;++j)
								{
									valid&=Expand(	pass,w,
									              	best.f[j],best.e[j],
									              	horizon);
								}
								if(valid&&(horizon.nf>=3))
								{
									Bind(horizon.cf,1,horizon.ff,2);
									Remove(m_hull,best);
									Append(m_stock,best);
									best=FindBest();
									if (best.p >= outer.p)
									{
										outer = best;
									}
								} 
								else 
								{ 
									m_status=eStatus.InvalidHull;
									break; 
								}
							} 
							else 
							{ 
								m_status=eStatus.AccuraryReached;
								break; 
							}
						} 
						else 
						{ 
							m_status=eStatus.OutOfVertices;
							break; 
						}
					}
					Vector3	projection=outer.n*outer.d;
					m_normal	=	outer.n;
					m_depth		=	outer.d;
					m_result.rank	=	3;
					m_result.c[0]	=	outer.c[0];
					m_result.c[1]	=	outer.c[1];
					m_result.c[2]	=	outer.c[2];
					m_result.p[0]	=	Vector3.Cross(	outer.c[1].w-projection,
					             	 	              	outer.c[2].w-projection).Length();
					m_result.p[1] = Vector3.Cross(outer.c[2].w - projection,
					                              outer.c[0].w-projection).Length();
					m_result.p[2] = Vector3.Cross(outer.c[0].w - projection,
					                              outer.c[1].w-projection).Length();
					float sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
					m_result.p[0]	/=	sum;
					m_result.p[1]	/=	sum;
					m_result.p[2]	/=	sum;
					return(m_status);
				}
			}
			/* Fallback		*/ 
			m_status	=	eStatus.FallBack;
			m_normal	=	-guess;
			float nl=m_normal.LengthSquared();
			if(nl>0)
			{
				m_normal.Normalize();
			}
			else
			{
				m_normal = new Vector3(1,0,0);
			}

			m_depth	=	0;
			m_result.rank=1;
			m_result.c[0]=simplex.c[0];
			m_result.p[0]=1;	
			return(m_status);
		}

            public Triangle(Vector3 a, Vector3 b, Vector3 c)
            {
                A = a;
                Edge0 = b - a;
                Edge1 = c - a;

                PointOnPath = Vector3.Zero;
                Tangent = Vector3.Zero;

                // ReSharper disable once ImpureMethodCallOnReadonlyValueField
                var edge0LengthSquared = Edge0.LengthSquared();

                var edge0DotEdge1 = Vector3.Dot(Edge0, Edge1);
                var edge1LengthSquared = Vector3.Dot(Edge1, Edge1);

                Determinant = edge0LengthSquared * edge1LengthSquared - edge0DotEdge1 * edge0DotEdge1;
            }

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

            float z = 3.0f;

            Vector3 target = new Vector3(a, z);

            float expected = 14.0f;
            float actual;

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

        ///<summary>
        /// Adds a new point to the simplex.
        ///</summary>
        ///<param name="shapeA">First shape in the pair.</param>
        ///<param name="shapeB">Second shape in the pair.</param>
        ///<param name="iterationCount">Current iteration count.</param>
        ///<param name="closestPoint">Current point on simplex closest to origin.</param>
        ///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
        public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
        {
            Vector3 negativeDirection;
            Vector3.Negate(ref closestPoint, out negativeDirection);
            Vector3 sa, sb;
            shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
            shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
            Vector3 S;
            Vector3.Subtract(ref sa, ref sb, out S);
            //If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
            float dotS;
            Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
            float distanceToClosest = closestPoint.LengthSquared();

            float progression = dotS + distanceToClosest;
            //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
            //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
            //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
            //After a bunch of iterations, the system lets it pick the 'better' one.
            if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
                return true;
            if (distanceToClosest < previousDistanceToClosest)
                previousDistanceToClosest = distanceToClosest;

            //If "A" is the new point always, then the switch statement can be removed
            //in favor of just pushing three points up.
            switch (State)
            {
                case SimplexState.Point:
                    if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Segment;
                    B = S;
                    SimplexA.B = sa;
                    SimplexB.B = sb;
                    return false;
                case SimplexState.Segment:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Triangle;
                    C = S;
                    SimplexA.C = sa;
                    SimplexB.C = sb;
                    return false;
                case SimplexState.Triangle:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Tetrahedron;
                    D = S;
                    SimplexA.D = sa;
                    SimplexB.D = sb;
                    return false;
            }
            return false;
        }

		public bool AddSupportPoint(ref Vector3 newPoint)
		{
			int num = (BitsToIndices[simplexBits ^ 15] & 7) - 1;
			
			y[num] = newPoint;
			yLengthSq[num] = newPoint.LengthSquared();
			
			for (int num2 = BitsToIndices[simplexBits]; num2 != 0; num2 >>= 3)
			{
				int num3 = (num2 & 7) - 1;
				Vector3 vector = y[num3] - newPoint;
				edges[num3][num] = vector;
				edges[num][num3] = -vector;
				edgeLengthSq[num][num3] = (edgeLengthSq[num3][num] = vector.LengthSquared());
			}

			UpdateDeterminant(num);

			return UpdateSimplex(num);
		}

	    public static void CalculateDiffAxisAngle(ref Matrix transform0,ref Matrix transform1,ref Vector3 axis,ref float angle)
	    {
            //Matrix dmat = GetRotateMatrix(ref transform1) * Matrix.Invert(GetRotateMatrix(ref transform0));
            Matrix dmat = MathUtil.BulletMatrixMultiplyBasis(transform1, Matrix.Invert(transform0));
		    Quaternion dorn = Quaternion.Identity;
		    GetRotation(ref dmat,out dorn);

		    ///floating point inaccuracy can lead to w component > 1..., which breaks 
		    dorn.Normalize();
    		
		    angle = MathUtil.QuatAngle(ref dorn);
            
		    axis = new Vector3(dorn.X,dorn.Y,dorn.Z);
            //axis[3] = float(0.);
		    //check for axis length
		    float len = axis.LengthSquared();
            if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
            {
                axis = Vector3.Right;
            }
            else
            {
                axis.Normalize();
            }
	    }

	    public static void CalculateDiffAxisAngleQuaternion(ref Quaternion orn0,ref Quaternion orn1a,ref Vector3 axis,ref float angle)
	    {
            Quaternion orn1 = MathUtil.QuatFurthest(ref orn0,ref orn1a);
            Quaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);

		    ///floating point inaccuracy can lead to w component > 1..., which breaks 
		    dorn.Normalize();
		    angle = MathUtil.QuatAngle(ref dorn);
		    axis = new Vector3(dorn.X,dorn.Y,dorn.Z);

		    //check for axis length
		    float len = axis.LengthSquared();
		    if (len < MathUtil.SIMD_EPSILON*MathUtil.SIMD_EPSILON)
            {
			    axis = new Vector3(1f,0,0);
            }
		    else
            {
			    axis.Normalize();
            }
	    }