Esempio n. 1
0
        private void PreallocateChildAlgorithms(CollisionObject body0, CollisionObject body1)
        {
            CollisionObject colObj   = m_isSwapped ? body1 : body0;
            CollisionObject otherObj = m_isSwapped ? body0 : body1;

            Debug.Assert(colObj.GetCollisionShape().IsCompound());

            CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());

            int numChildren = compoundShape.GetNumChildShapes();
            int i;

            //m_childCollisionAlgorithms.resize(numChildren);
            m_childCollisionAlgorithms.Clear();
            for (i = 0; i < numChildren; i++)
            {
                if (compoundShape.GetDynamicAabbTree() != null)
                {
                    m_childCollisionAlgorithms.Add(null);
                }
                else
                {
                    CollisionShape tmpShape   = colObj.GetCollisionShape();
                    CollisionShape childShape = compoundShape.GetChildShape(i);
                    colObj.InternalSetTemporaryCollisionShape(childShape);
                    CollisionAlgorithm ca = m_dispatcher.FindAlgorithm(colObj, otherObj, m_sharedManifold);
                    m_childCollisionAlgorithms.Add(ca);
                    colObj.InternalSetTemporaryCollisionShape(tmpShape);
                }
            }
        }
Esempio n. 2
0
        public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            IndexedMatrix convexFromTrans = convexFromWorld;
            IndexedMatrix convexToTrans   = convexToWorld;

            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            IndexedVector3 linVel, angVel;

            TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);

            // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
            IndexedMatrix R = IndexedMatrix.Identity;

            R.SetRotation(convexFromTrans.GetRotation());


            castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);

            /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
            // do a ray-shape query using convexCaster (CCD)
            for (int i = 0; i < m_overlappingObjects.Count; i++)
            {
                CollisionObject collisionObject = m_overlappingObjects[i];
                //only perform raycast if filterMask matches
                if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
                {
                    //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                    IndexedVector3 collisionObjectAabbMin;
                    IndexedVector3 collisionObjectAabbMax;
                    IndexedMatrix  t = collisionObject.GetWorldTransform();
                    collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
                    AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
                    float          hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
                    IndexedVector3 hitNormal;

                    if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
                    {
                        IndexedMatrix wt = collisionObject.GetWorldTransform();
                        CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
                                                         collisionObject,
                                                         collisionObject.GetCollisionShape(),
                                                         ref wt,
                                                         resultCallback,
                                                         allowedCcdPenetration);
                    }
                }
            }
        }
Esempio n. 3
0
        public virtual void Initialize(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool isSwapped)
        {
            base.Initialize(ci, body0, body1);
            m_isSwapped      = isSwapped;
            m_sharedManifold = ci.GetManifold();
            m_ownsManifold   = false;
            CollisionObject colObj = m_isSwapped ? body1 : body0;

            Debug.Assert(colObj.GetCollisionShape().IsCompound());
            CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());

            m_compoundShapeRevision = compoundShape.GetUpdateRevision();
            PreallocateChildAlgorithms(body0, body1);
        }
Esempio n. 4
0
        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            resultOut = null;
            CollisionObject colObj   = m_isSwapped ? body1 : body0;
            CollisionObject otherObj = m_isSwapped ? body0 : body1;

            Debug.Assert(colObj.GetCollisionShape().IsCompound());

            CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());

            //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
            //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
            //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
            //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
            //then use each overlapping node AABB against Tree0
            //and vise versa.

            float hitFraction = 1f;

            int           numChildren = m_childCollisionAlgorithms.Count;
            IndexedMatrix orgTrans;
            float         frac;

            for (int i = 0; i < numChildren; i++)
            {
                //temporarily exchange parent btCollisionShape with childShape, and recurse
                CollisionShape childShape = compoundShape.GetChildShape(i);

                //backup
                orgTrans = colObj.GetWorldTransform();

                IndexedMatrix childTrans         = compoundShape.GetChildTransform(i);
                IndexedMatrix newChildWorldTrans = orgTrans * childTrans;

                colObj.SetWorldTransform(ref newChildWorldTrans);

                CollisionShape tmpShape = colObj.GetCollisionShape();
                colObj.InternalSetTemporaryCollisionShape(childShape);

                frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
                if (frac < hitFraction)
                {
                    hitFraction = frac;
                }
                //revert back
                colObj.InternalSetTemporaryCollisionShape(tmpShape);
                colObj.SetWorldTransform(ref orgTrans);
            }
            return(hitFraction);
        }
        protected void ConvexVsConvexCollision(CollisionObject body0,
                                               CollisionObject body1,
                                               CollisionShape shape0,
                                               CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

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


            m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
            m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

            CheckConvexAlgorithm(body0, body1);
            m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);
        }
        //public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
        //{
        //    CollisionObject convexBody = m_isSwapped ? body1 : body0;
        //    CollisionObject triBody = m_isSwapped ? body0 : body1;

        //    if (triBody.getCollisionShape().isConcave())
        //    {
        //        CollisionObject triOb = triBody;
        //        ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());

        //        if (convexBody.getCollisionShape().isConvex())
        //        {
        //            float collisionMarginTriangle = concaveShape.getMargin();

        //            resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
        //            m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);

        //            //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
        //            //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr);

        //            m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);

        //            IndexedVector3 min = m_convexTriangleCallback.getAabbMin();
        //            IndexedVector3 max = m_convexTriangleCallback.getAabbMax();

        //            concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );

        //            resultOut.refreshContactPoints();

        //        }
        //    }
        //}


        //public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        //{
        //    CollisionObject convexbody = m_isSwapped ? body1 : body0;
        //    CollisionObject triBody = m_isSwapped ? body0 : body1;

        //    //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)

        //    //only perform CCD above a certain threshold, this prevents blocking on the long run
        //    //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
        //    float squareMot0 = (convexbody.getInterpolationWorldTransform()._origin - convexbody.getWorldTransform()._origin).LengthSquared();
        //    if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
        //    {
        //        return 1f;
        //    }

        //    //const IndexedVector3& from = convexbody.m_worldTransform._origin;
        //    //IndexedVector3 to = convexbody.m_interpolationWorldTransform._origin;
        //    //todo: only do if the motion exceeds the 'radius'

        //    //IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
        //    //IndexedMatrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
        //    //IndexedMatrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());

        //    IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
        //    IndexedMatrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
        //    IndexedMatrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());

        //    if (triBody.getCollisionShape().isConcave())
        //    {
        //        IndexedVector3 rayAabbMin = convexFromLocal._origin;
        //        MathUtil.vectorMin(convexToLocal._origin, ref rayAabbMin);
        //        IndexedVector3 rayAabbMax = convexFromLocal._origin;
        //        MathUtil.vectorMax(convexToLocal._origin,ref rayAabbMax);
        //        float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
        //        rayAabbMin -= new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
        //        rayAabbMax += new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);

        //        float curHitFraction = 1.0f; //is this available?
        //        LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
        //            convexbody.getCcdSweptSphereRadius(),curHitFraction);

        //        raycastCallback.m_hitFraction = convexbody.getHitFraction();

        //        CollisionObject concavebody = triBody;

        //        ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();

        //        if (triangleMesh != null)
        //        {
        //            triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
        //        }

        //        if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
        //        {
        //            convexbody.setHitFraction( raycastCallback.m_hitFraction);
        //            float result = raycastCallback.m_hitFraction;
        //            raycastCallback.cleanup();
        //            return result;
        //        }

        //        raycastCallback.cleanup();
        //    }
        //    return 1f;
        //}

        public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //fixme

            CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
            CollisionObject triBody    = m_isSwapped ? bodyA : bodyB;

            if (triBody.GetCollisionShape().IsConcave())
            {
                CollisionObject triOb        = triBody;
                ConcaveShape    concaveShape = triOb.GetCollisionShape() as ConcaveShape;

                if (convexBody.GetCollisionShape().IsConvex())
                {
                    float collisionMarginTriangle = concaveShape.GetMargin();

                    resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
                    m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);

                    //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
                    //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);

                    m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
                    IndexedVector3 min = m_convexTriangleCallback.GetAabbMin();
                    IndexedVector3 max = m_convexTriangleCallback.GetAabbMax();

                    concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min, ref max);
                    resultOut.RefreshContactPoints();
                }
            }
        }
        protected void ShapeVsShapeCollision(
            CollisionObject body0,
            CollisionObject body1,
            CollisionShape shape0,
            CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

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



            {
                CollisionAlgorithm algor = NewAlgorithm(body0, body1);
                // post :	checkManifold is called

                m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
                m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

                algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

                m_dispatcher.FreeCollisionAlgorithm(algor);
            }

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);
        }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //resultOut = new ManifoldResult();
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject sphereObj = m_swapped ? body1 : body0;
            CollisionObject triObj    = m_swapped ? body0 : body1;

            SphereShape   sphere   = sphereObj.GetCollisionShape() as SphereShape;
            TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;

            /// report a contact. internally this will be kept persistent, and contact reduction is done
            resultOut.SetPersistentManifold(m_manifoldPtr);
            using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
            {
                detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
                ClosestPointInput input = ClosestPointInput.Default();
                input.m_maximumDistanceSquared = float.MaxValue;
                sphereObj.GetWorldTransform(out input.m_transformA);
                triObj.GetWorldTransform(out input.m_transformB);

                bool swapResults = m_swapped;

                detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);

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

        }
Esempio n. 10
0
        public virtual PersistentManifold GetNewManifold(CollisionObject b0, CollisionObject b1)
        {
            gNumManifold++;

            CollisionObject body0 = b0;
            CollisionObject body1 = b1;

            //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)

            float contactBreakingThreshold = ((m_dispatcherFlags & DispatcherFlags.CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) > 0) ?
                                             Math.Min(body0.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold), body1.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold))
                : BulletGlobals.gContactBreakingThreshold;

            float contactProcessingThreshold = Math.Min(body0.GetContactProcessingThreshold(), body1.GetContactProcessingThreshold());

            // nothing in our pool so create a new one and return it.
            // need a way to flush the pool ideally
            PersistentManifold manifold = BulletGlobals.PersistentManifoldPool.Get();

            manifold.Initialise(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);

            manifold.m_index1a = m_manifoldsPtr.Count;


            m_manifoldsPtr.Add(manifold);
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
            {
                BulletGlobals.g_streamWriter.WriteLine("GetNewManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
            }
#endif

            return(manifold);
        }
        public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
            CollisionObject triBody    = m_isSwapped ? bodyA : bodyB;


            //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)

            //only perform CCD above a certain threshold, this prevents blocking on the long run
            //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
            float squareMot0 = (convexbody.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
            {
                return(1);
            }

            //IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
            IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();

            IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
            IndexedMatrix convexToLocal   = triInv * convexbody.GetInterpolationWorldTransform();

            if (triBody.GetCollisionShape().IsConcave())
            {
                IndexedVector3 rayAabbMin = convexFromLocal._origin;
                MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
                IndexedVector3 rayAabbMax = convexFromLocal._origin;
                MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
                IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
                rayAabbMin -= ccdRadius0;
                rayAabbMax += ccdRadius0;

                float curHitFraction = 1f; //is this available?
                using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
                {
                    raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
                                               convexbody.GetCcdSweptSphereRadius(), curHitFraction);

                    raycastCallback.m_hitFraction = convexbody.GetHitFraction();

                    CollisionObject concavebody = triBody;

                    ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;

                    if (triangleMesh != null)
                    {
                        triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
                    }

                    if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
                    {
                        convexbody.SetHitFraction(raycastCallback.m_hitFraction);
                        return(raycastCallback.m_hitFraction);
                    }
                }
            }
            return(1);
        }
        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 void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            m_dispatchInfoPtr         = dispatchInfo;
            m_collisionMarginTriangle = collisionMarginTriangle;
            m_resultOut = resultOut;

            //recalc aabbs
            //IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
            IndexedMatrix  convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
            CollisionShape convexShape           = m_convexBody.GetCollisionShape();

            convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
            float          extraMargin = collisionMarginTriangle;
            IndexedVector3 extra       = new IndexedVector3(extraMargin);

            m_aabbMax += extra;
            m_aabbMin -= extra;
        }
Esempio n. 14
0
        public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
        {
            //aabb filter is already applied!
            CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();

            ci.SetDispatcher(m_dispatcher);

            CollisionObject ob = m_triBody as CollisionObject;

            ///debug drawing of the overlapping triangles
            ///
#if false
            if (m_dispatchInfoPtr != null && m_dispatchInfoPtr.getDebugDraw() != null && ((m_dispatchInfoPtr.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawWireframe) > 0))
            {
                IndexedVector3 color = new IndexedVector3(1, 1, 0);
                IndexedMatrix  tr    = ob.GetWorldTransform();

                IndexedVector3[] transformedTriangles = new IndexedVector3[3];
                IndexedVector3.Transform(triangle, ref tr, transformedTriangles);

                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[0], ref transformedTriangles[1], ref color);
                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[1], ref transformedTriangles[2], ref color);
                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[2], ref transformedTriangles[0], ref color);
            }
#endif
            if (m_convexBody.GetCollisionShape().IsConvex())
            {
                using (TriangleShape tm = BulletGlobals.TriangleShapePool.Get())
                {
                    tm.Initialize(ref triangle[0], ref triangle[1], ref triangle[2]);
                    tm.SetMargin(m_collisionMarginTriangle);

                    CollisionShape tmpShape = ob.GetCollisionShape();
                    ob.InternalSetTemporaryCollisionShape(tm);

                    CollisionAlgorithm colAlgo = ci.GetDispatcher().FindAlgorithm(m_convexBody, m_triBody, m_manifoldPtr);
                    ///this should use the btDispatcher, so the actual registered algorithm is used
                    //		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);

                    if (m_resultOut.GetBody0Internal() == m_triBody)
                    {
                        m_resultOut.SetShapeIdentifiersA(partId, triangleIndex);
                    }
                    else
                    {
                        m_resultOut.SetShapeIdentifiersB(partId, triangleIndex);
                    }

                    colAlgo.ProcessCollision(m_convexBody, m_triBody, m_dispatchInfoPtr, m_resultOut);
                    ci.GetDispatcher().FreeCollisionAlgorithm(colAlgo);
                    colAlgo = null;

                    ob.InternalSetTemporaryCollisionShape(tmpShape);
                }
            }
        }
Esempio n. 15
0
        public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
        {
            CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);

            ci.SetManifold(sharedManifold);
            int index1 = (int)body0.GetCollisionShape().GetShapeType();
            int index2 = (int)body1.GetCollisionShape().GetShapeType();

            return(m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1));
        }
        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


        }
Esempio n. 17
0
        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
        }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ClearCache();

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


            m_resultOut    = resultOut;
            m_dispatchInfo = dispatchInfo;
            GImpactShapeInterface gimpactshape0;
            GImpactShapeInterface gimpactshape1;

            if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;

                if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
                {
                    gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                    GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
                }
                else
                {
                    GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
                }
            }
            else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
            }
        }
        public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            CollisionObject convexObj = m_isSwapped ? body1 : body0;
            CollisionObject planeObj  = m_isSwapped ? body0 : body1;

            ConvexShape      convexShape = convexObj.GetCollisionShape() as ConvexShape;
            StaticPlaneShape planeShape  = planeObj.GetCollisionShape() as StaticPlaneShape;

            bool           hasCollision  = false;
            IndexedVector3 planeNormal   = planeShape.GetPlaneNormal();
            float          planeConstant = planeShape.GetPlaneConstant();

            IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
            IndexedMatrix convexInPlaneTrans   = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;

            //now perturbe the convex-world transform

            convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);

            IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();;

            IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);

            IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
            float          distance   = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant);

            IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal);
            IndexedVector3 vtxInPlaneWorld     = planeObj.GetWorldTransform() * vtxInPlaneProjected;

            hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();

            resultOut.SetPersistentManifold(m_manifoldPtr);
            if (hasCollision)
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
                IndexedVector3 pOnB             = vtxInPlaneWorld;
                resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
            }
        }
        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
        }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject col0 = body0;
            CollisionObject col1 = body1;
            Box2dShape      box0 = (Box2dShape)col0.GetCollisionShape();
            Box2dShape      box1 = (Box2dShape)col1.GetCollisionShape();

            resultOut.SetPersistentManifold(m_manifoldPtr);

            B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());

            //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
        }
Esempio n. 22
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //(void)dispatchInfo;
            //(void)resultOut;
            if (m_manifoldPtr == null)
            {
                return;
            }

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

            IndexedVector3 pOnBox = new IndexedVector3();;

            IndexedVector3 normalOnSurfaceB   = new IndexedVector3();
            float          penetrationDepth   = 0f;
            IndexedVector3 sphereCenter       = sphereObj.GetWorldTransform()._origin;
            SphereShape    sphere0            = sphereObj.GetCollisionShape() as SphereShape;
            float          radius             = sphere0.GetRadius();
            float          maxContactDistance = m_manifoldPtr.GetContactBreakingThreshold();

            resultOut.SetPersistentManifold(m_manifoldPtr);

            if (GetSphereDistance(boxObj, ref pOnBox, ref normalOnSurfaceB, ref penetrationDepth, sphereCenter, radius, maxContactDistance))
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.GetNumContacts() > 0)
                {
                    resultOut.RefreshContactPoints();
                }
            }
        }
Esempio n. 23
0
        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
                ));
        }
Esempio n. 24
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");
                }
            }
        }
Esempio n. 25
0
        /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
        public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
                          CollisionObject collisionObject, CollisionShape collisionShape,
                          ref IndexedMatrix colObjWorldTransform,
                          ConvexResultCallback resultCallback, float allowedPenetration)
        {
            if (collisionShape.IsConvex())
            {

                BulletGlobals.StartProfile("convexSweepConvex");
                CastResult castResult = BulletGlobals.CastResultPool.Get();
                castResult.m_allowedPenetration = allowedPenetration;
                castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??

                ConvexShape convexShape = collisionShape as ConvexShape;
                VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
                GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();

                ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
                convexCaster1.Initialize(castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver);
                //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
                //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);

                IConvexCast castPtr = convexCaster1;

                if (castPtr.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
                {
                    //add hit
                    if (castResult.m_normal.LengthSquared() > 0.0001f)
                    {
                        if (castResult.m_fraction < resultCallback.m_closestHitFraction)
                        {
                            castResult.m_normal.Normalize();
                            LocalConvexResult localConvexResult = new LocalConvexResult
                                        (
                                            collisionObject,
                                            //null, // updated to allow different ctor on struct
                                            ref castResult.m_normal,
                                            ref castResult.m_hitPoint,
                                            castResult.m_fraction
                                        );

                            bool normalInWorldSpace = true;
                            resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);

                        }
                    }
                }
                BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
                BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
                BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
                castResult.Cleanup();
                BulletGlobals.StopProfile();
            }
            else
            {
				if (collisionShape.IsConcave())
				{
					if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
					{
						BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
						BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
						IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
                        IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
                        IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
						// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation

						IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));

                        using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
                        {
                            tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
                            tccb.m_hitFraction = resultCallback.m_closestHitFraction;
                            tccb.m_allowedPenetration = allowedPenetration;

                            IndexedVector3 boxMinLocal;
                            IndexedVector3 boxMaxLocal;
                            castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
                            triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
                        }
						BulletGlobals.StopProfile();
					}
					else
					{
						if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
						{
                            CastResult castResult = BulletGlobals.CastResultPool.Get();
							castResult.m_allowedPenetration = allowedPenetration;
							castResult.m_fraction = resultCallback.m_closestHitFraction;
							StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
							ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);

							if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
							{
								//add hit
								if (castResult.m_normal.LengthSquared() > 0.0001f)
								{
									if (castResult.m_fraction < resultCallback.m_closestHitFraction)
									{
										castResult.m_normal.Normalize();
										LocalConvexResult localConvexResult = new LocalConvexResult
											(
											collisionObject,
                                            //null, // updated to allow different ctor on struct
											ref castResult.m_normal,
											ref castResult.m_hitPoint,
											castResult.m_fraction
											);

										bool normalInWorldSpace = true;
										resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
									}
								}
							}
                            castResult.Cleanup();
						}
						else
						{
							BulletGlobals.StartProfile("convexSweepConcave");
							ConcaveShape concaveShape = (ConcaveShape)collisionShape;
                            IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
                            IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
                            IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
                            // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                            IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));

                            using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
                            {
                                tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
                                tccb.m_allowedPenetration = allowedPenetration;
                                IndexedVector3 boxMinLocal;
                                IndexedVector3 boxMaxLocal;
                                castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);

							IndexedVector3 rayAabbMinLocal = convexFromLocal;
							MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
							//rayAabbMinLocal.setMin(convexToLocal);
							IndexedVector3 rayAabbMaxLocal = convexFromLocal;
							//rayAabbMaxLocal.setMax(convexToLocal);
							MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);

                                rayAabbMinLocal += boxMinLocal;
                                rayAabbMaxLocal += boxMaxLocal;
                                concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
                                BulletGlobals.StopProfile();
                            }
						}
					}
				}
				else
				{
					///@todo : use AABB tree or other BVH acceleration structure!
					if (collisionShape.IsCompound())
					{
						BulletGlobals.StartProfile("convexSweepCompound");
						CompoundShape compoundShape = (CompoundShape)collisionShape;
						for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
						{
							IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
							CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
							IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
							// replace collision shape so that callback can determine the triangle
							CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
							collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);

							LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
							my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;


							ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
								collisionObject,
								childCollisionShape,
								ref childWorldTrans,
								my_cb, allowedPenetration);
							// restore
							collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
						}
						BulletGlobals.StopProfile();
					}
				}
            }
        }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ClearCache();

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


            m_resultOut = resultOut;
            m_dispatchInfo = dispatchInfo;
            GImpactShapeInterface gimpactshape0;
            GImpactShapeInterface gimpactshape1;

            if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;

                if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
                {
                    gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                    GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
                }
                else
                {
                    GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
                }

            }
            else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
            }

        }
        protected void ConvexVsConvexCollision(CollisionObject body0,
                          CollisionObject body1,
                          CollisionShape shape0,
                          CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

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


            m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
            m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

            CheckConvexAlgorithm(body0, body1);
            m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);

        }
        protected void ShapeVsShapeCollision(
                          CollisionObject body0,
                          CollisionObject body1,
                          CollisionShape shape0,
                          CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

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



            {
                CollisionAlgorithm algor = NewAlgorithm(body0, body1);
                // post :	checkManifold is called

                m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
                m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

                algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

                m_dispatcher.FreeCollisionAlgorithm(algor);
            }

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);

        }
Esempio n. 29
0
        public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
        {
            CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
            ci.SetManifold(sharedManifold);
            int index1 = (int)body0.GetCollisionShape().GetShapeType();
            int index2 = (int)body1.GetCollisionShape().GetShapeType();

            return m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1);
        }
Esempio n. 30
0
        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            ///body0.m_worldTransform,
            float resultFraction = 1.0f;


            float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
            float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
                squareMot1 < body1.GetCcdSquareMotionThreshold())
            {
                return(resultFraction);
            }

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)


            /// Convex0 against sphere for Convex1
            {
                ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;

                SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
                sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(convex0, sphere1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere1);
                    result.Cleanup();
                }
            }

            /// Sphere (for convex0) against Convex1
            {
                ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;

                SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
                sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(sphere0, convex1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere0);
                    result.Cleanup();
                }
            }

            return(resultFraction);
        }
Esempio n. 31
0
        public void ProcessChildShape(CollisionShape childShape, int index)
        {
            Debug.Assert(index >= 0);
            CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());

            Debug.Assert(index < compoundShape.GetNumChildShapes());

            //backup
            IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
            IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
            IndexedMatrix childTrans            = compoundShape.GetChildTransform(index);
            IndexedMatrix newChildWorldTrans    = orgTrans * childTrans;

            //perform an AABB check first
            IndexedVector3 aabbMin0;
            IndexedVector3 aabbMax0;
            IndexedVector3 aabbMin1;
            IndexedVector3 aabbMax1;

            childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
            m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);

            if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
            {
                m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
                m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);

                //the contactpoint is still projected back using the original inverted worldtrans
                CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
                m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);

                if (m_childCollisionAlgorithms[index] == null)
                {
                    m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
                    if (m_childCollisionAlgorithms[index] == m_parent)
                    {
                        int ibreak = 0;
                    }
                }

                ///detect swapping case
                if (m_resultOut.GetBody0Internal() == m_compoundColObj)
                {
                    m_resultOut.SetShapeIdentifiersA(-1, index);
                }
                else
                {
                    m_resultOut.SetShapeIdentifiersB(-1, index);
                }


                m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
                if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
                {
                    IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
                    m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
                    m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
                }

                //revert back transform
                m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
                m_compoundColObj.SetWorldTransform(ref orgTrans);
                m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
            }
        }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

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

            ConvexShape      convexShape = convexObj.GetCollisionShape() as ConvexShape;
            StaticPlaneShape planeShape  = planeObj.GetCollisionShape() as StaticPlaneShape;

            bool           hasCollision  = false;
            IndexedVector3 planeNormal   = planeShape.GetPlaneNormal();
            float          planeConstant = planeShape.GetPlaneConstant();
            IndexedMatrix  planeInConvex;

            planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
            IndexedMatrix convexInPlaneTrans;

            convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();

            IndexedVector3 vtx        = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
            IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
            float          distance   = (planeNormal.Dot(vtxInPlane) - planeConstant);

            IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
            IndexedVector3 vtxInPlaneWorld     = planeObj.GetWorldTransform() * vtxInPlaneProjected;

            hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
            resultOut.SetPersistentManifold(m_manifoldPtr);
            if (hasCollision)
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
                IndexedVector3 pOnB             = vtxInPlaneWorld;
                resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
            }
            ////first perform a collision query with the non-perturbated collision objects
            //{
            //    IndexedQuaternion rotq = IndexedQuaternion.Identity;
            //    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
            //}

            if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
            {
                IndexedVector3 v0;
                IndexedVector3 v1;
                TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
                //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                float angleLimit = 0.125f * MathUtil.SIMD_PI;
                float perturbeAngle;
                float radius = convexShape.GetAngularMotionDisc();
                perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
                if (perturbeAngle > angleLimit)
                {
                    perturbeAngle = angleLimit;
                }
                IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
                for (int i = 0; i < m_numPerturbationIterations; i++)
                {
                    float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                    IndexedQuaternion rotq           = new IndexedQuaternion(planeNormal, iterationAngle);
                    rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
                    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
                }
            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.GetNumContacts() > 0)
                {
                    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;

                            if (dispatchInfo.m_enableSatConvex)
                            {
                                foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                    polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(),
                                    out sepNormalWorldSpace);
                            }
                            else
                            {
#if ZERO_MARGIN
                                gjkPairDetector.SetIgnoreMargin(true);
                                gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif

                                float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                if (l2 > MathUtil.SIMD_EPSILON)
                                {
                                    sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                    //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
                                    minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();

#if ZERO_MARGIN
                                    foundSepAxis = true;    //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
                                    foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
                                }
                            }
                            if (foundSepAxis)
                            {
                                //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());

                                PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                                                              body0.GetWorldTransform(),
                                                                              body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
                            }
                            if (m_ownManifold)
                            {
                                resultOut.RefreshContactPoints();
                            }

                            return;
                        }
                        else
                        {
                            //we can also deal with convex versus triangle (without connectivity data)
                            if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
                            {
                                m_vertices.Clear();
                                TriangleShape tri = polyhedronB as TriangleShape;
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);

                                float          threshold           = m_manifoldPtr.GetContactBreakingThreshold();
                                IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
                                float          minDist             = float.MinValue;
                                float          maxDist             = threshold;

                                bool foundSepAxis = false;
                                if (false)
                                {
                                    polyhedronB.InitializePolyhedralFeatures();
                                    foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                        polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                        body0.GetWorldTransform(),
                                        body1.GetWorldTransform(),
                                        out sepNormalWorldSpace);
                                    //	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
                                }
                                else
                                {
#if ZERO_MARGIN
                                    gjkPairDetector.SetIgnoreMargin(true);
                                    gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                    gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN

                                    float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                    if (l2 > MathUtil.SIMD_EPSILON)
                                    {
                                        sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                        //minDist = gjkPairDetector.getCachedSeparatingDistance();
                                        //maxDist = threshold;
                                        minDist      = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
                                        foundSepAxis = true;
                                    }
                                }


                                if (foundSepAxis)
                                {
                                    PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
                                                                                  body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
                                }

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


                    gjkPairDetector.GetClosestPoints(ref 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)
                    {
                        IndexedVector3 v0, v1;

                        IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
                        sepNormalWorldSpace.Normalize();
                        TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out 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;
                        }

                        IndexedMatrix unPerturbedTransform;
                        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)
                            {
                                IndexedQuaternion perturbeRot    = new IndexedQuaternion(v0, perturbeAngle);
                                float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                                IndexedQuaternion rotq           = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);

                                if (perturbeA)
                                {
                                    input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
                                    input.m_transformB        = body1.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._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#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(ref 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();
            }
        }
Esempio n. 34
0
        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 = 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;

                            if (dispatchInfo.m_enableSatConvex)
                            {
                                foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                    polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(),
                                    out sepNormalWorldSpace);
                            }
                            else
                            {

#if ZERO_MARGIN
                gjkPairDetector.SetIgnoreMargin(true);
                gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else

                                gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif

                                float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                if (l2 > MathUtil.SIMD_EPSILON)
                                {
                                    sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                    //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
                                    minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();

#if ZERO_MARGIN
					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
                                    foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
                                }
                            }
                            if (foundSepAxis)
                            {
                                //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());

                                PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);

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

                            return;

                        }
                        else
                        {

                            //we can also deal with convex versus triangle (without connectivity data)
                            if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
                            {
                                m_vertices.Clear();
                                TriangleShape tri = polyhedronB as TriangleShape;
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);

                                float threshold = m_manifoldPtr.GetContactBreakingThreshold();
                                IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); ;
                                float minDist = float.MinValue;
                                float maxDist = threshold;

                                bool foundSepAxis = false;
                                if (false)
                                {
                                    polyhedronB.InitializePolyhedralFeatures();
                                    foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                    polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(),
                                    out sepNormalWorldSpace);
                                    //	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());

                                }
                                else
                                {
#if ZERO_MARGIN
					gjkPairDetector.SetIgnoreMargin(true);
					gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
                                    gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN

                                    float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                    if (l2 > MathUtil.SIMD_EPSILON)
                                    {
                                        sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                        //minDist = gjkPairDetector.getCachedSeparatingDistance();
                                        //maxDist = threshold;
                                        minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
                                        foundSepAxis = true;
                                    }
                                }


                                if (foundSepAxis)
                                {
                                    PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
                                        body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
                                }

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

                        }


                    }


                    gjkPairDetector.GetClosestPoints(ref 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)
                    {
                        IndexedVector3 v0, v1;

                        IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
                        sepNormalWorldSpace.Normalize();
                        TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out 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;
                        }

                        IndexedMatrix unPerturbedTransform;
                        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)
                            {

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

                                if (perturbeA)
                                {
                                    input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
                                    input.m_transformB = body1.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._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#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(ref 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();
            }
        }
Esempio n. 36
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //resultOut = null;
            CollisionObject colObj   = m_isSwapped ? body1 : body0;
            CollisionObject otherObj = m_isSwapped ? body0 : body1;

            Debug.Assert(colObj.GetCollisionShape().IsCompound());
            CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());

            ///btCompoundShape might have changed:
            ////make sure the internal child collision algorithm caches are still valid
            if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
            {
                ///clear and update all
                RemoveChildAlgorithms();
                PreallocateChildAlgorithms(body0, body1);
            }


            Dbvt tree = compoundShape.GetDynamicAabbTree();

            //use a dynamic aabb tree to cull potential child-overlaps
            using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
            {
                callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);

                ///we need to refresh all contact manifolds
                ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
                ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
                {
                    m_manifoldArray.Clear();
                    for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
                    {
                        if (m_childCollisionAlgorithms[i] != null)
                        {
                            m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
                            for (int m = 0; m < m_manifoldArray.Count; m++)
                            {
                                if (m_manifoldArray[m].GetNumContacts() > 0)
                                {
                                    resultOut.SetPersistentManifold(m_manifoldArray[m]);
                                    resultOut.RefreshContactPoints();
                                    resultOut.SetPersistentManifold(null);//??necessary?
                                }
                            }
                            m_manifoldArray.Clear();
                        }
                    }
                }

                if (tree != null)
                {
                    IndexedVector3 localAabbMin;
                    IndexedVector3 localAabbMax;
                    IndexedMatrix  otherInCompoundSpace;
                    //otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
                    otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();

                    otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);

                    DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                    //process all children, that overlap with  the given AABB bounds
                    Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
                }
                else
                {
                    //iterate over all children, perform an AABB check inside ProcessChildShape
                    int numChildren = m_childCollisionAlgorithms.Count;
                    for (int i = 0; i < numChildren; i++)
                    {
                        callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
                    }
                }

                {
                    //iterate over all children, perform an AABB check inside ProcessChildShape
                    int numChildren = m_childCollisionAlgorithms.Count;

                    m_manifoldArray.Clear();
                    CollisionShape childShape = null;
                    IndexedMatrix  orgTrans;
                    IndexedMatrix  orgInterpolationTrans;
                    IndexedMatrix  newChildWorldTrans;


                    for (int i = 0; i < numChildren; i++)
                    {
                        if (m_childCollisionAlgorithms[i] != null)
                        {
                            childShape = compoundShape.GetChildShape(i);
                            //if not longer overlapping, remove the algorithm
                            orgTrans = colObj.GetWorldTransform();
                            orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
                            IndexedMatrix childTrans = compoundShape.GetChildTransform(i);

                            newChildWorldTrans = orgTrans * childTrans;

                            //perform an AABB check first
                            IndexedVector3 aabbMin0;
                            IndexedVector3 aabbMax0;
                            IndexedVector3 aabbMin1;
                            IndexedVector3 aabbMax1;

                            childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
                            otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);

                            if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
                            {
                                m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
                                m_childCollisionAlgorithms[i] = null;
                            }
                        }
                    }
                }
            }
        }
        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //(void)resultOut;
            //(void)dispatchInfo;
            ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            ///body0.m_worldTransform,
            float resultFraction = 1.0f;

            float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
            float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
                squareMot1 < body1.GetCcdSquareMotionThreshold())
            {
                return resultFraction;
            }
            if (disableCcd)
            {
                return 1f;
            }


            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)


            /// Convex0 against sphere for Convex1
            {
                ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;

                SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
                sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(convex0, sphere1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                        body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {

                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }
                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }
                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere1);
                    result.Cleanup();
                }
            }

            /// Sphere (for convex0) against Convex1
            {
                ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;

                SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
                sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(sphere0, convex1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                        body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {

                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }
                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }
                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere0);
                    result.Cleanup();
                }
            }

            return resultFraction;
        }
        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;
        }