C# IndexedVector3.LengthSquared方法代码示例

        public bool CleanupVertices(int svcount,
                           IList<IndexedVector3> svertices,
                           int stride,
                           ref int vcount,       // output number of vertices
                           IList<IndexedVector3> vertices,                 // location to store the results.
                           float normalepsilon,
                           ref IndexedVector3 scale)
        {
            if (svcount == 0)
            {
                return false;
            }

            m_vertexIndexMapping.Clear();

            vcount = 0;

            IndexedVector3 recip = new IndexedVector3();

            scale = new IndexedVector3(1);

            IndexedVector3 bmin = MathUtil.MAX_VECTOR;
            IndexedVector3 bmax = MathUtil.MIN_VECTOR;

            //char *vtx = (char *) svertices;

            //	if ( 1 )
            {
                for (int i = 0; i < svcount; i++)
                {
                    IndexedVector3 p = svertices[i];
                    MathUtil.VectorMin(ref p, ref bmin);
                    MathUtil.VectorMax(ref p, ref bmax);
                    svertices[i] = p;
                }
            }

            IndexedVector3 diff = bmax - bmin;

            IndexedVector3 center = diff * 0.5f;
            center += bmin;
            if (diff.X < EPSILON || diff.Y < EPSILON || diff.Z < EPSILON || svcount < 3)
            {

                float len = float.MaxValue;

                if (diff.X > EPSILON && diff.X < len) len = diff.X;
                if (diff.Y > EPSILON && diff.Y < len) len = diff.Y;
                if (diff.Z > EPSILON && diff.Z < len) len = diff.Z;

                if (len == float.MaxValue)
                {
                    diff = new IndexedVector3(0.01f);
                }
                else
                {
                    if (diff.X < EPSILON) diff.X = len * 0.05f; // 1/5th the shortest non-zero edge.
                    if (diff.Y < EPSILON) diff.Y = len * 0.05f;
                    if (diff.Z < EPSILON) diff.Z = len * 0.05f;
                }

                float x1 = center.X - diff.X;
                float x2 = center.X + diff.X;

                float y1 = center.Y - diff.Y;
                float y2 = center.Y + diff.Y;

                float z1 = center.Z - diff.Z;
                float z2 = center.Z + diff.Z;

                AddPoint(ref vcount, vertices, x1, y1, z1);
                AddPoint(ref vcount, vertices, x2, y1, z1);
                AddPoint(ref vcount, vertices, x2, y2, z1);
                AddPoint(ref vcount, vertices, x1, y2, z1);
                AddPoint(ref vcount, vertices, x1, y1, z2);
                AddPoint(ref vcount, vertices, x2, y1, z2);
                AddPoint(ref vcount, vertices, x2, y2, z2);
                AddPoint(ref vcount, vertices, x1, y2, z2);

                return true; // return cube


            }
            else
            {
                if (scale.LengthSquared() > 0)
                {
                    scale = diff;
                    //scale.Value.X = dx;
                    //scale.Value.Y = dy;
                    //scale.Value.Z = dz;

                    recip.X = 1 / diff.X;
                    recip.Y = 1 / diff.Y;
                    recip.Z = 1 / diff.Z;

                    //recip[0] = 1 / dx;
                    //recip[1] = 1 / dy;
                    //recip[2] = 1 / dz;

//.........这里部分代码省略.........

//.........这里部分代码省略.........
                        checkSimplex = true;
                        break;
                    }
                    // are we getting any closer ?
                    float f0 = squaredDistance - delta;
                    float f1 = squaredDistance * REL_ERROR2;

                    if (f0 <= f1)
                    {
                        if (f0 <= 0f)
                        {
                            m_degenerateSimplex = 2;
                        }
                        else
                        {
                            m_degenerateSimplex = 11;
                        }
                        checkSimplex = true;
                        break;
                    }
                    //add current vertex to simplex
                    m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);

                    //calculate the closest point to the origin (update vector v)
                    IndexedVector3 newCachedSeparatingAxis;

                    if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
                    {
                        m_degenerateSimplex = 3;
                        checkSimplex = true;
                        break;
                    }

                    if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
                    {
                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
                        m_degenerateSimplex = 6;
                        checkSimplex = true;
                        break;
                    }

                    float previousSquaredDistance = squaredDistance;
                    squaredDistance = newCachedSeparatingAxis.LengthSquared();
#if DEBUG
					if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
                    }
#endif

#if false
                    ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
                    if (squaredDistance>previousSquaredDistance)
                    {
                        m_degenerateSimplex = 7;
                        squaredDistance = previousSquaredDistance;
                        checkSimplex = false;
                        break;
                    }

        public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
        {
            BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
            IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
            float boxMargin = boxShape.GetMargin();
            penetrationDepth = 1.0f;

            // convert the sphere position to the box's local space
            IndexedMatrix m44T = boxObj.GetWorldTransform();
            IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);

            // Determine the closest point to the sphere center in the box
            IndexedVector3 closestPoint = sphereRelPos;
            closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
            closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
            closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
            closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
            closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
            closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));

            float intersectionDist = fRadius + boxMargin;
            float contactDist = intersectionDist + maxContactDistance;
            normal = sphereRelPos - closestPoint;

            //if there is no penetration, we are done
            float dist2 = normal.LengthSquared();
            if (dist2 > contactDist * contactDist)
            {
                return false;
            }

            float distance;

            //special case if the sphere center is inside the box
            if (dist2 == 0.0f)
            {
                distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
            }
            else //compute the penetration details
            {
                distance = normal.Length();
                normal /= distance;
            }

            pointOnBox = closestPoint + normal * boxMargin;
            //	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
            penetrationDepth = distance - intersectionDist;

            // transform back in world space
            IndexedVector3 tmp = m44T * pointOnBox;
            pointOnBox = tmp;
            //	tmp = m44T(v3PointOnSphere);
            //	v3PointOnSphere = tmp;
            tmp = m44T._basis * normal;
            normal = tmp;

            return true;
        }

        public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
        {
            IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
            IndexedQuaternion 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 IndexedVector3(dorn.X, dorn.Y, dorn.Z);

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

        public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle)
        {
            //IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0));
            IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse();
            IndexedQuaternion dorn = IndexedQuaternion.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 IndexedVector3(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 = new IndexedVector3(1,0,0);
            }
            else
            {
                axis.Normalize();
            }
        }

//.........这里部分代码省略.........
                tetra[3] = 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=IndexedVector3.Dot(ref best.n,ref 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],
                                        ref 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; 
                        }
                    }
                    IndexedVector3	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]	=	IndexedVector3.Cross(	outer.c[1].w-projection,
                        outer.c[2].w-projection).Length();
                    m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
                        outer.c[0].w-projection).Length();
                    m_result.p[2] = IndexedVector3.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 IndexedVector3(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 void	ApplyDamping(float timeStep)
        {
	        //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
	        //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway

        //#define USE_OLD_DAMPING_METHOD 1
        #if USE_OLD_DAMPING_METHOD
	        m_linearVelocity *= GEN_clamped((float(1.) - timeStep * m_linearDamping), (float)float(0.0), (float)float(1.0));
	        m_angularVelocity *= GEN_clamped((float(1.) - timeStep * m_angularDamping), (float)float(0.0), (float)float(1.0));
        #else
	        m_linearVelocity *= (float)Math.Pow((1f-m_linearDamping), timeStep);
            m_angularVelocity *= (float)Math.Pow((1f - m_angularDamping), timeStep);
            MathUtil.SanityCheckVector(ref m_linearVelocity);
            MathUtil.SanityCheckVector(ref m_angularVelocity);
#endif

	        if (m_additionalDamping)
	        {
		        //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
		        //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
		        if ((m_angularVelocity.LengthSquared() < m_additionalAngularDampingThresholdSqr) &&
			        (m_linearVelocity.LengthSquared() < m_additionalLinearDampingThresholdSqr))
		        {
			        m_angularVelocity *= m_additionalDampingFactor;
			        m_linearVelocity *= m_additionalDampingFactor;
		        }


                MathUtil.SanityCheckVector(ref m_linearVelocity);
                MathUtil.SanityCheckVector(ref m_angularVelocity);
                
                float speed = m_linearVelocity.Length();
		        if (speed < m_linearDamping)
		        {
			        float dampVel = 0.005f;
			        if (speed > dampVel)
			        {
				        IndexedVector3 dir = m_linearVelocity;
                        dir.Normalize();
				        m_linearVelocity -=  dir * dampVel;
			        } 
                    else
			        {
				        m_linearVelocity = IndexedVector3.Zero;
			        }
		        }

		        float angSpeed = m_angularVelocity.Length();
		        if (angSpeed < m_angularDamping)
		        {
			        float angDampVel = 0.005f;
			        if (angSpeed > angDampVel)
			        {
				        IndexedVector3 dir = m_angularVelocity;
                        dir.Normalize();
				        m_angularVelocity -=  dir * angDampVel;
			        } else
			        {
                        m_angularVelocity = IndexedVector3.Zero;
			        }
		        }
	        }
            MathUtil.SanityCheckVector(ref m_linearVelocity);
            MathUtil.SanityCheckVector(ref m_angularVelocity);

        }

 public static float ProjectOrigin(ref IndexedVector3 a,ref IndexedVector3 b,ref IndexedVector4 w,ref uint m)
 {
     IndexedVector3	d=b-a;
     float l=d.LengthSquared();
     if (l > GjkEpaSolver2.GJK_SIMPLEX2_EPS)
     {
         float t = (l>0f?(-IndexedVector3.Dot(ref a,ref d)/l):0f);
         if(t>=1)		
         { 
             w.X=0f;
             w.Y=1f;
             m=2;
             return b.LengthSquared(); 
         }
         else if(t<=0)	
         { 
             w.X=1f;
             w.Y=0f;
             m=1;
             return a.LengthSquared(); 
         }
         else
         { 
             w.X=1-(w.Y=t);
             m=3;
             return (a+d*t).LengthSquared(); 
         }
     }
     return(-1);
 }

        public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB)
		{
			m_swingCorrection = 0;
			m_twistLimitSign = 0;
			m_solveTwistLimit = false;
			m_solveSwingLimit = false;

			// compute rotation of A wrt B (in constraint space)
            if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
			{	// it is assumed that setMotorTarget() was alredy called 
				// and motor target m_qTarget is within constraint limits
				// TODO : split rotation to pure swing and pure twist
				// compute desired transforms in world
				IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
				IndexedMatrix trA = transA * m_rbAFrame;
				IndexedMatrix trB = transB * m_rbBFrame;
                IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse();
				IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation();
				IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
                float swingAxisLen2 = swingAxis.LengthSquared();
                if (MathUtil.FuzzyZero(swingAxisLen2))
                {
                    return;
                }

				m_swingAxis = swingAxis;
				m_swingAxis.Normalize();
				m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
				if (!MathUtil.FuzzyZero(m_swingCorrection))
				{
					m_solveSwingLimit = true;
				}
				return;
			}


			{

				// compute rotation of A wrt B (in constraint space)
				// Not sure if these need order swapping as well?
                IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation();
                IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation();
                
                IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA;

				// split rotation into cone and twist
				// (all this is done from B's perspective. Maybe I should be averaging axes...)
				IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
				vConeNoTwist.Normalize();
				IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
				qABCone.Normalize();
				IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
				qABTwist.Normalize();

				if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
				{
					float swingAngle = 0f, swingLimit = 0f;
					IndexedVector3 swingAxis = IndexedVector3.Zero;
					ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);

					if (swingAngle > swingLimit * m_limitSoftness)
					{
						m_solveSwingLimit = true;

						// compute limit ratio: 0->1, where
						// 0 == beginning of soft limit
						// 1 == hard/real limit
						m_swingLimitRatio = 1f;
						if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
						{
							m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
												(swingLimit - (swingLimit * m_limitSoftness));
						}

						// swing correction tries to get back to soft limit
						m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);

						// adjustment of swing axis (based on ellipse normal)
						AdjustSwingAxisToUseEllipseNormal(ref swingAxis);

						// Calculate necessary axis & factors		
						m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);

						m_twistAxisA = IndexedVector3.Zero;

						m_kSwing = 1f /
							(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
							 ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
					}
				}
				else
				{
					// you haven't set any limits;
					// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
					// anyway, we have either hinge or fixed joint

                    IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0);
                    IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1);
                    IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2);
                    IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0);
//.........这里部分代码省略.........

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

			IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
			IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
			IndexedVector3 vel = vel1 - vel2;

            IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
            IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();


			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(),
                body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
                body2.GetLinearVelocity(),
                body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
            float a = jacDiagABInv;

			rel_vel = normal.Dot(ref 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 static void ZeroCheckVector(ref IndexedVector3 v)
        {
#if DEBUG

            if (FuzzyZero(v.LengthSquared()))
            {
                //Debug.Assert(false);
            }
#endif
        }