예제 #1
0
        public eStatus Evaluate(GJK gjk, ref IndexedVector3 guess)
        {
            sSimplex simplex = gjk.m_simplex;

            if ((simplex.rank > 1) && gjk.EncloseOrigin())
            {
                /* Clean up				*/
                while (m_hull.Count > 0)
                {
                    sFace f = m_hull[0];
                    Remove(m_hull, f);
                    Append(m_stock, f);
                }

                m_status = eStatus.Valid;
                m_nextsv = 0;
                /* Orient simplex		*/
                if (GJK.Det(simplex.c[0].w - simplex.c[3].w,
                            simplex.c[1].w - simplex.c[3].w,
                            simplex.c[2].w - simplex.c[3].w) < 0)
                {
                    SwapSv(simplex.c, 0, 1);
                    SwapFloat(simplex.p, 0, 1);
                }
                /* Build initial hull	*/
                tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true);
                tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true);
                tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true);
                tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true);
                if (m_hull.Count == 4)
                {
                    sFace best       = FindBest();
                    sFace outer      = best;
                    uint  pass       = 0;
                    uint  iterations = 0;
                    Bind(tetra[0], 0, tetra[1], 0);
                    Bind(tetra[0], 1, tetra[2], 0);
                    Bind(tetra[0], 2, tetra[3], 0);
                    Bind(tetra[1], 1, tetra[3], 2);
                    Bind(tetra[1], 2, tetra[2], 1);
                    Bind(tetra[2], 2, tetra[3], 1);
                    m_status = eStatus.Valid;
                    for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
                    {
                        if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
                        {
                            sHorizon horizon = new sHorizon();
                            sSV      w       = m_sv_store[m_nextsv++];
                            bool     valid   = true;
                            best.pass = (uint)(++pass);
                            gjk.GetSupport(ref best.n, ref w);
                            float wdist = IndexedVector3.Dot(ref best.n, ref w.w) - best.d;
                            if (wdist > GjkEpaSolver2.EPA_ACCURACY)
                            {
                                for (int j = 0; (j < 3) && valid; ++j)
                                {
                                    valid &= Expand(pass, w,
                                                    best.f[j], best.e[j],
                                                    ref horizon);
                                }
                                if (valid && (horizon.nf >= 3))
                                {
                                    Bind(horizon.cf, 1, horizon.ff, 2);
                                    Remove(m_hull, best);
                                    Append(m_stock, best);
                                    best = FindBest();
                                    if (best.p >= outer.p)
                                    {
                                        outer = best;
                                    }
                                }
                                else
                                {
                                    m_status = eStatus.InvalidHull;
                                    break;
                                }
                            }
                            else
                            {
                                m_status = eStatus.AccuraryReached;
                                break;
                            }
                        }
                        else
                        {
                            m_status = eStatus.OutOfVertices;
                            break;
                        }
                    }
                    IndexedVector3 projection = outer.n * outer.d;
                    m_normal      = outer.n;
                    m_depth       = outer.d;
                    m_result.rank = 3;
                    m_result.c[0] = outer.c[0];
                    m_result.c[1] = outer.c[1];
                    m_result.c[2] = outer.c[2];
                    m_result.p[0] = IndexedVector3.Cross(outer.c[1].w - projection,
                                                         outer.c[2].w - projection).Length();
                    m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
                                                         outer.c[0].w - projection).Length();
                    m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection,
                                                         outer.c[1].w - projection).Length();
                    float sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
                    m_result.p[0] /= sum;
                    m_result.p[1] /= sum;
                    m_result.p[2] /= sum;
                    return(m_status);
                }
            }
            /* Fallback		*/
            m_status = eStatus.FallBack;
            m_normal = -guess;
            float nl = m_normal.LengthSquared();

            if (nl > 0)
            {
                m_normal.Normalize();
            }
            else
            {
                m_normal = new IndexedVector3(1, 0, 0);
            }

            m_depth       = 0;
            m_result.rank = 1;
            m_result.c[0] = simplex.c[0];
            m_result.p[0] = 1;
            return(m_status);
        }
예제 #2
0
        public bool     EncloseOrigin()
        {
            switch (m_simplex.rank)
            {
            case    1:
            {
                for (int i = 0; i < 3; ++i)
                {
                    IndexedVector3 axis = IndexedVector3.Zero;
                    axis[i] = 1f;
                    AppendVertice(m_simplex, ref axis);
                    if (EncloseOrigin())
                    {
                        return(true);
                    }
                    RemoveVertice(m_simplex);
                    IndexedVector3 temp = -axis;
                    AppendVertice(m_simplex, ref temp);
                    if (EncloseOrigin())
                    {
                        return(true);
                    }
                    RemoveVertice(m_simplex);
                }
                break;
            }

            case    2:
            {
                IndexedVector3 d = m_simplex.c[1].w - m_simplex.c[0].w;
                for (int i = 0; i < 3; ++i)
                {
                    IndexedVector3 axis = IndexedVector3.Zero;
                    axis[i] = 1f;
                    IndexedVector3 p = IndexedVector3.Cross(d, axis);
                    if (p.LengthSquared() > 0)
                    {
                        AppendVertice(m_simplex, ref p);
                        if (EncloseOrigin())
                        {
                            return(true);
                        }
                        RemoveVertice(m_simplex);
                        IndexedVector3 temp = -p;
                        AppendVertice(m_simplex, ref temp);
                        if (EncloseOrigin())
                        {
                            return(true);
                        }
                        RemoveVertice(m_simplex);
                    }
                }
                break;
            }

            case 3:
            {
                IndexedVector3 n = IndexedVector3.Cross(m_simplex.c[1].w - m_simplex.c[0].w,
                                                        m_simplex.c[2].w - m_simplex.c[0].w);
                if (n.LengthSquared() > 0)
                {
                    AppendVertice(m_simplex, ref n);
                    if (EncloseOrigin())
                    {
                        return(true);
                    }
                    RemoveVertice(m_simplex);
                    IndexedVector3 temp = -n;
                    AppendVertice(m_simplex, ref temp);
                    if (EncloseOrigin())
                    {
                        return(true);
                    }
                    RemoveVertice(m_simplex);
                }
                break;
            }

            case 4:
            {
                if (Math.Abs(GJK.Det(m_simplex.c[0].w - m_simplex.c[3].w,
                                     m_simplex.c[1].w - m_simplex.c[3].w,
                                     m_simplex.c[2].w - m_simplex.c[3].w)) > 0)
                {
                    return(true);
                }
                break;
            }

            default:
            {
                break;
            }
            }
            return(false);
        }