示例#1
0
        public static void ComputeDistance(out DistanceOutput output,
                                           out SimplexCache cache,
                                           ref DistanceInput input)
        {
            cache = new SimplexCache();
            ++b2_gjkCalls;

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

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

            // Get simplex vertices as an array.
            int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            FixedArray3 <int> saveA = new FixedArray3 <int>();
            FixedArray3 <int> saveB = new FixedArray3 <int>();
            int saveCount           = 0;

            Vector2 closestPoint = simplex.GetClosestPoint();
            float   distanceSqr1 = closestPoint.sqrMagnitude;
            float   distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;

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

                switch (simplex._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._count == 3)
                {
                    break;
                }

                // Compute closest point.
                Vector2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.sqrMagnitude;

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

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

                // Ensure the search direction is numerically fit.
                if (d.sqrMagnitude < Settings.b2_epsilon * Settings.b2_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 = simplex._v[simplex._count];
                vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
                vertex.wA     = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));

                vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
                vertex.wB     = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++b2_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._count;
            }

            b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance   = (output.pointA - output.pointB).magnitude;
            output.iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(ref cache);

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

                if (output.distance > rA + rB && output.distance > Settings.b2_epsilon)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    Vector2 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.
                    Vector2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA   = p;
                    output.pointB   = p;
                    output.distance = 0.0f;
                }
            }
        }
示例#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;
			    }
			}
		}
示例#3
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;
                }
            }
        }