예제 #1
0
    public static bool CollideWithEnv(EnvTraceInfo info, bool SupportPlane = false)
    {
        bool collide = false;

        info.Fraction   = 100.0f;
        info.StartSolid = false;
        info.ClsFlag    = 0;
        info.HitEnv     = HIT_ENVTYPE.HIT_NULL;

        mCapsuleTraceBrushInfo.Init(info.Start, info.HalfLen, info.Radius, Quaternion.Euler(0, 0, 0), info.Delta, info.CheckFlag);

        if (CapsuleCollideWithBrush(mCapsuleTraceBrushInfo) && mCapsuleTraceBrushInfo.Fraction < info.Fraction)
        {
            collide         = true;
            info.Fraction   = mCapsuleTraceBrushInfo.Fraction;
            info.StartSolid = mCapsuleTraceBrushInfo.StartSolid;
            info.HitEnv     = HIT_ENVTYPE.HIT_BRUSH;
            info.ClsFlag    = mCapsuleTraceBrushInfo.HitFlags;
            info.HitPos     = mCapsuleTraceBrushInfo.CloseB;

            if (!info.StartSolid)
            {
                if (SupportPlane && mCapsuleTraceBrushInfo.HitObject != null && mCapsuleTraceBrushInfo.HitObject.cd != null)
                {
                    CollidePoints cp = mCapsuleTraceBrushInfo.HitPoints;
                    if (cp.size == 3)//和面最近,直接用陷入法线
                    {
                        info.HitNormal = mCapsuleTraceBrushInfo.Normal;
                    }
                    if (cp.size == 2)//和线最近
                    {
                        info.HitNormal = mCapsuleTraceBrushInfo.HitObject.cd.GetSupportPlaneNormal(cp.a, cp.b);
                    }
                    if (cp.size == 1)//和面最近,直接用陷入法线
                    {
                        info.HitNormal = mCapsuleTraceBrushInfo.HitObject.cd.GetSupportPlaneNormal(cp.a);
                    }
                }
                else
                {
                    info.HitNormal = mCapsuleTraceBrushInfo.Normal;
                }
            }
        }


        if (info.Fraction > 1.0f)
        {
            info.Fraction = 1.0f;
        }
        //UEMathUtil.Clamp(ref info.Fraction, 0.0f, 1.0f);

        return(collide);
    }
예제 #2
0
    static float GJK_RELATIVE_EPSILON = 0.0004f;    //square of 2%.
    //ML: if we are using gjk local which means one of the object will be sphere/capsule, in that case, if we define takeCoreShape is true, we just need to return the closest point as the sphere center or a point in the capsule segment. This will increase the stability
    //for the manifold recycling code
    public static GJKType GjkLocalPenetration_CapsuleConvex(CAPSULE a, ConvexData b, ref Vector3 normal, ref float penetrationDepth, ref CollidePoints points, ref Vector3 closeB)
    {
        float marginA = a.getMargin();
        float marginB = 0;    //b.getMargin();

        //float minMargin = 0;// Mathf.Min(a.getMinMargin(), b.getMinMargin());
        //float _eps2 = (minMargin * (0.1f));
        //ML: eps2 is the threshold that uses to detemine whether two (shrunk) shapes overlap. We calculate eps2 based on 10% of the minimum margin of two shapes
        float eps2 = 0;    // (_eps2 * _eps2);
        //ML: epsRel2 is the square of 1.5%. This is used to scale the the sqaure distance of a closet point to origin to detemine whether two shrunk shapes overlap in the margin, but
        //they don't overlap.
        float epsRel2 = GJK_RELATIVE_EPSILON;

        float sumOrignalMargin = marginA + marginB;

        float sDist   = float.MaxValue;
        float minDist = sDist;

        bool    bNotTerminated = true;
        bool    bCon           = true;
        Vector3 closest;

        Vector3 supportA = Vector3.zero, supportB = Vector3.zero, support = Vector3.zero;

        int size = 0;

        Vector3 _initialSearchDir = a.Center - b.GetAABB().center;

        closest = Vector3.Dot(_initialSearchDir, _initialSearchDir) > 0 ? _initialSearchDir : Vector3.right;

        Vector3 prevClosest = closest;

        int bIndex = 0;

        // ML : termination condition
        //(1)two (shrunk)shapes overlap. GJK will terminate based on sq(v) < eps2 and indicate that two shapes are overlapping.
        //(2)two (shrunk + margin)shapes separate. If sq(vw) > sqMargin * sq(v), which means the original objects do not intesect, GJK terminate with GJK_NON_INTERSECT.
        //(3)two (shrunk) shapes don't overlap. However, they interect within margin distance. if sq(v)- vw < epsRel2*sq(v), this means the shrunk shapes interect in the margin,
        //   GJK terminate with GJK_CONTACT.
        while (bNotTerminated)
        {
            //minDist, tempClosA, tempClosB are used to store the previous iteration's closest points(in A and B space) and the square distance from the closest point
            //to origin in Mincowski space
            minDist     = sDist;
            prevClosest = closest;

            supportA = a.supportSweepLocal(-closest);
            supportB = b.supportSweepLocal(closest, ref bIndex);

            //calculate the support point
            support = supportA - supportB;
            Q[size] = support;
            B[size] = bIndex;

            //ML: because we shrink the shapes by plane shifting(box and convexhull), the distance from the "shrunk" vertices to the original vertices may be larger than contact distance.
            //therefore, we need to take the largest of these 2 values into account so that we don't incorrectly declare shapes to be disjoint. If we don't do this, there is
            //an inherent inconsistency between fallback SAT tests and GJK tests that may result in popping due to SAT discovering deep penetrations that were not detected by
            //GJK operating on a shrunk shape.
            float sqMargin = (sumOrignalMargin * sumOrignalMargin);

            float vw = Vector3.Dot(closest, support);

            bool con          = (vw > 0) && (vw * vw > sDist * sqMargin); //this is the non intersect condition
            bool conGrtr      = (epsRel2 * sDist) >= (sDist - vw);        //this is the margin intersect condition
            bool conOrconGrtr = con || conGrtr;

            if (conOrconGrtr)
            {
                if (!con)     //must be true otherwise we wouldn't be in here...
                {
                    float dist = Mathf.Sqrt(sDist);
                    //PX_ASSERT(FAllGrtr(dist, FEps));
                    Vector3 n = closest / dist;    //normalise
                    normal = n;

                    closeB = getClosestPoint(closest, size, b);

                    penetrationDepth = dist - sumOrignalMargin;

                    points.size = size;
                    points.a    = B[0];
                    points.b    = B[1];
                    points.c    = B[2];

                    return(GJKType.GJK_CONTACT);
                }
                else
                {
                    return(GJKType.GJK_NON_INTERSECT);
                }
            }

            size++;
            //PX_ASSERT(size <= 4);

            //calculate the closest point between two convex hull
            closest = GJKCPairDoSimplex(support, ref size);

            sDist = Vector3.Dot(closest, closest);

            bCon           = minDist > sDist;
            bNotTerminated = sDist > eps2 && bCon;
        }

        if (!bCon)
        {
            sDist = minDist;

            float sqExpandedMargin = sumOrignalMargin * sumOrignalMargin;
            //Reset back to older closest point
            closest = prevClosest;

            closeB = getClosestPoint(closest, size, b);

            sDist = minDist;

            float dist = Mathf.Sqrt(sDist);
            //PX_ASSERT(dist > FEps);
            Vector3 n = closest / dist;    //normalise

            penetrationDepth = dist - sumOrignalMargin;

            normal = n;

            points.size = size;
            points.a    = B[0];
            points.b    = B[1];
            points.c    = B[2];

            if (sqExpandedMargin >= sDist)
            {
                return(GJKType.GJK_CONTACT);
            }

            //此时明明没有碰撞,但是误差导致的碰撞,向法线方向移动0.001f
            //penetrationDepth = -0.001f;
            return(GJKType.GJK_DEGENERATE);
        }
        else
        {
            //this two shapes are deeply intersected with each other, we need to use EPA algorithm to calculate MTD
            return(GJKType.EPA_CONTACT);
        }
    }
예제 #3
0
    static public bool GjkLocalRayCast_CapsuleConvex(CAPSULE a, ConvexData b, Vector3 r, ref float lambda, ref Vector3 normal, ref bool StartSolid, ref CollidePoints points, ref Vector3 closeB)
    {
        if (b.GetVertexNum() == 0)
        {
            return(false);
        }

        bool  _StartSolid = true;
        float inflation   = a.Radius;
        float maxDist     = float.MaxValue;

        float _lambda = 0;

        r = -r;
        Vector3 x    = r * _lambda;
        int     size = 1;

        Vector3 dir = a.Center - b.GetAABB().center;
        Vector3 _initialSearchDir = (Vector3.Dot(dir, dir) > FEps) ? dir : Vector3.right;
        Vector3 initialSearchDir  = Vector3.Normalize(_initialSearchDir);

        int     bIndex          = 0;
        Vector3 initialSupportA = a.supportSweepLocal(-initialSearchDir);
        Vector3 initialSupportB = b.supportSweepLocal(initialSearchDir, ref bIndex);

        Q[0] = initialSupportA - initialSupportB; Q[1] = Vector3.zero; Q[2] = Vector3.zero; Q[3] = Vector3.zero; //simplex set
        B[0] = bIndex; B[1] = 0; B[2] = 0; B[3] = 0;                                                             //ConvexHull b simplex set

        Vector3 closest  = Q[0];
        Vector3 supportA = initialSupportA;
        Vector3 supportB = initialSupportB;
        Vector3 support  = Q[0];

        //float minMargin = Mathf.Min(a.getSweepMargin(), b.getSweepMargin());
        float eps1             = 0;//minMargin * 0.1f;
        float inflationPlusEps = eps1 + inflation;
        float eps2             = eps1 * eps1;

        float inflation2 = inflationPlusEps * inflationPlusEps;

        float sDist   = Vector3.Dot(closest, closest);
        float minDist = sDist;

        bool bNotTerminated = sDist > eps2;
        bool bCon           = true;

        Vector3 nor         = closest;
        Vector3 prevClosest = closest;

        while (bNotTerminated == true)
        {
            minDist     = sDist;
            prevClosest = closest;

            Vector3 vNorm = -Vector3.Normalize(closest);

            supportA = a.supportSweepLocal(vNorm);
            supportB = x + b.supportSweepLocal(-vNorm, ref bIndex);

            //calculate the support point
            support = supportA - supportB;
            Vector3 w  = -support;
            float   vw = Vector3.Dot(vNorm, w) - inflationPlusEps;
            float   vr = Vector3.Dot(vNorm, r);
            if (vw > 0)
            {
                if (vr >= 0)
                {
                    return(false);
                }
                else
                {
                    _StartSolid = false;
                    float _oldLambda = _lambda;
                    _lambda = _lambda - vw / vr;
                    if (_lambda > _oldLambda)
                    {
                        if (_lambda > 1)
                        {
                            return(false);
                        }
                        Vector3 bPreCenter = x;
                        x = r * _lambda;

                        Vector3 offSet = x - bPreCenter;
                        Q[0] -= offSet;
                        Q[1] -= offSet;
                        Q[2] -= offSet;

                        supportB += offSet;
                        support   = supportA - supportB;
                        minDist   = maxDist;
                        nor       = closest;
                        //size=0;
                    }
                }
            }

            //ASSERT(size < 4); lxq test
            B[size]   = bIndex;
            Q[size++] = support;

            //calculate the closest point between two convex hull
            closest = GJKCPairDoSimplex(support, ref size);
            sDist   = Vector3.Dot(closest, closest);

            bCon           = minDist > sDist;
            bNotTerminated = (sDist > inflation2) && bCon;
        }

        //bool aQuadratic = a.isMarginEqRadius();
        //ML:if the Minkowski sum of two objects are too close to the original(eps2 > sDist), we can't take v because we will lose lots of precision. Therefore, we will take
        //previous configuration's normal which should give us a reasonable approximation. This effectively means that, when we do a sweep with inflation, we always keep v because
        //the shapes converge separated. If we do a sweep without inflation, we will usually use the previous configuration's normal.
        nor    = ((sDist > eps2) && bCon) ? closest : nor;
        nor    = Vector3.Normalize(nor);
        normal = nor;

        lambda = _lambda;
        //float cosAngle = Vector3.Dot(nor, r);
        //float offset = 0.001f / cosAngle;
        //lambda -= offset;
        if (lambda < 0)
        {
            //lambda *= cosAngle;
            lambda = 0;
        }

        points.size = size;
        points.a    = B[0];
        points.b    = B[1];
        points.c    = B[2];

        Vector3 closestP = bCon? closest: prevClosest;

        closeB = getClosestPoint(closestP, size, b);
        //closestA = V3Sel(aQuadratic, V3NegScaleSub(nor, a.getMargin(), closA), closA);

        StartSolid = false;
        if (_StartSolid)
        {
            GJKType ret = GjkLocalPenetration_CapsuleConvex(a, b, ref normal, ref lambda, ref points, ref closeB);
            if (ret == GJKType.EPA_CONTACT)
            {
                StartSolid = true;
            }
            else
            {
                if (lambda > 0)
                {
                    lambda = 0;
                }
            }
        }

        return(true);
    }
예제 #4
0
    private bool _CapsuleTraceBrush(CollisioinTreeNode pNode, CapsuleTraceBrushInfo pInfo)
    {
        if (null == pNode || null == pInfo)
        {
            return(false);
        }

        if (!BoundsExtansions.AABBAABBOverlap(pNode.AABB, pInfo.Bound))
        {
            return(false);
        }

        bool bCollide    = false;
        bool bStartSolid = false;

        CDBrush       HitObject = null;
        CollidePoints HitPoints = new CollidePoints();
        Vector3       normal    = Vector3.zero;
        float         fFraction = 100.0f;

        if (pNode.IsLeaf())
        {
            int i;
            for (i = 0; i < (int)pNode.LstCDBrush.Count; ++i)
            {
                CDBrush pBrush = pNode.LstCDBrush[i];

                if (pBrush.CapsuleTraceBrush(pInfo) && (pInfo.Fraction < fFraction))
                {
                    //update the saving info
                    bStartSolid = pInfo.StartSolid;
                    fFraction   = pInfo.Fraction;
                    HitObject   = pInfo.HitObject;
                    HitPoints   = pInfo.HitPoints;
                    normal      = pInfo.Normal;
                    bCollide    = true;

                    //if (pInfo.Fraction == 0.0f)
                    //{
                    //    break;
                    //}
                }
            }
        }
        else
        {
            for (int j = 0; j < 8; j++)
            {
                if (_CapsuleTraceBrush(pNode.Children[j], pInfo) && pInfo.Fraction < fFraction)
                {
                    bStartSolid = pInfo.StartSolid;
                    fFraction   = pInfo.Fraction;
                    HitObject   = pInfo.HitObject;
                    HitPoints   = pInfo.HitPoints;
                    normal      = pInfo.Normal;
                    bCollide    = true;
                    //if (pInfo.Fraction == 0.0f)
                    //{
                    //    break;
                    //}
                }
            }
        }

        if (bCollide)
        {
            //set back
            pInfo.StartSolid = bStartSolid;
            pInfo.Fraction   = fFraction;
            pInfo.HitObject  = HitObject;
            pInfo.HitPoints  = HitPoints;
            pInfo.Normal     = normal;
        }
        return(bCollide);
    }