protected void AddContactPoint(CollisionObject body0,
                                       CollisionObject body1,
                                       ref IndexedVector3 point,
                                       ref IndexedVector3 normal,
                                       float distance)
        {
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAlgo::AddContactPoint");
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "point", point);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "normal", normal);
            }

            m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
            m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
            CheckManifold(body0, body1);
            m_resultOut.AddContactPoint(normal, point, distance);
        }
Пример #2
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);
                }
            }
        }
Пример #3
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);
            }
        }