C# CollisionObject.GetWorldTransform方法代码示例

 public void Initialise(CollisionObject body0, CollisionObject body1)
 {
     m_body0 = body0;
     m_body1 = body1;
     m_rootTransA = body0.GetWorldTransform();
     m_rootTransB = body1.GetWorldTransform();
     m_manifoldPtr = null;
 }

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            resultOut.SetPersistentManifold(m_manifoldPtr);

            SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
            SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;

            IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
            float len = diff.Length();
            float radius0 = sphere0.GetRadius();
            float radius1 = sphere1.GetRadius();

#if CLEAR_MANIFOLD
	        m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif

            ///iff distance positive, don't generate a new contact
            if (len > (radius0 + radius1))
            {
#if !CLEAR_MANIFOLD
                resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
                return;
            }
            ///distance (negative means penetration)
            float dist = len - (radius0 + radius1);

            IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
            if (len > MathUtil.SIMD_EPSILON)
            {
                normalOnSurfaceB = diff / len;
            }

            ///point on A (worldspace)
            ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
            ///point on B (worldspace)
            IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;

            /// report a contact. internally this will be kept persistent, and contact reduction is done

            resultOut.AddContactPoint(ref normalOnSurfaceB, ref pos1, dist);

#if !CLEAR_MANIFOLD
            resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD


        }

		//response  between two dynamic objects without friction, assuming 0 penetration depth
		public static float ResolveSingleCollision(
				RigidBody body1,
				CollisionObject colObj2,
				ref IndexedVector3 contactPositionWorld,
				ref IndexedVector3 contactNormalOnB,
				ContactSolverInfo solverInfo,
				float distance)
		{
			RigidBody body2 = RigidBody.Upcast(colObj2);


			IndexedVector3 normal = contactNormalOnB;

			IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
			IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;

			IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
			IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
			IndexedVector3 vel = vel1 - vel2;
			float rel_vel = normal.Dot(ref vel);

			float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
			float restitution = combinedRestitution * -rel_vel;

			float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
			float velocityError = -(1.0f + restitution) * rel_vel;// * damping;
			float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
			float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
			float relaxation = 1.0f;
			float jacDiagABInv = relaxation / (denom0 + denom1);

			float penetrationImpulse = positionalError * jacDiagABInv;
			float velocityImpulse = velocityError * jacDiagABInv;

			float normalImpulse = penetrationImpulse + velocityImpulse;
			normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;

			body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
			if (body2 != null)
			{
				body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
			}

			return normalImpulse;
		}

        public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
        {
            //check that the object isn't already added
            //btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());

            Debug.Assert(collisionObject != null);
            //Debug.Assert(!m_collisionObjects.Contains(collisionObject));

            if (m_collisionObjects.Contains(collisionObject))
            {
                return;
            }

            m_collisionObjects.Add(collisionObject);

            //calculate new AABB
            IndexedMatrix trans = collisionObject.GetWorldTransform();
            IndexedVector3 minAabb;
            IndexedVector3 maxAabb;

            collisionObject.GetCollisionShape().GetAabb(ref trans, out minAabb, out maxAabb);

            BroadphaseNativeTypes type = collisionObject.GetCollisionShape().GetShapeType();
            collisionObject.SetBroadphaseHandle(GetBroadphase().CreateProxy(
                ref minAabb,
                ref maxAabb,
                type,
                collisionObject,
                collisionFilterGroup,
                collisionFilterMask,
                m_dispatcher1, 0
                ));
        }

        public void UpdateSingleAabb(CollisionObject colObj)
        {
            IndexedVector3 minAabb;
            IndexedVector3 maxAabb;
            IndexedMatrix wt = colObj.GetWorldTransform();
            colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb);
            //need to increase the aabb for contact thresholds
            IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold);
            minAabb -= contactThreshold;
            maxAabb += contactThreshold;

            if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject())
	        {
		        IndexedVector3 minAabb2,maxAabb2;
		        colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2);
		        minAabb2 -= contactThreshold;
		        maxAabb2 += contactThreshold;
		        MathUtil.VectorMin(ref minAabb2,ref minAabb);
                MathUtil.VectorMax(ref maxAabb2, ref maxAabb);
            }

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld)
            {
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
            }


            IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface;

            //moving objects should be moderately sized, probably something wrong if not
            if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f))
            {
                bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
            }
            else
            {
                //something went wrong, investigate
                //this assert is unwanted in 3D modelers (danger of loosing work)
                colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);

                //static bool reportMe = true;
                bool reportMe = true;
                if (reportMe && m_debugDrawer != null)
                {
                    reportMe = false;
                    m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
                    m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
                    m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
                    m_debugDrawer.ReportErrorWarning("Thanks.\n");
                }
            }
        }

        public void GImpactVsConcave(
                          CollisionObject body0,
                          CollisionObject body1,
                          GImpactShapeInterface shape0,
                          ConcaveShape shape1, bool swapped)
        {
            GImpactTriangleCallback tricallback = new GImpactTriangleCallback();
            tricallback.algorithm = this;
            tricallback.body0 = body0;
            tricallback.body1 = body1;
            tricallback.gimpactshape0 = shape0;
            tricallback.swapped = swapped;
            tricallback.margin = shape1.GetMargin();

            //getting the trimesh AABB
            IndexedMatrix gimpactInConcaveSpace;

            gimpactInConcaveSpace = body1.GetWorldTransform().Inverse() * body0.GetWorldTransform();

            IndexedVector3 minAABB, maxAABB;
            shape0.GetAabb(gimpactInConcaveSpace, out minAABB, out maxAABB);

            shape1.ProcessAllTriangles(tricallback, ref minAABB, ref maxAABB);

        }

        public void GImpactVsCompoundshape(CollisionObject body0,
                          CollisionObject body1,
                          GImpactShapeInterface shape0,
                          CompoundShape shape1, bool swapped)
        {
            IndexedMatrix orgtrans1 = body1.GetWorldTransform();

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsCompoundshape");
            }


            int i = shape1.GetNumChildShapes();
            while (i-- != 0)
            {

                CollisionShape colshape1 = shape1.GetChildShape(i);
                IndexedMatrix childtrans1 = orgtrans1 * shape1.GetChildTransform(i);

                body1.SetWorldTransform(ref childtrans1);

                //collide child shape
                GImpactVsShape(body0, body1,
                              shape0, colshape1, swapped);


                //restore transforms
                body1.SetWorldTransform(ref orgtrans1);
            }

        }

        public void GImpactVsShape(CollisionObject body0,
                          CollisionObject body1,
                          GImpactShapeInterface shape0,
                          CollisionShape shape1, bool swapped)
        {
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
	        {
		        BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape");
	        }


            if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
            {
                GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;

                // check this...
                //int& part = swapped ? m_part1 : m_part0;
                //part = meshshape0.GetMeshPartCount();
                int part = meshshape0.GetMeshPartCount();

                while (part-- != 0)
                {

                    GImpactVsShape(body0,
                          body1,
                          meshshape0.GetMeshPart(part),
                          shape1, swapped);

                }
                if (swapped)
                {
                    m_part1 = part;
                }
                else
                {
                    m_part0 = part;
                }
                return;
            }

#if GIMPACT_VS_PLANE_COLLISION
	if(shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
		shape1.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
	{
		GImpactMeshShapePart shapepart = shape0 as GImpactMeshShapePart;
		StaticPlaneShape planeshape = shape1 as StaticPlaneShape;
        GImpactTrimeshpartVsPlaneCollision(body0, body1, shapepart, planeshape, swapped);
		return;
	}

#endif



            if (shape1.IsCompound())
            {
                CompoundShape compoundshape = shape1 as CompoundShape;
                GImpactVsCompoundshape(body0, body1, shape0, compoundshape, swapped);
                return;
            }
            else if (shape1.IsConcave())
            {
                ConcaveShape concaveshape = shape1 as ConcaveShape;
                GImpactVsConcave(body0, body1, shape0, concaveshape, swapped);
                return;
            }


            IndexedMatrix orgtrans0 = body0.GetWorldTransform();

            IndexedMatrix orgtrans1 = body1.GetWorldTransform();

            ObjectArray<int> collided_results = new ObjectArray<int>(64);

            GImpactVsShapeFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, collided_results);

            if (collided_results.Count == 0) return;


            shape0.LockChildShapes();

            using (GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
            {
                retriever0.Initialize(shape0);
                bool child_has_transform0 = shape0.ChildrenHasTransform();


                int i = collided_results.Count;

                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
                {
                    BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape [{0}]", collided_results.Count);
                }


                while (i-- != 0)
                {
                    int child_index = collided_results[i];
                    if (swapped)
                        m_triface1 = child_index;
//.........这里部分代码省略.........

        //! Collides two gimpact shapes
        /*!
        \pre shape0 and shape1 couldn't be btGImpactMeshShape objects
        */


        public void GImpactVsGImpact(CollisionObject body0,
                          CollisionObject body1,
                          GImpactShapeInterface shape0,
                          GImpactShapeInterface shape1)
        {
            if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
            {
                GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;
                m_part0 = meshshape0.GetMeshPartCount();

                while (m_part0-- != 0)
                {
                    GImpactVsGImpact(body0, body1, meshshape0.GetMeshPart(m_part0), shape1);
                }

                return;
            }

            if (shape1.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
            {
                GImpactMeshShape meshshape1 = shape1 as GImpactMeshShape;
                m_part1 = meshshape1.GetMeshPartCount();

                while (m_part1-- != 0)
                {

                    GImpactVsGImpact(body0, body1, shape0, meshshape1.GetMeshPart(m_part1));

                }

                return;
            }


            IndexedMatrix orgtrans0 = body0.GetWorldTransform();
            IndexedMatrix orgtrans1 = body1.GetWorldTransform();

            PairSet pairset = new PairSet();

            GImpactVsGImpactFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, pairset);

            if (pairset.Count == 0) return;


            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsGImpact [{0}]",pairset.Count);
            }

            if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
                shape1.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART)
            {
                GImpactMeshShapePart shapepart0 = shape0 as GImpactMeshShapePart;
                GImpactMeshShapePart shapepart1 = shape1 as GImpactMeshShapePart;
                //specialized function
#if BULLET_TRIANGLE_COLLISION
    CollideGjkTriangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
#else
                CollideSatTriangles(body0, body1, shapepart0, shapepart1, pairset, pairset.Count);
#endif

                return;
            }

            //general function

            shape0.LockChildShapes();
            shape1.LockChildShapes();

            using(GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
            using (GIM_ShapeRetriever retriever1 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
            {
                retriever0.Initialize(shape0);
                retriever1.Initialize(shape1);

                bool child_has_transform0 = shape0.ChildrenHasTransform();
                bool child_has_transform1 = shape1.ChildrenHasTransform();

                int i = pairset.Count;
                while (i-- != 0)
                {
                    GIM_PAIR pair = pairset[i];
                    m_triface0 = pair.m_index1;
                    m_triface1 = pair.m_index2;
                    CollisionShape colshape0 = retriever0.GetChildShape(m_triface0);
                    CollisionShape colshape1 = retriever1.GetChildShape(m_triface1);

                    if (child_has_transform0)
                    {
                        body0.SetWorldTransform(orgtrans0 * shape0.GetChildTransform(m_triface0));
                    }

                    if (child_has_transform1)
                    {
//.........这里部分代码省略.........

        protected void CollideSatTriangles(CollisionObject body0,
                          CollisionObject body1,
                          GImpactMeshShapePart shape0,
                          GImpactMeshShapePart shape1,
                          PairSet pairs, int pair_count)
        {
            IndexedMatrix orgtrans0 = body0.GetWorldTransform();
            IndexedMatrix orgtrans1 = body1.GetWorldTransform();

            PrimitiveTriangle ptri0 = new PrimitiveTriangle();
            PrimitiveTriangle ptri1 = new PrimitiveTriangle();

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideSatTriangles [{0}]", pair_count);
            }


            shape0.LockChildShapes();
            shape1.LockChildShapes();

            int pair_pointer = 0;

            while (pair_count-- != 0)
            {

                m_triface0 = pairs[pair_pointer].m_index1;
                m_triface1 = pairs[pair_pointer].m_index2;
                pair_pointer += 1;


                shape0.GetPrimitiveTriangle(m_triface0, ptri0);
                shape1.GetPrimitiveTriangle(m_triface1, ptri1);

#if TRI_COLLISION_PROFILING
		BulletGlobal.StartProfile("gim02_tri_time");
#endif

                ptri0.ApplyTransform(ref orgtrans0);
                ptri1.ApplyTransform(ref orgtrans1);


                //build planes
                ptri0.BuildTriPlane();
                ptri1.BuildTriPlane();
                // test conservative

                if (ptri0.OverlapTestConservative(ptri1))
                {
                    if (ptri0.FindTriangleCollisionClipMethod(ptri1, m_contact_data))
                    {

                        int j = m_contact_data.m_point_count;
                        while (j-- != 0)
                        {

                            AddContactPoint(body0, body1,
                                        m_contact_data.m_points[j],
                                        MathUtil.Vector4ToVector3(ref m_contact_data.m_separating_normal),
                                        -m_contact_data.m_penetration_depth);
                        }
                    }
                }

#if TRI_COLLISION_PROFILING
		BulletGlobals.StopProfile();
#endif

            }

            shape0.UnlockChildShapes();
            shape1.UnlockChildShapes();


        }

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject col0 = body0;
            CollisionObject col1 = body1;
            //resultOut = new ManifoldResult(body0, body1);
            BoxShape box0 = col0.GetCollisionShape() as BoxShape;
            BoxShape box1 = col1.GetCollisionShape() as BoxShape;

            //if (((String)col0.getUserPointer()).Contains("Box") &&
            //    ((String)col1.getUserPointer()).Contains("Box") )
            //{
            //    int ibreak = 0;
            //}
            /// report a contact. internally this will be kept persistent, and contact reduction is done
            resultOut.SetPersistentManifold(m_manifoldPtr);

#if !USE_PERSISTENT_CONTACTS	
	            m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS

            ClosestPointInput input = ClosestPointInput.Default();
            input.m_maximumDistanceSquared = float.MaxValue;
            input.m_transformA = body0.GetWorldTransform();
            input.m_transformB = body1.GetWorldTransform();

            BoxBoxDetector.GetClosestPoints(box0,box1,ref input, resultOut, dispatchInfo.getDebugDraw(), false);

#if USE_PERSISTENT_CONTACTS
            //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
#endif //USE_PERSISTENT_CONTACTS
        }

		private static void ApplyAnisotropicFriction(CollisionObject colObj, ref IndexedVector3 frictionDirection)
		{
			if (colObj != null && colObj.HasAnisotropicFriction())
			{
				// transform to local coordinates
                IndexedVector3 loc_lateral = frictionDirection * colObj.GetWorldTransform()._basis;
				IndexedVector3 friction_scaling = colObj.GetAnisotropicFriction();
				//apply anisotropic friction
				loc_lateral *= friction_scaling;
				// ... and transform it back to global coordinates
                frictionDirection = colObj.GetWorldTransform()._basis * loc_lateral;
			}
		}

        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 override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.getPersistentManifold().clearManifold();


            ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;

            IndexedVector3 normalOnB = IndexedVector3.Zero;
            IndexedVector3 pointOnBWorld = IndexedVector3.Zero;

            {
                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

                    {
                        input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();

                    gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
                    if (BulletGlobals.g_streamWriter != null)
                    {
                        BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
                    }
#endif                    
                }
                //BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
                //btVector3 v0,v1;
                //btVector3 sepNormalWorldSpace;

            }

            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }

        }

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            //resultOut = new ManifoldResult();
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.GetPersistentManifold().ClearManifold();


            ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
            IndexedVector3 normalOnB;
            IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
            if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
            {
                CapsuleShape capsuleA = min0 as CapsuleShape;
                CapsuleShape capsuleB = min1 as CapsuleShape;
                //IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
                //IndexedVector3 localScalingB = capsuleB.GetLocalScaling();

                float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
                    capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
                    body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);

                if (dist < threshold)
                {
                    Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
                    resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
                }
                resultOut.RefreshContactPoints();
                return;
            }
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER



#if USE_SEPDISTANCE_UTIL2
        	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
            {
                m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
            }

	        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2

            {


                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

#if USE_SEPDISTANCE_UTIL2
	        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	        {
		        input.m_maximumDistanceSquared = float.MaxValue;
	        } 
            else
#endif //USE_SEPDISTANCE_UTIL2
                    {
                        input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();


                    if (min0.IsPolyhedral() && min1.IsPolyhedral())
                    {


                        DummyResult dummy = new DummyResult();


                        PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
                        PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
                        if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
                        {
                            float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                            float minDist = float.MinValue;
                            IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
                            bool foundSepAxis = true;

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