C# IndexedVector3.Length方法代码示例

 public void SetRotation(ref IndexedVector3 axis, float angle)
 {
     float d = axis.Length();
     Debug.Assert(d != 0.0f);
     float halfAngle = angle * 0.5f;
     float s = (float)Math.Sin(halfAngle) / d;
     X = axis.X * s;
     Y = axis.Y * s;
     Z = axis.Z * s;
     W = (float)Math.Cos(halfAngle);
 }

        protected IndexedVector3 ConeLocalSupport(ref IndexedVector3 v)
        {
	        float halfHeight = m_height * 0.5f;

         if (v[m_coneIndices[1]] > v.Length() * m_sinAngle)
         {
	        IndexedVector3 tmp = new IndexedVector3();

	        tmp[m_coneIndices[0]] = 0.0f;
	        tmp[m_coneIndices[1]] = halfHeight;
	        tmp[m_coneIndices[2]] = 0.0f;
	        return tmp;
         }
          else 
         {
            float s = (float)Math.Sqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
            if (s > MathUtil.SIMD_EPSILON) 
            {
              float d = m_radius / s;
	        IndexedVector3 tmp = new IndexedVector3();
	          tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
	          tmp[m_coneIndices[1]] = -halfHeight;
	          tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
	          return tmp;
            }
            else  
            {
	        IndexedVector3 tmp = new IndexedVector3();
		        tmp[m_coneIndices[0]] = 0.0f;
		        tmp[m_coneIndices[1]] = -halfHeight;
		        tmp[m_coneIndices[2]] = 0.0f;
		        return tmp;
	        }
          }

        }

        ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
	    ///result is conservative
	    public void CalculateTemporalAabb(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep, out IndexedVector3 temporalAabbMin,out IndexedVector3 temporalAabbMax)
        {
	        //start with static aabb
	        GetAabb(ref curTrans,out temporalAabbMin,out temporalAabbMax);

	        float temporalAabbMaxx = temporalAabbMax.X;
	        float temporalAabbMaxy = temporalAabbMax.Y;
	        float temporalAabbMaxz = temporalAabbMax.Z;
	        float temporalAabbMinx = temporalAabbMin.X;
	        float temporalAabbMiny = temporalAabbMin.Y;
	        float temporalAabbMinz = temporalAabbMin.Z;

	        // add linear motion
	        IndexedVector3 linMotion = linvel*timeStep;
	        ///@todo: simd would have a vector max/min operation, instead of per-element access
	        if (linMotion.X > 0f)
		        temporalAabbMaxx += linMotion.X; 
	        else
		        temporalAabbMinx += linMotion.X;
	        if (linMotion.Y > 0f)
		        temporalAabbMaxy += linMotion.Y; 
	        else
		        temporalAabbMiny += linMotion.Y;
	        if (linMotion.Z > 0f)
		        temporalAabbMaxz += linMotion.Z; 
	        else
		        temporalAabbMinz += linMotion.Z;

	        //add conservative angular motion
	        float angularMotion = angvel.Length() * GetAngularMotionDisc() * timeStep;
            IndexedVector3 angularMotion3d = new IndexedVector3(angularMotion);
	        temporalAabbMin = new IndexedVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
	        temporalAabbMax = new IndexedVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);

	        temporalAabbMin -= angularMotion3d;
	        temporalAabbMax += angularMotion3d;

        }

	    public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
	    {
            predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
    //	#define QUATERNION_DERIVATIVE
	    #if QUATERNION_DERIVATIVE
            IndexedVector3 pos;
            IndexedQuaternion predictedOrn;
            IndexedVector3 scale;

            curTrans.Decompose(ref scale, ref predictedOrn, ref pos);


		    predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
		    predictedOrn.Normalize();
        #else
            //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    IndexedVector3 axis;
		    float	fAngle = angvel.Length(); 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );

            IndexedQuaternion orn0 = curTrans.GetRotation();

		    IndexedQuaternion predictedOrn = dorn * orn0;
		    predictedOrn.Normalize();
	    #endif

            IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
            predictedTransform._basis = newMatrix._basis;
	    }

        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 void IntegrateVelocities(float step)
        {
	        if (IsStaticOrKinematicObject())
		        return;

#if DEBUG
			if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
			{
                BulletGlobals.g_streamWriter.WriteLine(String.Format("[{0}] RigidBody integrateVelocities", (String)m_userObjectPointer));
				MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate LinVel pre", m_linearVelocity);
				MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate AngVel pre", m_angularVelocity);
			}
#endif


	        m_linearVelocity += m_totalForce * (m_inverseMass * step);
            MathUtil.SanityCheckVector(ref m_linearVelocity);
            m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
            MathUtil.SanityCheckVector(ref m_angularVelocity);
        
	        /// clamp angular velocity. collision calculations will fail on higher angular velocities	
	        float angvel = m_angularVelocity.Length();
	        if (angvel*step > MAX_ANGVEL)
	        {
		        m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
	        }
            MathUtil.SanityCheckVector(ref m_angularVelocity);

#if DEBUG
			if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
			{
				MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate LinVel post", m_linearVelocity);
				MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate AngVel post", m_angularVelocity);
			}
#endif			
        }

 public void	GetSupport(ref IndexedVector3 d,ref sSV sv)
 {
     sv.d = d/d.Length();
     sv.w = m_shape.Support(ref sv.d);
 }

        public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
        {
            uint iterations=0;
            float sqdist=0f;
            float alpha=0f;
            uint clastw=0;
            /* Initialize solver		*/ 
            m_free[0] =	m_store[0];
            m_free[1] =	m_store[1];
            m_free[2] =	m_store[2];
            m_free[3] =	m_store[3];
            m_nfree	= 4;
            m_current =	0;
            m_status = GJKStatus.Valid;
            m_shape	= shapearg;
            m_distance = 0f;
            /* Initialize simplex		*/ 
            m_simplices[0].rank	= 0;
            m_ray =	guess;
            float sqrl=	m_ray.LengthSquared();
            IndexedVector3 temp = sqrl>0?-m_ray:new IndexedVector3(1,0,0);
            AppendVertice(m_simplices[0],ref temp);
            m_simplices[0].p[0]	= 1;
            m_ray =	m_simplices[0].c[0].w;	
            sqdist =	sqrl;
            lastw[0]=lastw[1]=lastw[2]=lastw[3]	= m_ray;
            /* Loop						*/ 
            do	
            {
                uint next = 1-m_current;
                sSimplex cs = m_simplices[m_current];
                sSimplex ns = m_simplices[next];
                /* Check zero							*/ 
                float rl=m_ray.Length();
                if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
                {/* Touching or inside				*/ 
                    m_status=GJKStatus.Inside;
                    break;
                }
                /* Append new vertice in -'v' direction	*/ 
                IndexedVector3 temp2 = -m_ray;
                AppendVertice(cs,ref temp2);
                IndexedVector3	w = cs.c[cs.rank-1].w;
                bool found = false;
                for(int i=0;i<4;++i)
                {
                    if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
                    { 
                        found=true;
                        break; 
                    }
                }
                if(found)
                {/* Return old simplex				*/ 
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }
                else
                {/* Update lastw					*/ 
                    lastw[clastw=(clastw+1)&3]=w;
                }
                /* Check for termination				*/ 
                float omega=IndexedVector3.Dot(ref m_ray,ref w)/rl;
                alpha=Math.Max(omega,alpha);
                if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
                {/* Return old simplex				*/ 
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }		

                /* Reduce simplex						*/ 
                IndexedVector4 weights = new IndexedVector4();
                uint mask=0;
                switch(cs.rank)
                {
                    case 2 :
                    {
                        sqdist=GJK.ProjectOrigin(ref cs.c[0].w,ref cs.c[1].w,ref weights,ref mask);
                        break;
                    }
                    case 3:
                    {
                        sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
                        break;
                    }
                    case 4:
                    {
                        sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
                        break;
                    }
                }
                if(sqdist>=0)
                {/* Valid	*/ 
                    ns.rank	= 0;
                    m_ray =	IndexedVector3.Zero;
                    m_current =	next;
                    for(uint i=0,ni=cs.rank;i<ni;++i)
                    {
                        if((mask&(1<<(int)i)) != 0)
                        {
//.........这里部分代码省略.........

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

	        imp0 = 0f;
	        imp1 = 0f;

	        float len = Math.Abs(normalA.Length()) - 1f;
	        if (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 = IndexedVector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
	        float vel1 = IndexedVector3.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]
        }

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