Пример #1
0
        // TODO_ERIN might not need to return the separation
        public float initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA,
            DistanceProxy proxyB, Sweep sweepB, float t1)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;
            Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            m_sweepA.getTransform(xfa, t1);
            m_sweepB.getTransform(xfb, t1);

            // log.debug("initializing separation.\n" +
            // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
            // "distance: "+proxyA.

            if (count == 1)
            {
                m_type = TOIType.POINTS;
                /*
               * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
               * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
               * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
               */
                localPointA.set(m_proxyA.getVertex(cache.indexA[0]));
                localPointB.set(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, ref pointA);
                Transform.mulToOutUnsafe(xfb, localPointB, ref pointB);
                m_axis.set(pointB);
                m_axis.subLocal(pointA);
                float s = m_axis.normalize();
                return s;
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                m_type = TOIType.FACE_B;

                localPointB1.set(m_proxyB.getVertex(cache.indexB[0]));
                localPointB2.set(m_proxyB.getVertex(cache.indexB[1]));

                temp.set(localPointB2);
                temp.subLocal(localPointB1);
                Vec2.crossToOutUnsafe(temp, 1f, ref m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfb.q, m_axis, ref normal);

                m_localPoint.set(localPointB1);
                m_localPoint.addLocal(localPointB2);
                m_localPoint.mulLocal(.5f);
                Transform.mulToOutUnsafe(xfb, m_localPoint, ref pointB);

                localPointA.set(proxyA.getVertex(cache.indexA[0]));
                Transform.mulToOutUnsafe(xfa, localPointA, ref pointA);

                temp.set(pointA);
                temp.subLocal(pointB);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                m_type = TOIType.FACE_A;

                localPointA1.set(m_proxyA.getVertex(cache.indexA[0]));
                localPointA2.set(m_proxyA.getVertex(cache.indexA[1]));

                temp.set(localPointA2);
                temp.subLocal(localPointA1);
                Vec2.crossToOutUnsafe(temp, 1.0f, ref m_axis);
                m_axis.normalize();

                Rot.mulToOutUnsafe(xfa.q, m_axis, ref normal);

                m_localPoint.set(localPointA1);
                m_localPoint.addLocal(localPointA2);
                m_localPoint.mulLocal(.5f);

                Transform.mulToOutUnsafe(xfa, m_localPoint, ref pointA);

                localPointB.set(m_proxyB.getVertex(cache.indexB[0]));
                Transform.mulToOutUnsafe(xfb, localPointB, ref pointB);

                temp.set(pointB);
                temp.subLocal(pointA);
                float s = Vec2.dot(temp, normal);
                if (s < 0.0f)
                {
                    m_axis.negateLocal();
                    s = -s;
                }
                return s;
            }
        }
Пример #2
0
 public void set(SimplexCache sc)
 {
     Array.Copy(sc.indexA, 0, indexA, 0, indexA.Length);
     Array.Copy(sc.indexB, 0, indexB, 0, indexB.Length);
     metric = sc.metric;
     count = sc.count;
 }
Пример #3
0
            public void writeCache(SimplexCache cache)
            {
                cache.metric = getMetric();
                cache.count = m_count;

                for (int i = 0; i < m_count; ++i)
                {
                    cache.indexA[i] = (vertices[i].indexA);
                    cache.indexB[i] = (vertices[i].indexB);
                }
            }
Пример #4
0
        /**
           * Compute the closest points between two shapes. Supports any combination of: CircleShape and
           * PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
           * zero.
           *
           * @param output
           * @param cache
           * @param input
           */
        public void distance(DistanceOutput output, SimplexCache cache, DistanceInput input)
        {
            GJK_CALLS++;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Transform transformA = input.transformA;
            Transform transformB = input.transformB;

            // Initialize the simplex.
            simplex.readCache(cache, proxyA, transformA, proxyB, transformB);

            // Get simplex vertices as an array.
            SimplexVertex[] vertices = simplex.vertices;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            // (pooled above)
            int saveCount = 0;

            simplex.getClosestPoint(ref closestPoint);
            float distanceSqr1 = closestPoint.lengthSquared();
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop
            int iter = 0;
            while (iter < MAX_ITERS)
            {

                // Copy simplex so we can identify duplicates.
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; i++)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

                switch (simplex.m_count)
                {
                    case 1:
                        break;
                    case 2:
                        simplex.solve2();
                        break;
                    case 3:
                        simplex.solve3();
                        break;
                    default:
                        Debug.Assert(false);
                        break;
                }

                // If we have 3 points, then the origin is in the corresponding triangle.
                if (simplex.m_count == 3)
                {
                    break;
                }

                // Compute closest point.
                simplex.getClosestPoint(ref closestPoint);
                distanceSqr2 = closestPoint.lengthSquared();

                // ensure progress
                if (distanceSqr2 >= distanceSqr1)
                {
                    // break;
                }
                distanceSqr1 = distanceSqr2;

                // get search direction;
                simplex.getSearchDirection(ref d);

                // Ensure the search direction is numerically fit.
                if (d.lengthSquared() < Settings.EPSILON*Settings.EPSILON)
                {
                    // The origin is probably contained by a line segment
                    // or triangle. Thus the shapes are overlapped.

                    // We can't return zero here even though there may be overlap.
                    // In case the simplex is a point, segment, or triangle it is difficult
                    // to determine if the origin is contained in the CSO or very close to it.
                    break;
                }
                /*
               * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
               * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
               * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
               * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
               * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
               */

                // Compute a tentative new simplex vertex using support points.
                SimplexVertex vertex = vertices[simplex.m_count];

                d.negateLocal();
                Rot.mulTransUnsafe(transformA.q, d, ref temp);
                vertex.indexA = proxyA.getSupport(temp);
                Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), ref vertex.wA);
                // Vec2 wBLocal;
                d.negateLocal();
                Rot.mulTransUnsafe(transformB.q, d, ref temp);
                vertex.indexB = proxyB.getSupport(temp);

                Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), ref vertex.wB);
                vertex.w.set(vertex.wB);
                vertex.w.subLocal(vertex.wA);

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++GJK_ITERS;

                // Check for duplicate support points. This is the main termination criteria.
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
                    {
                        duplicate = true;
                        break;
                    }
                }

                // If we found a duplicate support point we must exit to avoid cycling.
                if (duplicate)
                {
                    break;
                }

                // New vertex is ok and needed.
                ++simplex.m_count;
            }

            GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter);

            // Prepare output.
            simplex.getWitnessPoints(output.pointA, output.pointB);
            output.distance = MathUtils.distance(output.pointA, output.pointB);
            output.iterations = iter;

            // Cache the simplex.
            simplex.writeCache(cache);

            // Apply radii if requested.
            if (input.useRadii)
            {
                float rA = proxyA.m_radius;
                float rB = proxyB.m_radius;

                if (output.distance > rA + rB && output.distance > Settings.EPSILON)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    normal.set(output.pointB);
                    normal.subLocal(output.pointA);
                    normal.normalize();
                    temp.set(normal);
                    temp.mulLocal(rA);
                    output.pointA.addLocal(temp);
                    temp.set(normal);
                    temp.mulLocal(rB);
                    output.pointB.subLocal(temp);
                }
                else
                {
                    // Shapes are overlapped when radii are considered.
                    // Move the witness points to the middle.
                    // Vec2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA.addLocal(output.pointB);
                    output.pointA.mulLocal(.5f);
                    output.pointB.set(output.pointA);
                    output.distance = 0.0f;
                }
            }
        }
Пример #5
0
            public void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA,
                DistanceProxy proxyB, Transform transformB)
            {
                Debug.Assert(cache.count <= 3);

                // Copy data from cache.
                m_count = cache.count;

                for (int i = 0; i < m_count; ++i)
                {
                    SimplexVertex v = vertices[i];
                    v.indexA = cache.indexA[i];
                    v.indexB = cache.indexB[i];
                    Vec2 wALocal = proxyA.getVertex(v.indexA);
                    Vec2 wBLocal = proxyB.getVertex(v.indexB);
                    Transform.mulToOutUnsafe(transformA, wALocal, ref v.wA);
                    Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB);
                    v.w.set(v.wB);
                    v.w.subLocal(v.wA);
                    v.a = 0.0f;
                }

                // Compute the new simplex metric, if it is substantially different than
                // old metric then flush the simplex.
                if (m_count > 1)
                {
                    float metric1 = cache.metric;
                    float metric2 = getMetric();
                    if (metric2 < 0.5f*metric1 || 2.0f*metric1 < metric2 || metric2 < Settings.EPSILON)
                    {
                        // Reset the simplex.
                        m_count = 0;
                    }
                }

                // If the cache is empty or invalid ...
                if (m_count == 0)
                {
                    SimplexVertex v = vertices[0];
                    v.indexA = 0;
                    v.indexB = 0;
                    Vec2 wALocal = proxyA.getVertex(0);
                    Vec2 wBLocal = proxyB.getVertex(0);
                    Transform.mulToOutUnsafe(transformA, wALocal, ref v.wA);
                    Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB);
                    v.w.set(v.wB);
                    v.w.subLocal(v.wA);
                    m_count = 1;
                }
            }