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 static void Initialize(ConvexShape shape0, ref IndexedMatrix wtrs0,
                                      ConvexShape shape1, ref IndexedMatrix wtrs1,
                                      ref GjkEpaSolver2Results results,
                                      GjkEpaSolver2MinkowskiDiff shapeR,
                                      bool withmargins)
        {
            /* Results		*/
            results.witnesses0 = IndexedVector3.Zero;
            results.witnesses1 = IndexedVector3.Zero;
            results.status     = GjkEpaSolver2Status.Separated;
            /* Shape		*/
            shapeR.m_shapes[0] = shape0;
            shapeR.m_shapes[1] = shape1;

            shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis);
            shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1);
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1);

                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
            }
#endif

            shapeR.EnableMargin(withmargins);
        }
Esempio n. 3
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. 4
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);
        }
Esempio n. 5
0
        public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
        {
            uint  iterations = 0;
            float sqdist     = 0f;
            float alpha      = 0f;
            uint  clastw     = 0;

            /* Initialize solver		*/
            m_free[0]  = m_store[0];
            m_free[1]  = m_store[1];
            m_free[2]  = m_store[2];
            m_free[3]  = m_store[3];
            m_nfree    = 4;
            m_current  = 0;
            m_status   = GJKStatus.Valid;
            m_shape    = shapearg;
            m_distance = 0f;
            /* Initialize simplex		*/
            m_simplices[0].rank = 0;
            m_ray = guess;
            float          sqrl = m_ray.LengthSquared();
            IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(1, 0, 0);

            AppendVertice(m_simplices[0], ref temp);
            m_simplices[0].p[0] = 1;
            m_ray    = m_simplices[0].c[0].w;
            sqdist   = sqrl;
            lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray;
            /* Loop						*/
            do
            {
                uint     next = 1 - m_current;
                sSimplex cs   = m_simplices[m_current];
                sSimplex ns   = m_simplices[next];
                /* Check zero							*/
                float rl = m_ray.Length();
                if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
                {/* Touching or inside				*/
                    m_status = GJKStatus.Inside;
                    break;
                }
                /* Append new vertice in -'v' direction	*/
                IndexedVector3 temp2 = -m_ray;
                AppendVertice(cs, ref temp2);
                IndexedVector3 w     = cs.c[cs.rank - 1].w;
                bool           found = false;
                for (int i = 0; i < 4; ++i)
                {
                    if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
                    {
                        found = true;
                        break;
                    }
                }
                if (found)
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }
                else
                {/* Update lastw					*/
                    lastw[clastw = (clastw + 1) & 3] = w;
                }
                /* Check for termination				*/
                float omega = IndexedVector3.Dot(ref m_ray, ref w) / rl;
                alpha = Math.Max(omega, alpha);
                if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }

                /* Reduce simplex						*/
                IndexedVector4 weights = new IndexedVector4();
                uint           mask    = 0;
                switch (cs.rank)
                {
                case 2:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref weights, ref mask);
                    break;
                }

                case 3:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
                    break;
                }

                case 4:
                {
                    sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
                    break;
                }
                }
                if (sqdist >= 0)
                {/* Valid	*/
                    ns.rank   = 0;
                    m_ray     = IndexedVector3.Zero;
                    m_current = next;
                    for (uint i = 0, ni = cs.rank; i < ni; ++i)
                    {
                        if ((mask & (1 << (int)i)) != 0)
                        {
                            ns.c[ns.rank] = cs.c[i];
                            float weight = weights[(int)i];
                            ns.p[ns.rank++] = weight;
                            m_ray          += cs.c[i].w * weight;
                        }
                        else
                        {
                            m_free[m_nfree++] = cs.c[i];
                        }
                    }
                    if (mask == 15)
                    {
                        m_status = GJKStatus.Inside;
                    }
                }
                else
                {/* Return old simplex				*/
                    RemoveVertice(m_simplices[m_current]);
                    break;
                }
                m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
            } while(m_status == GJKStatus.Valid);

            m_simplex = m_simplices[m_current];

            switch (m_status)
            {
            case    GJKStatus.Valid:
            {
                m_distance = m_ray.Length();
                break;
            }

            case    GJKStatus.Inside:
            {
                m_distance = 0;
                break;
            }
            }

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
            {
                BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
            }
#endif

            return(m_status);
        }