public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
 {
     base.Initialize(ci, body0, body1);
     m_ownManifold = false;
     m_manifoldPtr = mf;
     m_swapped     = swapped;
 }
Exemplo n.º 2
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);
        }
        }                                             // for pool

        public SphereTriangleCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
            : base(ci, body0, body1)
        {
            m_ownManifold = false;
            m_manifoldPtr = mf;
            m_swapped     = swapped;
        }
Exemplo n.º 4
0
        public virtual void ReleaseManifold(PersistentManifold manifold)
        {
            gNumManifold--;
            ClearManifold(manifold);
            int findIndex = manifold.m_index1a;

            //m_manifoldsPtr.Remove(manifold);
            m_manifoldsPtr.RemoveAtQuick(findIndex);
            m_manifoldsPtr[findIndex].m_index1a = findIndex;

            //PersistentManifold swapTemp = m_manifoldsPtr[findIndex];
            //m_manifoldsPtr[findIndex] = m_manifoldsPtr[m_manifoldsPtr.Count - 1];
            //m_manifoldsPtr[m_manifoldsPtr.Count - 1] = swapTemp;
            //m_manifoldsPtr[findIndex].m_index1a = findIndex;
            //m_manifoldsPtr.RemoveAt(m_manifoldsPtr.Count - 1);

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
            {
                BulletGlobals.g_streamWriter.WriteLine("ReleaseManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
            }
#endif
            // and return it to free list.
            BulletGlobals.PersistentManifoldPool.Free(manifold);
        }
Exemplo n.º 5
0
 public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
 {
     m_dispatcher  = dispatcher;
     m_convexBody  = isSwapped ? body1 : body0;
     m_triBody     = isSwapped ? body0 : body1;
     m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
     ClearCache();
 }
Exemplo n.º 6
0
 public void Initialize(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
 {
     m_dispatcher  = dispatcher;
     m_convexBody  = isSwapped ? body1 : body0;
     m_triBody     = isSwapped ? body0 : body1;
     m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
     ClearCache();
 }
Exemplo n.º 7
0
 public void DestroyContactManifolds()
 {
     if (m_manifoldPtr == null)
     {
         return;
     }
     m_dispatcher.ReleaseManifold(m_manifoldPtr);
     m_manifoldPtr = null;
 }
Exemplo n.º 8
0
 public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
 {
     base.Initialize(ci);
     m_ownManifold = false;
     m_manifoldPtr = mf;
     if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(body0, body1))
     {
         m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
         m_ownManifold = true;
     }
 }
Exemplo n.º 9
0
 public Convex2dConvex2dAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
     : base(ci, body0, body1)
 {
     m_simplexSolver                      = simplexSolver;
     m_pdSolver                           = pdSolver;
     m_ownManifold                        = false;
     m_manifoldPtr                        = mf;
     m_lowLevelOfDetail                   = false;
     m_numPerturbationIterations          = numPerturbationIterations;
     m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
 }
 public void Initialize(CollisionObject compoundObj, CollisionObject otherObj, IDispatcher dispatcher, DispatcherInfo dispatchInfo, ManifoldResult resultOut, CompoundCollisionAlgorithm parent, IList <CollisionAlgorithm> childCollisionAlgorithms, PersistentManifold sharedManifold)
 {
     m_compoundColObj           = compoundObj;
     m_otherObj                 = otherObj;
     m_dispatcher               = dispatcher;
     m_dispatchInfo             = dispatchInfo;
     m_resultOut                = resultOut;
     m_childCollisionAlgorithms = childCollisionAlgorithms;
     m_sharedManifold           = sharedManifold;
     m_parent = parent;
 }
Exemplo n.º 11
0
 public int CompareTo(object obj)
 {
     if (obj is PersistentManifold)
     {
         PersistentManifold otherManifold = (PersistentManifold)obj;
         return(getIslandId(this) - getIslandId(otherManifold));
     }
     else
     {
         throw new ArgumentException("Object is not a PersistentManifold");
     }
 }
        public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject colObj0, CollisionObject colObj1)
        {
            base.Initialize(ci, colObj0, colObj1);
            m_ownManifold = false;
            m_manifoldPtr = mf;

            if (m_manifoldPtr == null)
            {
                m_manifoldPtr = m_dispatcher.GetNewManifold(colObj0, colObj1);
                m_ownManifold = true;
            }
        }
Exemplo n.º 13
0
 public override void Cleanup()
 {
     if (m_ownManifold)
     {
         if (m_manifoldPtr != null)
         {
             m_dispatcher.ReleaseManifold(m_manifoldPtr);
         }
         m_ownManifold = false;
     }
     m_manifoldPtr = null;
     BulletGlobals.ConvexConvexAlgorithmPool.Free(this);
 }
        }                                           // for pool

        public SphereSphereCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
            : base(ci, body0, body1)
        {
            m_ownManifold = false;
            m_manifoldPtr = mf;

            // for some reason theres no check in 2.76 or 2.78 for a needs collision, just a check on pointer being null.
            if (m_manifoldPtr == null)
            {
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
        }
        public static int GetIslandId(PersistentManifold lhs)
        {
            CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
            int             islandId = rcolObj0.GetIslandTag();

            if (islandId >= 0)
            {
                return(islandId);
            }

            CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;

            return(rcolObj1.GetIslandTag());
        }
Exemplo n.º 16
0
        public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
        {
            base.Initialize(ci, body0, body1);
            m_simplexSolver    = simplexSolver;
            m_pdSolver         = pdSolver;
            m_ownManifold      = false;
            m_manifoldPtr      = mf;
            m_lowLevelOfDetail = false;
#if USE_SEPDISTANCE_UTIL2
            m_sepDistance((static_cast <btConvexShape *>(body0.getCollisionShape())).getAngularMotionDisc(),
                          (static_cast <btConvexShape *>(body1.getCollisionShape())).getAngularMotionDisc()),
#endif
            m_numPerturbationIterations          = numPerturbationIterations;
            m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
        }
Exemplo n.º 17
0
        }                                        // for pool

        public SphereBoxCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped)
            : base(ci, col0, col1)
        {
            m_isSwapped   = isSwapped;
            m_ownManifold = false;
            m_manifoldPtr = mf;
            CollisionObject sphereObj = m_isSwapped ? col1 : col0;
            CollisionObject boxObj    = m_isSwapped ? col0 : col1;

            if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(sphereObj, boxObj))
            {
                m_manifoldPtr = m_dispatcher.GetNewManifold(sphereObj, boxObj);
                m_ownManifold = true;
            }
        }
        public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback)
        {
            ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();

            BuildIslands(dispatcher, collisionWorld);

            int endIslandIndex = 1;
            int startIslandIndex;
            int numElem = GetUnionFind().GetNumElements();

            BulletGlobals.StartProfile("processIslands");

            if (!m_splitIslands)
            {
                PersistentManifoldArray manifolds = dispatcher.GetInternalManifoldPointer();
                int maxNumManifolds = dispatcher.GetNumManifolds();
                callback.ProcessIsland(collisionObjects, collisionObjects.Count, manifolds, 0, maxNumManifolds, -1);
            }
            else
            {
                // Sort manifolds, based on islands
                // Sort the vector using predicate and std::sort
                //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);

                int numManifolds = m_islandmanifold.Count;

                //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))

                m_islandmanifold.QuickSort(m_sortPredicate);

                //now process all active islands (sets of manifolds for now)

                int startManifoldIndex = 0;
                int endManifoldIndex   = 1;
                //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
                for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
                {
                    int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;

#if DEBUG
                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                    {
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
                    }
#endif

                    bool islandSleeping = true;

                    for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
                    {
                        int             i       = GetUnionFind().GetElement(endIslandIndex).m_sz;
                        CollisionObject colObj0 = collisionObjects[i];
                        m_islandBodies.Add(colObj0);
                        if (colObj0.IsActive())
                        {
                            islandSleeping = false;
                        }
                    }

                    //find the accompanying contact manifold for this islandId
                    int numIslandManifolds           = 0;
                    PersistentManifold startManifold = null;

                    if (startManifoldIndex < numManifolds)
                    {
                        int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]);
#if DEBUG
                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                        {
                            BulletGlobals.g_streamWriter.WriteLine("curIsland[{0}] startManifold[{1}].", curIslandId, startManifoldIndex);
                        }
#endif


                        if (curIslandId == islandId)
                        {
                            startManifold = m_islandmanifold[startManifoldIndex];

                            for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
                            {
#if DEBUG
                                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                                {
                                    BulletGlobals.g_streamWriter.WriteLine("endManifoldIndex[{0}] islandId[{1}] getIsland[{2}].", endManifoldIndex, startManifoldIndex, GetIslandId(m_islandmanifold[endManifoldIndex]));
                                }
#endif
                            }
                            /// Process the actual simulation, only if not sleeping/deactivated
                            numIslandManifolds = endManifoldIndex - startManifoldIndex;
                        }
                    }

                    if (!islandSleeping)
                    {
                        callback.ProcessIsland(m_islandBodies, m_islandBodies.Count, m_islandmanifold, startManifoldIndex, numIslandManifolds, islandId);
                        //			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
                    }
                    else
                    {
#if DEBUG
                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                        {
                            BulletGlobals.g_streamWriter.WriteLine("islandSleeping.");
                        }
#endif
                    }

                    if (numIslandManifolds != 0)
                    {
                        startManifoldIndex = endManifoldIndex;
                    }

                    m_islandBodies.Clear();
                }
            } // else if(!splitIslands)
            BulletGlobals.StopProfile();
        }
Exemplo n.º 19
0
        }                                      // for pool

        //! Creates a new contact point
        public PersistentManifold NewContactManifold(CollisionObject body0, CollisionObject body1)
        {
            m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
            return(m_manifoldPtr);
        }
Exemplo n.º 20
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            //resultOut = new ManifoldResult();
            resultOut.SetPersistentManifold(m_manifoldPtr);

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


            ConvexShape    min0 = 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();
            }
        }
Exemplo n.º 21
0
        //public int GetDispatcherId()
        //{
        //    return 0;
        //}

        public void SetManifold(PersistentManifold manifold)
        {
            m_manifold = manifold;
        }
Exemplo n.º 22
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));
        }
Exemplo n.º 23
0
 public virtual void ClearManifold(PersistentManifold manifold)
 {
     manifold.ClearManifold();
 }
        public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
        {
            BulletGlobals.StartProfile("islandUnionFindAndQuickSort");

            ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();

            m_islandmanifold.Clear();

            //we are going to sort the unionfind array, and store the element id in the size
            //afterwards, we clean unionfind, to make sure no-one uses it anymore

            GetUnionFind().sortIslands();
            int numElem = GetUnionFind().GetNumElements();

            int endIslandIndex = 1;


            //update the sleeping state for bodies, if all are sleeping
            for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
            {
                int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                {
                    BulletGlobals.g_streamWriter.WriteLine(String.Format("buildIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
                }
#endif
                for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
                {
                }

                //int numSleeping = 0;

                bool allSleeping = true;

                for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                {
                    int i = GetUnionFind().GetElement(idx).m_sz;

                    CollisionObject colObj0 = collisionObjects[i];
                    if ((colObj0.GetIslandTag() != islandId) && (colObj0.GetIslandTag() != -1))
                    {
                        //				printf("error in island management\n");
                    }

                    Debug.Assert((colObj0.GetIslandTag() == islandId) || (colObj0.GetIslandTag() == -1));
                    if (colObj0.GetIslandTag() == islandId)
                    {
                        if (colObj0.GetActivationState() == ActivationState.ACTIVE_TAG)
                        {
                            allSleeping = false;
                        }
                        if (colObj0.GetActivationState() == ActivationState.DISABLE_DEACTIVATION)
                        {
                            allSleeping = false;
                        }
                    }
                }


                if (allSleeping)
                {
                    for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                    {
                        int             i         = GetUnionFind().GetElement(idx).m_sz;
                        CollisionObject colObj0   = collisionObjects[i];
                        int             islandTag = colObj0.GetIslandTag();
                        if (islandTag != islandId && islandTag != -1)
                        {
                            //					printf("error in island management\n");
                        }

                        Debug.Assert((islandTag == islandId) || (islandTag == -1));

                        if (islandTag == islandId)
                        {
                            colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
                        }
                    }
                }
                else
                {
                    for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                    {
                        int i = GetUnionFind().GetElement(idx).m_sz;

                        CollisionObject colObj0   = collisionObjects[i];
                        int             islandTag = colObj0.GetIslandTag();
                        if (islandTag != islandId && islandTag != -1)
                        {
                            //					printf("error in island management\n");
                        }

                        Debug.Assert((islandTag == islandId) || (islandTag == -1));

                        if (islandTag == islandId)
                        {
                            if (colObj0.GetActivationState() == ActivationState.ISLAND_SLEEPING)
                            {
                                colObj0.SetActivationState(ActivationState.WANTS_DEACTIVATION);
                                colObj0.SetDeactivationTime(0f);
                            }
                        }
                    }
                }
            }

            int maxNumManifolds = dispatcher.GetNumManifolds();

            //#definef SPLIT_ISLANDS 1
            //#ifdef SPLIT_ISLANDS


            //#endif //SPLIT_ISLANDS


            for (int i = 0; i < maxNumManifolds; i++)
            {
                PersistentManifold manifold = dispatcher.GetManifoldByIndexInternal(i);

                CollisionObject colObj0 = manifold.GetBody0() as CollisionObject;
                CollisionObject colObj1 = manifold.GetBody1() as CollisionObject;

                ///@todo: check sleeping conditions!
                if (((colObj0 != null) && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING) ||
                    ((colObj1 != null) && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING))
                {
                    //kinematic objects don't merge islands, but wake up all connected objects
                    if (colObj0.IsKinematicObject() && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING)
                    {
                        if (colObj0.HasContactResponse())
                        {
                            colObj1.Activate();
                        }
                    }
                    if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
                    {
                        if (colObj1.HasContactResponse())
                        {
                            colObj0.Activate();
                        }
                    }
                    if (m_splitIslands)
                    {
                        //filtering for response
                        if (dispatcher.NeedsResponse(colObj0, colObj1))
                        {
                            m_islandmanifold.Add(manifold);
                        }
                    }
                }
            }
            BulletGlobals.StopProfile();
        }
Exemplo n.º 25
0
 public void SetPersistentManifold(PersistentManifold manifoldPtr)
 {
     m_manifoldPtr = manifoldPtr;
 }