예제 #1
0
        /// Create a convex hull from the given array of local points.
        /// The count must be in the range [3, Settings._maxPolygonVertices].
        /// @warning the points may be re-ordered, even if they form a convex polygon
        /// @warning collinear points are handled but not removed. Collinear points
        /// may lead to poor stacking behavior.
        public void Set(Vec2[] vertices, int count)
        {
            Utilities.Assert(3 <= count && count <= Settings._maxPolygonVertices);
            if (count < 3)
            {
                SetAsBox(1.0f, 1.0f);
                return;
            }

            int n = Math.Min(count, Settings._maxPolygonVertices);

            // Copy vertices into local buffer
            Vec2[] ps = new Vec2[Settings._maxPolygonVertices];
            for (int i = 0; i < n; ++i)
            {
                ps[i] = vertices[i];
            }

            // Create the convex hull using the Gift wrapping algorithm
            // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm

            // Find the right most point on the hull
            int   i0 = 0;
            float x0 = ps[0].X;

            for (int i = 1; i < count; ++i)
            {
                float x = ps[i].X;
                if (x > x0 || (x == x0 && ps[i].Y < ps[i0].Y))
                {
                    i0 = i;
                    x0 = x;
                }
            }

            int[] hull = new int[Settings._maxPolygonVertices];
            int   m    = 0;
            int   ih   = i0;

            for (;;)
            {
                hull[m] = ih;

                int ie = 0;
                for (int j = 1; j < n; ++j)
                {
                    if (ie == ih)
                    {
                        ie = j;
                        continue;
                    }

                    Vec2  r = ps[ie] - ps[hull[m]];
                    Vec2  v = ps[j] - ps[hull[m]];
                    float c = Utilities.Cross(r, v);
                    if (c < 0.0f)
                    {
                        ie = j;
                    }

                    // Collinearity check
                    if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
                    {
                        ie = j;
                    }
                }

                ++m;
                ih = ie;

                if (ie == i0)
                {
                    break;
                }
            }

            m_count = m;

            // Copy vertices.
            for (int i = 0; i < m; ++i)
            {
                m_vertices[i] = ps[hull[i]];
            }

            // Compute normals. Ensure the edges have non-zero length.
            for (int i = 0; i < m; ++i)
            {
                int  i1   = i;
                int  i2   = i + 1 < m ? i + 1 : 0;
                Vec2 edge = m_vertices[i2] - m_vertices[i1];
                Utilities.Assert(edge.LengthSquared() > Single.Epsilon * Single.Epsilon);
                m_normals[i] = Utilities.Cross(edge, 1.0f);
                m_normals[i].Normalize();
            }

            // Compute the polygon centroid.
            m_centroid = ComputeCentroid(m_vertices, m);
        }
예제 #2
0
        /// Compute the closest points between two shapes. Supports any combination of:
        /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
        /// On the first call set SimplexCache.count to zero.
        public static void Distance(out DistanceOutput output,
                                    SimplexCache cache,
                                    DistanceInput input)
        {
            ++_gjkCalls;

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

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

            // Initialize the simplex.
            Simplex simplex = new Simplex();

            simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);

            // Get simplex vertices as an array.
            SimplexVertex[] vertices   = simplex.verticies;
            const int       k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            int[] saveA     = new int[3];
            int[] saveB     = new int[3];
            int   saveCount = 0;

            float distanceSqr1 = Single.MaxValue;
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;

            while (iter < k_maxIters)
            {
                // 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:
                    Utilities.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.
                Vec2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

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

                // Get search direction.
                Vec2 d = simplex.GetSearchDirection();

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Single.Epsilon * Single.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;
                }

                // Compute a tentative new simplex vertex using support points.
                SimplexVertex vertex = vertices[simplex.m_count];
                vertex.indexA = proxyA.GetSupport(Utilities.MulT(transformA.q, -d));
                vertex.wA     = Utilities.Mul(transformA, proxyA.GetVertex(vertex.indexA));
                Vec2 wBLocal;
                vertex.indexB = proxyB.GetSupport(Utilities.MulT(transformB.q, d));
                vertex.wB     = Utilities.Mul(transformB, proxyB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;

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

                // 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;
            }

            _gjkMaxIters = Math.Max(_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance   = Utilities.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 > Single.Epsilon)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    Vec2 normal = output.pointB - output.pointA;
                    normal.Normalize();
                    output.pointA += rA * normal;
                    output.pointB -= rB * normal;
                }
                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   = p;
                    output.pointB   = p;
                    output.distance = 0.0f;
                }
            }
        }