예제 #1
0
        public static float ProjectOrigin(ref IndexedVector3 a,
                                          ref IndexedVector3 b,
                                          ref IndexedVector3 c,
                                          ref IndexedVector3 d,
                                          ref IndexedVector4 w, ref uint m)
        {
            //uint[] imd3 ={1,2,0};
            //IndexedVector3[]	vt = {a,b,c,d};
            //IndexedVector3[]	dl= {a-d,b-d,c-d};
            Debug.Assert(inhere1 == false);
            inhere1 = true;
            vta[0]  = a; vta[1] = b; vta[2] = c; vta[3] = d;
            dla[0]  = a - d; dla[1] = b - d; dla[2] = c - d;

            float vl = Det(dl[0], dl[1], dl[2]);

            bool ng = (vl * IndexedVector3.Dot(a, IndexedVector3.Cross(b - c, a - b))) <= 0;

            if (ng && (Math.Abs(vl) > GjkEpaSolver2.GJK_SIMPLEX4_EPS))
            {
                float          mindist = -1;
                IndexedVector4 subw    = new IndexedVector4();
                uint           subm    = 0;
                for (int i = 0; i < 3; ++i)
                {
                    uint  j = imd3[i];
                    float s = vl * IndexedVector3.Dot(d, IndexedVector3.Cross(dl[i], dl[j]));
                    if (s > 0)
                    {
                        float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref d, ref subw, ref subm);
                        if ((mindist < 0) || (subd < mindist))
                        {
                            mindist = subd;
                            m       = (uint)((((subm & 1) != 0)?1 << i:0) +
                                             (((subm & 2) != 0)?1 << (int)j:0) +
                                             (((subm & 4) != 0)?8:0));

                            w[(int)i]       = subw.X;
                            w[(int)j]       = subw.Y;
                            w[(int)imd3[j]] = 0f;
                            w.W             = subw.Z;
                        }
                    }
                }
                if (mindist < 0)
                {
                    mindist = 0;
                    m       = 15;
                    w.X     = Det(c, b, d) / vl;
                    w.Y     = Det(a, c, d) / vl;
                    w.Z     = Det(b, a, d) / vl;
                    w.W     = 1 - (w.X + w.Y + w.Z);
                }
                inhere1 = false;
                return(mindist);
            }
            inhere1 = false;
            return(-1);
        }
예제 #2
0
        public static float ProjectOrigin(ref IndexedVector3 a,
                                          ref IndexedVector3 b,
                                          ref IndexedVector3 c,
                                          ref IndexedVector4 w, ref uint m)
        {
            Debug.Assert(inhere2 == false);
            inhere2 = true;
            vt[0]   = a; vt[1] = b; vt[2] = c;
            dl[0]   = a - b; dl[1] = b - c; dl[2] = c - a;

            IndexedVector3 n = IndexedVector3.Cross(dl[0], dl[1]);
            float          l = n.LengthSquared();

            if (l > GjkEpaSolver2.GJK_SIMPLEX3_EPS)
            {
                float          mindist = -1f;
                IndexedVector4 subw    = new IndexedVector4();
                uint           subm    = 0;
                for (int i = 0; i < 3; ++i)
                {
                    if (IndexedVector3.Dot(vt[i], IndexedVector3.Cross(dl[i], n)) > 0)
                    {
                        uint  j    = imd3[i];
                        float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref subw, ref subm);
                        if ((mindist < 0) || (subd < mindist))
                        {
                            mindist = subd;
                            m       = (uint)((((subm & 1) != 0)?1 << i:0) + (((subm & 2) != 0)?1 << (int)j:0));

                            w[(int)i]       = subw.X;
                            w[(int)j]       = subw.Y;
                            w[(int)imd3[j]] = 0f;
                        }
                    }
                }
                if (mindist < 0)
                {
                    float          d = IndexedVector3.Dot(ref a, ref n);
                    float          s = (float)Math.Sqrt(l);
                    IndexedVector3 p = n * (d / l);
                    mindist = p.LengthSquared();
                    m       = 7;
                    w.X     = (IndexedVector3.Cross(dl[1], b - p)).Length() / s;
                    w.Y     = (IndexedVector3.Cross(dl[2], c - p)).Length() / s;
                    w.Z     = 1 - (w.X + w.Y);
                }
                inhere2 = false;
                return(mindist);
            }
            inhere2 = false;
            return(-1);
        }
예제 #3
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);
        }