Esempio n. 1
0
 public static bool      Distance(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
 {
     using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
         using (GJK gjk = BulletGlobals.GJKPool.Get())
         {
             Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
             gjk.Initialise();
             GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
             if (gjk_status == GJKStatus.Valid)
             {
                 IndexedVector3 w0 = IndexedVector3.Zero;
                 IndexedVector3 w1 = IndexedVector3.Zero;
                 for (uint i = 0; i < gjk.m_simplex.rank; ++i)
                 {
                     float p = gjk.m_simplex.p[i];
                     w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
                     IndexedVector3 temp = -gjk.m_simplex.c[i].d;
                     w1 += shape.Support(ref temp, 1) * p;
                 }
                 results.witnesses0 = wtrs0 * w0;
                 results.witnesses1 = wtrs0 * w1;
                 results.normal     = w0 - w1;
                 results.distance   = results.normal.Length();
                 results.normal    /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
                 return(true);
             }
             else
             {
                 //GjkEpaSolver2Status
                 results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
                 return(false);
             }
         }
 }
Esempio n. 2
0
        //
        public float SignedDistance(ref IndexedVector3 position, float margin, ConvexShape shape0, ref IndexedMatrix wtrs0, ref GjkEpaSolver2Results results)
        {
            using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
                using (GJK gjk = BulletGlobals.GJKPool.Get())
                {
                    SphereShape shape1 = BulletGlobals.SphereShapePool.Get();
                    shape1.Initialize(margin);
                    IndexedMatrix wtrs1 = IndexedMatrix.CreateFromQuaternion(IndexedQuaternion.Identity);
                    wtrs0._origin = position;

                    Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
                    gjk.Initialise();
                    IndexedVector3 guess      = new IndexedVector3(1);
                    GJKStatus      gjk_status = gjk.Evaluate(shape, ref guess);
                    if (gjk_status == GJKStatus.Valid)
                    {
                        IndexedVector3 w0 = IndexedVector3.Zero;
                        IndexedVector3 w1 = IndexedVector3.Zero;
                        for (int i = 0; i < gjk.m_simplex.rank; ++i)
                        {
                            float p = gjk.m_simplex.p[i];
                            w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
                            IndexedVector3 temp = -gjk.m_simplex.c[i].d;
                            w1 += shape.Support(ref temp, 1) * p;
                        }
                        results.witnesses0 = wtrs0 * w0;
                        results.witnesses1 = wtrs0 * w1;
                        IndexedVector3 delta   = results.witnesses1 - results.witnesses0;
                        float          margin2 = shape0.GetMarginNonVirtual() + shape1.GetMarginNonVirtual();
                        float          length  = delta.Length();
                        results.normal      = delta / length;
                        results.witnesses0 += results.normal * margin2;
                        return(length - margin2);
                    }
                    else
                    {
                        if (gjk_status == GJKStatus.Inside)
                        {
                            if (Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, ref results))
                            {
                                IndexedVector3 delta  = results.witnesses0 - results.witnesses1;
                                float          length = delta.Length();
                                if (length >= MathUtil.SIMD_EPSILON)
                                {
                                    results.normal = delta / length;
                                }
                                return(-length);
                            }
                        }
                    }
                    BulletGlobals.SphereShapePool.Free(shape1);
                }
            return(MathUtil.SIMD_INFINITY);
        }
Esempio n. 3
0
        public static bool Penetration(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results, bool usemargins)
        {
            using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
                using (GJK gjk = BulletGlobals.GJKPool.Get())
                {
                    Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, usemargins);
                    gjk.Initialise();
                    IndexedVector3 minusGuess = -guess;
                    GJKStatus      gjk_status = gjk.Evaluate(shape, ref minusGuess);
                    switch (gjk_status)
                    {
                    case GJKStatus.Inside:
                    {
                        //EPA	epa = new EPA();
                        eStatus epa_status = epa.Evaluate(gjk, ref minusGuess);
                        if (epa_status != eStatus.Failed)
                        {
                            IndexedVector3 w0 = IndexedVector3.Zero;
                            for (uint i = 0; i < epa.m_result.rank; ++i)
                            {
                                // order of results here is 'different' , EPA.evaluate.
                                w0 += shape.Support(ref epa.m_result.c[i].d, 0) * epa.m_result.p[i];
                            }
                            results.status     = GjkEpaSolver2Status.Penetrating;
                            results.witnesses0 = wtrs0 * w0;
                            results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
                            results.normal     = -epa.m_normal;
                            results.distance   = -epa.m_depth;
                            return(true);
                        }
                        else
                        {
                            results.status = GjkEpaSolver2Status.EPA_Failed;
                        }
                    }
                    break;

                    case GJKStatus.Failed:
                        results.status = GjkEpaSolver2Status.GJK_Failed;
                        break;
                    }
                }
            return(false);
        }