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 = (ConvexShape)(body0.GetCollisionShape());
	        ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());

	        Vector3  normalOnB = Vector3.Zero;
	        Vector3  pointOnBWorld = Vector3.Zero;

	        {
		        ClosestPointInput input = new ClosestPointInput();

		        GjkPairDetector	gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
		        //TODO: if (dispatchInfo.m_useContinuous)
		        gjkPairDetector.SetMinkowskiA(min0);
		        gjkPairDetector.SetMinkowskiB(min1);

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

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

		        gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);

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

                //btVector3 v0,v1;
                //btVector3 sepNormalWorldSpace;

	        }

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

        }
예제 #2
0
        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 = (ConvexShape)(body0.GetCollisionShape());
	        ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
        	Vector3  normalOnB = Vector3.Up;
            Vector3  pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
            if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
	        {
		        CapsuleShape capsuleA = (CapsuleShape) min0;
		        CapsuleShape capsuleB = (CapsuleShape) min1;
		        Vector3 localScalingA = capsuleA.GetLocalScaling();
		        Vector3 localScalingB = capsuleB.GetLocalScaling();
        		
		        float threshold = m_manifoldPtr.GetContactBreakingThreshold();

		        float dist = CapsuleCapsuleDistance(ref normalOnB,ref 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 = new ClosestPointInput();

	        GjkPairDetector	gjkPairDetector = new GjkPairDetector(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.Margin + min1.Margin + 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();

	        gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
#if USE_SEPDISTANCE_UTIL2
	float sepDist = 0.f;
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		sepDist = gjkPairDetector.getCachedSeparatingDistance();
		if (sepDist>MathUtil.SIMD_EPSILON)
		{
			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
			//now perturbe directions to get multiple contact points
		}
	}
#endif //USE_SEPDISTANCE_UTIL2

	        //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
        	
	        //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
	        if (m_numPerturbationIterations > 0 &&  resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
	        {
                Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;

                Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
                sepNormalWorldSpace.Normalize();
                TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref v1);

		        bool perturbeA = true;
		        const float angleLimit = 0.125f * MathUtil.SIMD_PI;
		        float perturbeAngle;
		        float radiusA = min0.GetAngularMotionDisc();
		        float radiusB = min1.GetAngularMotionDisc();
		        if (radiusA < radiusB)
		        {
			        perturbeAngle = BulletGlobals.gContactBreakingThreshold /radiusA;
			        perturbeA = true;
		        } 
                else
		        {
                    perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
			        perturbeA = false;
		        }
                if (perturbeAngle > angleLimit)
                {
                    perturbeAngle = angleLimit;
                }

		        Matrix unPerturbedTransform = Matrix.Identity;
		        if (perturbeA)
		        {
			        unPerturbedTransform = input.m_transformA;
		        } 
                else
		        {
			        unPerturbedTransform = input.m_transformB;
		        }
        		
		        for (int i=0;i<m_numPerturbationIterations;i++)
		        {
                    if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
                    {

                        Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
                        float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                        Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);

                        if (perturbeA)
                        {
                            //input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
                            Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
                            input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.GetWorldTransform());
                            input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
				        dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA,10.0f);
#endif //DEBUG_CONTACTS
                        }
                        else
                        {
                            input.m_transformA = body0.GetWorldTransform();
                            //input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
                            Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
                            input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#if DEBUG_CONTACTS
				        dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB,10.0f);
#endif
                        }

                        PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
                        gjkPairDetector.GetClosestPoints(input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
                    }
        			
        			
		        }
	        }

        	

        #if USE_SEPDISTANCE_UTIL2
	        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
	        {
		        m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
	        }
        #endif //USE_SEPDISTANCE_UTIL2


	        }

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