public override void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
                return;

            resultOut.PersistentManifold = m_manifoldPtr;

            SphereShape sphere0 = (SphereShape)col0.CollisionShape;
            SphereShape sphere1 = (SphereShape)col1.CollisionShape;

            btVector3 diff = col0.WorldTransform.Origin - col1.WorldTransform.Origin;
            float len = diff.Length;
            float radius0 = sphere0.Radius;
            float radius1 = sphere1.Radius;

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

            btVector3 normalOnSurfaceB = new btVector3(1, 0, 0);
            if (len > BulletGlobal.SIMD_EPSILON)
            {
                //normalOnSurfaceB = diff / len;
                btVector3.Divide(ref diff, len, out normalOnSurfaceB);
            }

            ///point on A (worldspace)
            ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
            ///point on B (worldspace)
            btVector3 pos1;// = col1.WorldTransform.Origin + radius1 * normalOnSurfaceB;
            {
                btVector3 temp;
                btVector3.Multiply(ref normalOnSurfaceB, radius1, out temp);
                btVector3.Add(ref col1.WorldTransform.Origin, ref temp, out pos1);
            }
            /// 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

        }
示例#2
0
        public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
                return;

            CollisionObject sphereObj = m_isSwapped ? body1 : body0;
            CollisionObject boxObj = m_isSwapped ? body0 : body1;


            SphereShape sphere0 = (SphereShape)sphereObj.CollisionShape;

            //btVector3 normalOnSurfaceB;
            btVector3 pOnBox=btVector3.Zero, pOnSphere=btVector3.Zero;
            btVector3 sphereCenter = sphereObj.WorldTransform.Origin;
            float radius = sphere0.Radius;

            float dist = getSphereDistance(boxObj, ref pOnBox, ref pOnSphere, sphereCenter, radius);

            resultOut.PersistentManifold = m_manifoldPtr;

            if (dist < BulletGlobal.SIMD_EPSILON)
            {
                btVector3 normalOnSurfaceB;// = (pOnBox - pOnSphere).normalize();
                (pOnBox - pOnSphere).normalize(out normalOnSurfaceB);
                /// report a contact. internally this will be kept persistent, and contact reduction is done

                resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnBox, dist);

            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.NumContacts != 0)
                {
                    resultOut.refreshContactPoints();
                }
            }
        }
示例#3
0
        public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1);
                m_ownManifold = true;
            }
            resultOut.PersistentManifold = m_manifoldPtr;

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


            ConvexShape min0 = (ConvexShape)(body0.CollisionShape);
            ConvexShape min1 = (ConvexShape)(body1.CollisionShape);

            btVector3 normalOnB;
            btVector3 pointOnBWorld;
            if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
            {
                CapsuleShape capsuleA = (CapsuleShape)min0;
                CapsuleShape capsuleB = (CapsuleShape)min1;
                btVector3 localScalingA = capsuleA.LocalScaling;
                btVector3 localScalingB = capsuleB.LocalScaling;

                float threshold = m_manifoldPtr.ContactBreakingThreshold;

                float dist = capsuleCapsuleDistance(out normalOnB, out	pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius,
                    capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis,
                    body0.WorldTransform, body1.WorldTransform, threshold);

                if (dist < threshold)
                {
                    Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON));
                    resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist);
                }
                resultOut.refreshContactPoints();
                return;
            }


#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;

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

#if USE_SEPDISTANCE_UTIL2
	            if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	            {
		            input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
	            } else
#endif //USE_SEPDISTANCE_UTIL2
                {
                    input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold;
                    input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                }

                //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                input.m_transformA = body0.WorldTransform;
                input.m_transformB = body1.WorldTransform;

                gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw);

#if USE_SEPDISTANCE_UTIL2
	            btScalar sepDist = 0.f;
	            if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	            {
		            sepDist = gjkPairDetector.getCachedSeparatingDistance();
		            if (sepDist>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.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
                {

                    int i;
                    btVector3 v0, v1;
                    btVector3 sepNormalWorldSpace;

                    //sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
                    gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace);
                    btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);


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

                    btTransform unPerturbedTransform;
                    if (perturbeA)
                    {
                        unPerturbedTransform = input.m_transformA;
                    }
                    else
                    {
                        unPerturbedTransform = input.m_transformB;
                    }

                    for (i = 0; i < m_numPerturbationIterations; i++)
                    {
                        if (v0.Length2 > BulletGlobal.SIMD_EPSILON)
                        {
                            btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
                            float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations);
                            btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle);


                            if (perturbeA)
                            {
                                #region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis;
                                {
                                    btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
                                    btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis);
                                }
                                #endregion
                                input.m_transformB = body1.WorldTransform;
#if DEBUG
                                dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
                            }
                            else
                            {
                                input.m_transformA = body0.WorldTransform;
                                #region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis;
                                {
                                    btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
                                    btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis);
                                }
                                #endregion
#if DEBUG
                                dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f);
#endif
                            }
                            PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
                            gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw);

                        }

                    }
                }


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



            }

            if (m_ownManifold)
            {
                resultOut.refreshContactPoints();
            }
        }
        public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
                return;

            CollisionObject convexObj = m_isSwapped ? body1 : body0;
            CollisionObject planeObj = m_isSwapped ? body0 : body1;

            ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
            StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;

            btVector3 planeNormal = planeShape.PlaneNormal;
            //const btScalar& planeConstant = planeShape->getPlaneConstant();

            //first perform a collision query with the non-perturbated collision objects
            {
                btQuaternion rotq = new btQuaternion(0, 0, 0, 1);
                collideSingleContact(rotq, body0, body1, dispatchInfo, ref resultOut);
            }

            if (resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
            {
                btVector3 v0, v1;
                btVector3.PlaneSpace1(ref planeNormal, out v0, out v1);
                //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
                float perturbeAngle;
                float radius = convexShape.getAngularMotionDisc();
                perturbeAngle = PersistentManifold.gContactBreakingThreshold / radius;
                if (perturbeAngle > angleLimit)
                    perturbeAngle = angleLimit;

                btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
                for (int i = 0; i < m_numPerturbationIterations; i++)
                {
                    float iterationAngle = (float)i * (BulletGlobal.SIMD_2_PI / (float)m_numPerturbationIterations);
                    btQuaternion rotq = new btQuaternion(planeNormal, iterationAngle);
                    collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0, body1, dispatchInfo, ref resultOut);
                }
                
            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.NumContacts != 0)
                {
                    resultOut.refreshContactPoints();
                }
            }
        }