Example #1
0
        public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
        {
            m_updateRevision++;
            //m_childTransforms.push_back(localTransform);
            //m_childShapes.push_back(shape);
            CompoundShapeChild child = new CompoundShapeChild();

            child.m_transform      = localTransform;
            child.m_childShape     = shape;
            child.m_childShapeType = shape.GetShapeType();
            child.m_childMargin    = shape.GetMargin();

            //extend the local aabbMin/aabbMax
            IndexedVector3 localAabbMin;
            IndexedVector3 localAabbMax;

            shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
            MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
            MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);

            if (m_dynamicAabbTree != null)
            {
                DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                int        index  = m_children.Count;
                child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
            }

            m_children.Add(child);
        }
        public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
        {
            m_updateRevision++;
            //m_childTransforms.push_back(localTransform);
            //m_childShapes.push_back(shape);
            CompoundShapeChild child = new CompoundShapeChild();
            child.m_transform = localTransform;
            child.m_childShape = shape;
            child.m_childShapeType = shape.GetShapeType();
            child.m_childMargin = shape.GetMargin();

            //extend the local aabbMin/aabbMax
            IndexedVector3 localAabbMin;
            IndexedVector3 localAabbMax;
            shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
            MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
            MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);

            if (m_dynamicAabbTree != null)
            {
                DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                int index = m_children.Count;
                child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
            }

            m_children.Add(child);
        }
        protected void GImpactVsShapeFindPairs(
            ref IndexedMatrix trans0,
            ref IndexedMatrix trans1,
            GImpactShapeInterface shape0,
            CollisionShape shape1,
            ObjectArray <int> collided_primitives)
        {
            AABB boxshape = new AABB();


            if (shape0.HasBoxSet())
            {
                IndexedMatrix trans1to0 = trans0.Inverse();
                //trans1to0 *= trans1;
                trans1to0 = trans1to0 * trans1;
                //trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
                shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
                {
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
                }
#endif
                shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
            }
            else
            {
                shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);

                AABB boxshape0 = new AABB();
                int  i         = shape0.GetNumChildShapes();

                while (i-- != 0)
                {
                    shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);

                    if (boxshape.HasCollision(ref boxshape0))
                    {
                        collided_primitives.Add(i);
                    }
                }
            }
        }
Example #4
0
        public virtual void GetPrimitiveBox(int prim_index, out AABB primbox)
        {
            IndexedMatrix prim_trans;

            if (m_compoundShape.ChildrenHasTransform())
            {
                prim_trans = m_compoundShape.GetChildTransform(prim_index);
            }
            else
            {
                prim_trans = IndexedMatrix.Identity;
            }
            CollisionShape shape = m_compoundShape.GetChildShape(prim_index);

            shape.GetAabb(prim_trans, out primbox.m_min, out primbox.m_max);
        }
        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;
        }
        protected void GImpactVsShapeFindPairs(
                          ref IndexedMatrix trans0,
                          ref IndexedMatrix trans1,
                          GImpactShapeInterface shape0,
                          CollisionShape shape1,
                          ObjectArray<int> collided_primitives)
        {
            AABB boxshape = new AABB();


            if (shape0.HasBoxSet())
            {
                IndexedMatrix trans1to0 = trans0.Inverse();
                //trans1to0 *= trans1;
                trans1to0 = trans1to0 * trans1;
                //trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
                shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
                {
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
                }
                shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
            }
            else
            {
                shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);

                AABB boxshape0 = new AABB();
                int i = shape0.GetNumChildShapes();

                while (i-- != 0)
                {
                    shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);

                    if (boxshape.HasCollision(ref boxshape0))
                    {
                        collided_primitives.Add(i);
                    }
                }

            }


        }
        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);
            }
        }
Example #8
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);
            }
        }
Example #9
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;
                            }
                        }
                    }
                }
            }
        }