C# IndexedVector3.Normalize方法代码示例

//.........这里部分代码省略.........
                                sharedVertsB[0] = tmp;
                            }

                            int hash = GetHash(m_partIdA, m_triangleIndexA);


                            TriangleInfo info = null;
                            if (m_triangleInfoMap.ContainsKey(hash))
                            {
                                info = m_triangleInfoMap[hash];
                            }
                            else
                            {
                                info = new TriangleInfo();
                                m_triangleInfoMap[hash] = info;
                            }

                            int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
                            int otherIndexA = 3 - sumvertsA;


                            IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);

                            TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
                            int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);

                            TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
                            //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);

                            IndexedVector3 normalA;
                            IndexedVector3 normalB;
                            tA.CalcNormal(out normalA);
                            tB.CalcNormal(out normalB);
                            edge.Normalize();
                            IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));

                            {
                                IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
                                if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
                                {
                                    edgeCrossA *= -1;
                                }
                            }

                            IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();

                            {
                                IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
                                if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
                                {
                                    edgeCrossB *= -1;
                                }
                            }

                            float angle2 = 0;
                            float ang4 = 0.0f;

                            IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
                            float len2 = calculatedEdge.LengthSquared();

                            float correctedAngle = 0f;
                            IndexedVector3 calculatedNormalB = normalA;
                            bool isConvex = false;

                            if (len2 < m_triangleInfoMap.m_planarEpsilon)
                            {

        public void CalcNormal(out IndexedVector3 normal) 
	    {
            normal = IndexedVector3.Cross(m_vertices1[1]-m_vertices1[0],m_vertices1[2]-m_vertices1[0]);
		    normal.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 override void InitializeDemo()
        {
            //string filename = @"E:\users\man\bullet\xna-concaveray-output-1.txt";
            //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
            //BulletGlobals.g_streamWriter = new StreamWriter(filestream);

			m_cameraDistance = 400f;
			m_farClip = 1500f;
            m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);


            m_animatedMesh = true;
			base.InitializeDemo();
			int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);

			gVertices = new ObjectArray<IndexedVector3>(totalVerts);
			int indicesTotal = totalTriangles * 3;
            gIndices = new ObjectArray<int>(indicesTotal);

			BulletGlobals.gContactAddedCallback = null;

			SetVertexPositions(waveheight,0f);

			int vertStride = 1;
			int indexStride = 3;

			int index=0;
            //for (int i=0;i<NUM_VERTS_X-1;i++)
            //{
            //    for (int j=0;j<NUM_VERTS_Y-1;j++)
            //    {
            //        gIndices[index++] = j * NUM_VERTS_X + i;
            //        gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
            //        gIndices[index++] = j * NUM_VERTS_X + i + 1;

            //        gIndices[index++] = j * NUM_VERTS_X + i;
            //        gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
            //        gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;

            //    }
            //}

            for (int i = 0; i < NUM_VERTS_X - 1; i++)
            {
                for (int j = 0; j < NUM_VERTS_Y - 1; j++)
                {
                    gIndices[index++] = j * NUM_VERTS_X + i;
                    gIndices[index++] = j * NUM_VERTS_X + i + 1;
                    gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;

                    gIndices[index++] = j * NUM_VERTS_X + i;
                    gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
                    gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
                }
            }

			TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
				gIndices,indexStride,totalVerts,gVertices,vertStride);

			bool useQuantizedAabbCompression = true;

			float minX = NUM_VERTS_X * TRIANGLE_SIZE * 0.5f;
			float minZ = NUM_VERTS_Y * TRIANGLE_SIZE * 0.5f;

			//OptimizedBvh bvh = new OptimizedBvh();
	        IndexedVector3 aabbMin = new IndexedVector3(-minX,-5,-minZ);
	        IndexedVector3 aabbMax = new IndexedVector3(minX,5,minZ);




            m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
            //m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression,true);
	        IndexedVector3 scaling = IndexedVector3.One;
			CollisionShape groundShape = m_trimeshShape;
            //groundShape = new TriangleMeshShape(indexVertexArrays);
			//groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
            IndexedVector3 up = new IndexedVector3(0.4f,1,0);
            up.Normalize();
			//groundShape = new StaticPlaneShape(up, 0f);
			//groundShape = new BoxShape(new IndexedVector3(1000, 10, 1000));
			m_collisionConfiguration = new DefaultCollisionConfiguration();
			m_dispatcher = new	CollisionDispatcher(m_collisionConfiguration);

			IndexedVector3 worldMin = aabbMin;
			IndexedVector3 worldMax = aabbMax;

			m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
			m_constraintSolver = new SequentialImpulseConstraintSolver();
			m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

			float mass = 0f;
			IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2,-2,0));


			startTransform = IndexedMatrix.Identity;
			staticBody = LocalCreateRigidBody(mass, ref startTransform,groundShape);

			staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT);

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

        //----------------------------------------------------------------------------------------------

        public DemoApplication()
        {
            m_dynamicsWorld = null;
            m_pickConstraint = null;
            m_shootBoxShape = null;
            m_cameraDistance = 30f;
            m_pitch =(20f/360f)*MathUtil.SIMD_2_PI;
            m_yaw = 0f;
            m_cameraPosition = IndexedVector3.Zero;
            m_cameraTargetPosition = IndexedVector3.Zero;
            m_scaleBottom = 0.5f;
            m_scaleFactor = 2f;
            m_cameraUp = new IndexedVector3(0, 1, 0);
            m_forwardAxis = 2;
            m_glutScreenWidth = 0;
            m_glutScreenHeight = 0;
            m_ShootBoxInitialSpeed = 40f;
            m_stepping = true;
            m_singleStep = false;
            m_idle = false;
            m_enableshadows = true;
            m_lightPosition = new IndexedVector3(5, 5, 5);
            //m_lightDirection = IndexedVector3.Down;
            m_lightDirection = new IndexedVector3(.5f, -.5f, .5f);
            m_lightDirection.Normalize();

            //#ifndef BT_NO_PROFILE
            //    m_profileIterator = CProfileManager::Get_Iterator();
            //#endif //BT_NO_PROFILE
            
            Content.RootDirectory = "Content";
            m_graphics = new GraphicsDeviceManager(this);
            m_graphics.PreferredBackBufferWidth = 800;
            m_graphics.PreferredBackBufferHeight = 600;


            SetSize(m_graphics.PreferredBackBufferWidth,m_graphics.PreferredBackBufferHeight);
            m_nearClip = 1f;
            m_farClip = 1000f;

            m_aspect = m_glutScreenWidth / m_glutScreenHeight;
            m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
        }

        //----------------------------------------------------------------------------------------------

        public IndexedVector3 GetRayTo(int x, int y)
        {
            float fov = MathHelper.ToRadians(40.0f);

            IndexedVector3 rayFrom = new IndexedVector3(GetCameraPosition());
            IndexedVector3 rayForward = new IndexedVector3((GetCameraTargetPosition() - GetCameraPosition()));
            rayForward.Normalize();
            float farPlane = 10000f;
            rayForward *= farPlane;

            IndexedVector3 vertical = new IndexedVector3(m_cameraUp);

            IndexedVector3 hor = IndexedVector3.Cross(rayForward, vertical);
            hor.Normalize();
            vertical = IndexedVector3.Cross(hor, rayForward);
            vertical.Normalize();

            float tanfov = (float)Math.Tan(0.5f * fov);

            hor *= 2f * farPlane * tanfov;
            vertical *= 2f * farPlane * tanfov;

            float aspect = 1f;

            if (m_glutScreenWidth > m_glutScreenHeight)
            {
                aspect = m_glutScreenWidth / (float)m_glutScreenHeight;
                hor *= aspect;
            }
            else
            {
                aspect = m_glutScreenHeight / (float)m_glutScreenWidth;
                vertical *= aspect;
            }

            IndexedVector3 rayToCenter = rayFrom + rayForward;
            IndexedVector3 dHor = hor * 1f / (float)m_glutScreenWidth;
            IndexedVector3 dVert = vertical * 1f / (float)m_glutScreenHeight;

            IndexedVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
            rayTo += x * dHor;
            rayTo -= y * dVert;
            return rayTo;
        }

        public void ShootTrimesh(IndexedVector3 startPosition, IndexedVector3 destination)
    {

	    if (m_dynamicsWorld != null)
	    {
		    float mass = 4.0f;
            IndexedMatrix startTransform = IndexedMatrix.Identity;
            startTransform._origin = new IndexedVector3(startPosition);

		    RigidBody body = LocalCreateRigidBody(mass, startTransform,m_trimeshShape2);

		    IndexedVector3 linVel = new IndexedVector3(destination - startPosition);
		    linVel.Normalize();
		    linVel*=m_ShootBoxInitialSpeed*0.25f;

		    body.SetLinearVelocity(ref linVel);
		    body.SetAngularVelocity(IndexedVector3.Zero);
	    }
    }

        public override void InitializeDemo()
        {
            m_cameraDistance = 10.0f;
            //string filename = @"e:\users\man\bullet\gimpact-demo-xna.txt";
            //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
            //BulletGlobals.g_streamWriter = new StreamWriter(filestream);

            /// Init Bullet
            m_collisionConfiguration = new DefaultCollisionConfiguration();

            m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
            //btOverlappingPairCache* broadphase = new btSimpleBroadphase();
            //m_broadphase = new btSimpleBroadphase();

            int maxProxies = 1024;
            IndexedVector3 worldAabbMin = new IndexedVector3(-10000, -10000, -10000);
            IndexedVector3 worldAabbMax = new IndexedVector3(10000, 10000, 10000);
            m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, false);  
            //m_broadphase = new SimpleBroadphase(16384,null);
            m_constraintSolver = new SequentialImpulseConstraintSolver();

            m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

            //create trimesh model and shape
            InitGImpactCollision();



            /// Create Scene
            float mass = 0.0f;
            IndexedMatrix startTransform = IndexedMatrix.Identity;


            CollisionShape staticboxShape1 = new BoxShape(new IndexedVector3(200, 1, 200));//floor
            staticboxShape1.SetUserPointer("Floor");
            CollisionShape staticboxShape2 = new BoxShape(new IndexedVector3(1, 50, 200));//left wall
            staticboxShape1.SetUserPointer("LeftWall");
            CollisionShape staticboxShape3 = new BoxShape(new IndexedVector3(1, 50, 200));//right wall
            staticboxShape1.SetUserPointer("RightWall");
            CollisionShape staticboxShape4 = new BoxShape(new IndexedVector3(200, 50, 1));//front wall
            staticboxShape1.SetUserPointer("FrontWall");
            CollisionShape staticboxShape5 = new BoxShape(new IndexedVector3(200, 50, 1));//back wall
            staticboxShape1.SetUserPointer("BackWall");

            CompoundShape staticScenario = new CompoundShape();//static scenario

            startTransform._origin = new IndexedVector3(0, 0, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape1);
            startTransform._origin = new IndexedVector3(-200, 25, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape2);
            startTransform._origin = new IndexedVector3(200, 25, 0);
            staticScenario.AddChildShape(ref startTransform, staticboxShape3);
            startTransform._origin = new IndexedVector3(0, 25, 200);
            staticScenario.AddChildShape(ref startTransform, staticboxShape4);
            startTransform._origin = new IndexedVector3(0, 25, -200);
            staticScenario.AddChildShape(ref startTransform, staticboxShape5);

            startTransform._origin = new IndexedVector3(0, 0, 0);

            //RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticScenario);
            RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticboxShape1);


	        staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT);

	        //enable custom material callback
	        staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);

	        //static plane
	        IndexedVector3 normal = new IndexedVector3(0.4f,1.5f,-0.4f);
	        normal.Normalize();
	        CollisionShape staticplaneShape6 = new StaticPlaneShape(ref normal,0.0f);// A plane

	        startTransform._origin = IndexedVector3.Zero;

            RigidBody staticBody2 = LocalCreateRigidBody(mass, startTransform, staticplaneShape6);

	        staticBody2.SetCollisionFlags(staticBody2.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT);

            startTransform = IndexedMatrix.Identity;

            /// Create Dynamic Boxes
            {
                int numBoxes = 1;
                for (int i = 0; i < numBoxes; i++)
                {
                    CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
                    //CollisionShape mesh = new BvhTriangleMeshShape(m_indexVertexArrays2,true,true);
                    startTransform._origin = new IndexedVector3(2 * i - (numBoxes-1), 2, -3);
                    //startTransform._origin = new IndexedVector3(2 * i - 5, 10, -3);
                    //LocalCreateRigidBody(1, startTransform, m_trimeshShape2);
                    LocalCreateRigidBody(1, startTransform, boxShape);
                }
            }
        }

		protected void ComputeTwistLimitInfo(ref IndexedQuaternion qTwist, // in
			out float twistAngle, out IndexedVector3 vTwistAxis) // all outs
		{
			IndexedQuaternion qMinTwist = qTwist;
			twistAngle = MathUtil.QuatAngle(ref qTwist);

			if (twistAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate.
			{
				qMinTwist = -(qTwist);
				twistAngle = MathUtil.QuatAngle(ref qTwist);
			}
			if (twistAngle < 0f)
			{
				// this should never happen
				Debug.Assert(false);
			}

			vTwistAxis = new IndexedVector3(qMinTwist.X, qMinTwist.Y, qMinTwist.Z);
			if (twistAngle > MathUtil.SIMD_EPSILON)
			{
				vTwistAxis.Normalize();
			}
		}

		protected void AdjustSwingAxisToUseEllipseNormal(ref IndexedVector3 vSwingAxis)
		{
			// the swing axis is computed as the "twist-free" cone rotation,
			// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
			// so, if we're outside the limits, the closest way back inside the cone isn't 
			// along the vector back to the center. better (and more stable) to use the ellipse normal.

			// convert swing axis to direction from center to surface of ellipse
			// (ie. rotate 2D vector by PI/2)
			float y = -vSwingAxis.Z;
			float z = vSwingAxis.Y;

			// do the math...
			if (Math.Abs(z) > MathUtil.SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
			{
				// compute gradient/normal of ellipse surface at current "point"
				float grad = y / z;
				grad *= m_swingSpan2 / m_swingSpan1;

				// adjust y/z to represent normal at point (instead of vector to point)
				if (y > 0)
				{
					y = Math.Abs(grad * z);
				}
				else
				{
					y = -Math.Abs(grad * z);
				}

				// convert ellipse direction back to swing axis
				vSwingAxis.Z = -y;
				vSwingAxis.Y = z;
				vSwingAxis.Normalize();
			}
		}

		protected void ComputeConeLimitInfo(ref IndexedQuaternion qCone, // in
			ref float swingAngle, ref IndexedVector3 vSwingAxis, ref float swingLimit) // all outs
		{
			swingAngle = MathUtil.QuatAngle(ref qCone);
			if (swingAngle > MathUtil.SIMD_EPSILON)
			{
				vSwingAxis = new IndexedVector3(qCone.X, qCone.Y, qCone.Z);
				vSwingAxis.Normalize();
				if (Math.Abs(vSwingAxis.X) > MathUtil.SIMD_EPSILON)
				{
					// non-zero twist?! this should never happen.
					Debug.Assert(false);
				}

				// Compute limit for given swing. tricky:
				// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
				// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)

				// For starters, compute the direction from center to surface of ellipse.
				// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
				// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
				float xEllipse = vSwingAxis.Y;
				float yEllipse = -vSwingAxis.Z;

				// Now, we use the slope of the vector (using x/yEllipse) and find the length
				// of the line that intersects the ellipse:
				//  x^2   y^2
				//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
				//  a^2   b^2
				// Do the math and it should be clear.

				swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
				if (Math.Abs(xEllipse) > MathUtil.SIMD_EPSILON)
				{
					float surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
					float norm = 1f / (m_swingSpan2 * m_swingSpan2);
					norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
					float swingLimit2 = (1 + surfaceSlope2) / norm;
					swingLimit = (float)Math.Sqrt(swingLimit2);
				}

				// test!
				/*swingLimit = m_swingSpan2;
				if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
				{
				float mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
				float sinphi = m_swingSpan2 / sqrt(mag_2);
				float phi = asin(sinphi);
				float theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
				float alpha = 3.14159f - theta - phi;
				float sinalpha = sin(alpha);
				swingLimit = m_swingSpan1 * sinphi/sinalpha;
				}*/
			}
			else if (swingAngle < 0)
			{
				// this should never happen!
				Debug.Assert(false);
			}
		}

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

		public void CalcAngleInfo()
		{
			m_swingCorrection = 0f;
			m_twistLimitSign = 0f;
			m_solveTwistLimit = false;
			m_solveSwingLimit = false;

			IndexedVector3 b1Axis1 = IndexedVector3.Zero, b1Axis2 = IndexedVector3.Zero, b1Axis3 = IndexedVector3.Zero;
			IndexedVector3 b2Axis1 = IndexedVector3.Zero, b2Axis2 = IndexedVector3.Zero;

			IndexedMatrix transA = GetRigidBodyA().GetCenterOfMassTransform();
			IndexedMatrix transB = GetRigidBodyB().GetCenterOfMassTransform();

            b1Axis1 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(0);
            b2Axis1 = GetRigidBodyB().GetCenterOfMassTransform()._basis * m_rbBFrame._basis.GetColumn(0);

			float swing1 = 0f, swing2 = 0f;

			float swx = 0f, swy = 0f;
			float thresh = 10f;
			float fact;

			// Get Frame into world space
			if (m_swingSpan1 >= 0.05f)
			{
                b1Axis2 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(1);
				swx = b2Axis1.Dot(ref b1Axis1);
				swy = b2Axis1.Dot(ref b1Axis2);
				swing1 = (float)Math.Atan2(swy, swx);
				fact = (swy * swy + swx * swx) * thresh * thresh;
				fact = fact / (fact + 1f);
				swing1 *= fact;
			}

			if (m_swingSpan2 >= 0.05f)
			{
                b1Axis3 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(2);
				swx = b2Axis1.Dot(ref b1Axis1);
				swy = b2Axis1.Dot(ref b1Axis3);
				swing2 = (float)Math.Atan2(swy, swx);
				fact = (swy * swy + swx * swx) * thresh * thresh;
				fact = fact / (fact + 1f);
				swing2 *= fact;
			}

			float RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
			float RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
			float EllipseAngle = Math.Abs(swing1 * swing1) * RMaxAngle1Sq + Math.Abs(swing2 * swing2) * RMaxAngle2Sq;

			if (EllipseAngle > 1.0f)
			{
				m_swingCorrection = EllipseAngle - 1.0f;
				m_solveSwingLimit = true;
				// Calculate necessary axis & factors
				m_swingAxis = b2Axis1.Cross(b1Axis2 * b2Axis1.Dot(ref b1Axis2) + b1Axis3 * b2Axis1.Dot(ref b1Axis3));
				m_swingAxis.Normalize();
				float swingAxisSign = (b2Axis1.Dot(ref b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
				m_swingAxis *= swingAxisSign;
			}

			// Twist limits
			if (m_twistSpan >= 0f)
			{
                IndexedVector3 b2Axis2a = GetRigidBodyB().GetCenterOfMassTransform()._basis * m_rbBFrame._basis.GetColumn(1); 
				IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref b2Axis1, ref b1Axis1);
				IndexedVector3 TwistRef = MathUtil.QuatRotate(ref rotationArc, ref b2Axis2a);
				float twist = (float)Math.Atan2(TwistRef.Dot(ref b1Axis3), TwistRef.Dot(ref b1Axis2));
				m_twistAngle = twist;

				//		float lockedFreeFactor = (m_twistSpan > float(0.05f)) ? m_limitSoftness : float(0.);
				float lockedFreeFactor = (m_twistSpan > 0.05f) ? 1.0f : 0f;
				if (twist <= -m_twistSpan * lockedFreeFactor)
				{
					m_twistCorrection = -(twist + m_twistSpan);
					m_solveTwistLimit = true;
					m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
					m_twistAxis.Normalize();
					m_twistAxis *= -1.0f;
				}
				else if (twist > m_twistSpan * lockedFreeFactor)
				{
					m_twistCorrection = (twist - m_twistSpan);
					m_solveTwistLimit = true;
					m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
					m_twistAxis.Normalize();
				}
			}

		}

//.........这里部分代码省略.........
			//{
			//    int numBvh = import.getNumBvhs();
			//    if (numBvh != 0)
			//    {
			//        OptimizedBvh bvh = import.getBvhByIndex(0);
			//        IndexedVector3 aabbMin = new IndexedVector3(-1000,-1000,-1000);
			//        IndexedVector3 aabbMax = new IndexedVector3(1000,1000,1000);
			
			//        trimeshShape = new indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,false);
			//        IndexedVector3 scaling = IndexedVector3.One;
			//        trimeshShape.setOptimizedBvh(bvh, ref scaling);
			//        //trimeshShape  = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
			//        //trimeshShape.setOptimizedBvh(bvh);
			
			//    }
			//    int numShape = import.getNumCollisionShapes();
			//    if (numShape != 0)
			//    {
			//        trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByIndex(0);
					
			//        //if you know the name, you can also try to get the shape by name:
			//        String meshName = import.getNameForPointer(trimeshShape);
			//        if (meshName != null)
			//        {
			//            trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByName(meshName);
			//        }
					
			//    }
			//}


            //CollisionShape groundShape = trimeshShape;//m_trimeshShape;
            CollisionShape groundShape = m_trimeshShape;//m_trimeshShape;

            //groundShape = new TriangleShape(new IndexedVector3(0,` 0, 100), new IndexedVector3(100, 0, 0),new IndexedVector3(-100, 0, -100));
            //groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
            //groundShape = new BoxShape(new IndexedVector3(100f, 0.1f, 100f));
            IndexedVector3 up = new IndexedVector3(0.4f,1,0);
            up.Normalize();
            //groundShape = new StaticPlaneShape(up, 0f);
            //groundShape = new TriangleMeshShape(indexVertexArrays);
            
			m_collisionConfiguration = new DefaultCollisionConfiguration();
			m_dispatcher = new	CollisionDispatcher(m_collisionConfiguration);

			IndexedVector3 worldMin = new IndexedVector3(-1000,-1000,-1000);
			IndexedVector3 worldMax = new IndexedVector3(1000,1000,1000);
            //m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
            m_broadphase = new DbvtBroadphase();
			m_constraintSolver = new SequentialImpulseConstraintSolver();
			m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

			float mass = 0f;
			IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2,-2,0));

            //CompoundShape colShape = new CompoundShape();
            //IndexedVector3 halfExtents = new IndexedVector3(4, 1, 1);
            //CollisionShape cylinderShape = new CylinderShapeX(ref halfExtents);
            //CollisionShape boxShape = new BoxShape(new IndexedVector3(4, 1, 1));
            //IndexedMatrix localTransform = IndexedMatrix.Identity;
            //colShape.addChildShape(ref localTransform, boxShape);
            //Quaternion orn = Quaternion.CreateFromYawPitchRoll(MathUtil.SIMD_HALF_PI, 0f, 0f);
            //localTransform = IndexedMatrix.CreateFromQuaternion(orn);
            //colShape.addChildShape(ref localTransform, cylinderShape);

            ////BoxShape colShape = new BoxShape(new IndexedVector3(1, 1, 1));


            //int numCollideObjects = 1;
            //m_collisionShapes.Add(colShape);
            //{
            //    for (int i = 0; i < numCollideObjects; i++)
            //    {
            //        startTransform._origin = new IndexedVector3(4,10+i*2,1);
            //        localCreateRigidBody(1, ref startTransform,colShape);
            //    }
            //}

			CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
			//CollisionShape boxShape = new SphereShape(1);
            //CollisionShape boxShape = new SphereShape(1);
            //CollisionShape boxShape = new CapsuleShapeZ(0.5f, 1);
            m_collisionShapes.Add(boxShape);
            for (int i = 0; i < 1; i++)
            {
                startTransform._origin = new IndexedVector3(2f * i, 5, 1);
                LocalCreateRigidBody(1, ref startTransform, boxShape);
            }


			startTransform = IndexedMatrix.Identity;
			staticBody = LocalCreateRigidBody(mass, ref startTransform,groundShape);

			staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT);

			//enable custom material callback
			staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);

            //clientResetScene();
		}

        public bool Collide(ref IndexedVector3 sphereCenter, out IndexedVector3 point, out IndexedVector3 resultNormal, ref float depth, ref float timeOfImpact, float contactBreakingThreshold)
        {
            IndexedVector3[] vertices = m_triangle.GetVertexPtr(0);

            float radius = m_sphere.GetRadius();
            float radiusWithThreshold = radius + contactBreakingThreshold;
            IndexedVector3 v1;
            IndexedVector3.Subtract(out v1,ref vertices[1],ref vertices[0]);
            IndexedVector3 v2;
            IndexedVector3.Subtract(out v2,ref vertices[2],ref vertices[0]);

            IndexedVector3 normal = new IndexedVector3(v1.Y * v2.Z - v1.Z * v2.Y,v1.Z * v2.X - v1.X * v2.Z,v1.X * v2.Y - v1.Y * v2.X);

            //IndexedVector3 normal = IndexedVector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);

            normal.Normalize();
            IndexedVector3 p1ToCentre;
            IndexedVector3.Subtract(out p1ToCentre,ref sphereCenter,ref vertices[0]);
            float distanceFromPlane = IndexedVector3.Dot(ref p1ToCentre, ref normal);

            if (distanceFromPlane < 0f)
            {
                //triangle facing the other way
                distanceFromPlane *= -1f;
                normal *= -1f;
            }

            bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;


            // Check for contact / intersection
            bool hasContact = false;
            IndexedVector3 contactPoint = IndexedVector3.Zero;
            if (isInsideContactPlane)
            {
                if (FaceContains(ref sphereCenter, vertices, ref normal))
                {
                    // Inside the contact wedge - touches a point on the shell plane
                    hasContact = true;
                    contactPoint = sphereCenter - normal * distanceFromPlane;
                }
                else
                {
                    // Could be inside one of the contact capsules
                    float contactCapsuleRadiusSqr = (radiusWithThreshold) * (radiusWithThreshold);
                    IndexedVector3 nearestOnEdge;
                    for (int i = 0; i < m_triangle.GetNumEdges(); i++)
                    {

                        IndexedVector3 pa;
                        IndexedVector3 pb;

                        m_triangle.GetEdge(i, out pa, out pb);

                        float distanceSqr = SegmentSqrDistance(ref pa, ref pb, ref sphereCenter, out nearestOnEdge);
                        if (distanceSqr < contactCapsuleRadiusSqr)
                        {
                            // Yep, we're inside a capsule
                            hasContact = true;
                            contactPoint = nearestOnEdge;
                        }
                    }
                }
            }

            if (hasContact)
            {
                IndexedVector3 contactToCentre = sphereCenter - contactPoint;
                float distanceSqr = contactToCentre.LengthSquared();
                if (distanceSqr < (radiusWithThreshold) * (radiusWithThreshold))
                {
                    if (distanceSqr > MathUtil.SIMD_EPSILON)
                    {
                        float distance = (float)Math.Sqrt(distanceSqr);
                        resultNormal = contactToCentre;
                        resultNormal.Normalize();
                        point = contactPoint;
                        depth = -(radius - distance);
                    }
                    else
                    {
                        float distance = 0.0f;
                        resultNormal = normal;
                        point = contactPoint;
                        depth = -radius;
                    }
                    return true;
                }
            }
            resultNormal = new IndexedVector3(0, 1, 0);
            point = IndexedVector3.Zero;
            return false;
        }