Exemplo n.º 1
0
        // Solve a line segment using barycentric coordinates.
        //
        // p = a1 * w1 + a2 * w2
        // a1 + a2 = 1
        //
        // The vector from the origin to the closest point on the line is
        // perpendicular to the line.
        // e12 = w2 - w1
        // dot(p, e) = 0
        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
        //
        // 2-by-2 linear system
        // [1      1     ][a1] = [1]
        // [w1.e12 w2.e12][a2] = [0]
        //
        // Define
        // d12_1 =  dot(w2, e12)
        // d12_2 = -dot(w1, e12)
        // d12 = d12_1 + d12_2
        //
        // Solution
        // a1 = d12_1 / d12
        // a2 = d12_2 / d12
        internal void Solve2()
        {
            Vector2 w1  = _v1.w;
            Vector2 w2  = _v2.w;
            Vector2 e12 = w2 - w1;

            // w1 region
            float d12_2 = -Vector2.Dot(w1, e12);

            if (d12_2 <= 0.0f)
            {
                // a2 <= 0, so we clamp it to 0
                _v1.a  = 1.0f;
                _count = 1;
                return;
            }

            // w2 region
            float d12_1 = Vector2.Dot(w2, e12);

            if (d12_1 <= 0.0f)
            {
                // a1 <= 0, so we clamp it to 0
                _v2.a  = 1.0f;
                _count = 1;
                _v1    = _v2;
                return;
            }

            // Must be in e12 region.
            float inv_d12 = 1.0f / (d12_1 + d12_2);

            _v1.a  = d12_1 * inv_d12;
            _v2.a  = d12_2 * inv_d12;
            _count = 2;
        }
		static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
		{
			output = new DistanceOutput();

			Transform transformA = input.TransformA;
			Transform transformB = input.TransformB;

			// Initialize the simplex.
			Simplex simplex = new Simplex();
#if ALLOWUNSAFE
			fixed (SimplexCache* sPtr = &cache)
			{
				simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
			}
#else
			simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif

			// Get simplex vertices as an array.
#if ALLOWUNSAFE
			SimplexVertex* vertices = &simplex._v1;
#else
			SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif 

			// These store the vertices of the last simplex so that we
			// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
			int* lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
			int[] lastA = new int[4];
			int[] lastB = new int[4];
#endif // ALLOWUNSAFE
			int lastCount;

			// Main iteration loop.
			int iter = 0;
			const int k_maxIterationCount = 20;
			while (iter < k_maxIterationCount)
			{
				// Copy simplex so we can identify duplicates.
				lastCount = simplex._count;
				int i;
				for (i = 0; i < lastCount; ++i)
				{
					lastA[i] = vertices[i].indexA;
					lastB[i] = vertices[i].indexB;
				}

				switch (simplex._count)
				{
					case 1:
						break;

					case 2:
						simplex.Solve2();
						break;

					case 3:
						simplex.Solve3();
						break;

					default:
#if DEBUG
						Box2DXDebug.Assert(false);
#endif
						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();
				float distanceSqr = p.sqrMagnitude;

				// Ensure the search direction is numerically fit.
				if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
				{
					// 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.
#if ALLOWUNSAFE
				SimplexVertex* vertex = vertices + simplex._count;
				vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
				vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
				//Vec2 wBLocal;
				vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
				vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
				vertex->w = vertex->wB - vertex->wA;
#else
				SimplexVertex vertex = vertices[simplex._count - 1];
				vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
				vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
				//Vec2 wBLocal;
				vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
				vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
				vertex.w = vertex.wB - vertex.wA;	
#endif // ALLOWUNSAFE

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

				// Check for convergence.
#if ALLOWUNSAFE
				float lowerBound = Vector2.Dot(p, vertex->w);
#else
				float lowerBound = Vector2.Dot(p, vertex.w);
#endif
				float upperBound = distanceSqr;
				const float k_relativeTolSqr = 0.01f * 0.01f;	// 1:100
				if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
				{
					// Converged!
					break;
				}

				// Check for duplicate support points.
				bool duplicate = false;
				for (i = 0; i < lastCount; ++i)
				{
#if ALLOWUNSAFE
					if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
					if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
					{
						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;
			}

			
#if ALLOWUNSAFE
			fixed (DistanceOutput* doPtr = &output)
			{
				// Prepare output.
				simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
				doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
				doPtr->Iterations = iter;
			}

			fixed (SimplexCache* sPtr = &cache)
			{
				// Cache the simplex.
				simplex.WriteCache(sPtr);
			}
#else
			// Prepare output.
			simplex.GetWitnessPoints(out output.PointA, out output.PointB);
			output.Distance = Vector2.Distance(output.PointA, output.PointB);
			output.Iterations = iter;
			
			// Cache the simplex.
			simplex.WriteCache(cache);
#endif

			// Apply radii if requested.
			if (input.UseRadii)
			{
				float rA = shapeA._radius;
				float rB = shapeB._radius;

				if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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;
				}
			}
		}
		// Possible regions:
		// - points[2]
		// - edge points[0]-points[2]
		// - edge points[1]-points[2]
		// - inside the triangle
		internal void Solve3()
		{
			Vector2 w1 = _v1.w;
			Vector2 w2 = _v2.w;
			Vector2 w3 = _v3.w;

			// Edge12
			// [1      1     ][a1] = [1]
			// [w1.e12 w2.e12][a2] = [0]
			// a3 = 0
			Vector2 e12 = w2 - w1;
			float w1e12 = Vector2.Dot(w1, e12);
			float w2e12 = Vector2.Dot(w2, e12);
			float d12_1 = w2e12;
			float d12_2 = -w1e12;

			// Edge13
			// [1      1     ][a1] = [1]
			// [w1.e13 w3.e13][a3] = [0]
			// a2 = 0
			Vector2 e13 = w3 - w1;
			float w1e13 = Vector2.Dot(w1, e13);
			float w3e13 = Vector2.Dot(w3, e13);
			float d13_1 = w3e13;
			float d13_2 = -w1e13;

			// Edge23
			// [1      1     ][a2] = [1]
			// [w2.e23 w3.e23][a3] = [0]
			// a1 = 0
			Vector2 e23 = w3 - w2;
			float w2e23 = Vector2.Dot(w2, e23);
			float w3e23 = Vector2.Dot(w3, e23);
			float d23_1 = w3e23;
			float d23_2 = -w2e23;

			// Triangle123
			float n123 = e12.Cross(e13); 

			float d123_1 = n123 * w2.Cross(w3);
			float d123_2 = n123 * w3.Cross(w1);
			float d123_3 = n123 * w1.Cross(w2);

			// w1 region
			if (d12_2 <= 0.0f && d13_2 <= 0.0f)
			{
				_v1.a = 1.0f;
				_count = 1;
				return;
			}

			// e12
			if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
			{
				float inv_d12 = 1.0f / (d12_1 + d12_2);
				_v1.a = d12_1 * inv_d12;
				_v2.a = d12_1 * inv_d12;
				_count = 2;
				return;
			}

			// e13
			if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
			{
				float inv_d13 = 1.0f / (d13_1 + d13_2);
				_v1.a = d13_1 * inv_d13;
				_v3.a = d13_2 * inv_d13;
				_count = 2;
				_v2 = _v3;
				return;
			}

			// w2 region
			if (d12_1 <= 0.0f && d23_2 <= 0.0f)
			{
				_v2.a = 1.0f;
				_count = 1;
				_v1 = _v2;
				return;
			}

			// w3 region
			if (d13_1 <= 0.0f && d23_1 <= 0.0f)
			{
				_v3.a = 1.0f;
				_count = 1;
				_v1 = _v3;
				return;
			}

			// e23
			if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
			{
				float inv_d23 = 1.0f / (d23_1 + d23_2);
				_v2.a = d23_1 * inv_d23;
				_v3.a = d23_2 * inv_d23;
				_count = 2;
				_v1 = _v3;
				return;
			}

			// Must be in triangle123
			float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
			_v1.a = d123_1 * inv_d123;
			_v2.a = d123_2 * inv_d123;
			_v3.a = d123_3 * inv_d123;
			_count = 3;
		}
		// Solve a line segment using barycentric coordinates.
		//
		// p = a1 * w1 + a2 * w2
		// a1 + a2 = 1
		//
		// The vector from the origin to the closest point on the line is
		// perpendicular to the line.
		// e12 = w2 - w1
		// dot(p, e) = 0
		// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
		//
		// 2-by-2 linear system
		// [1      1     ][a1] = [1]
		// [w1.e12 w2.e12][a2] = [0]
		//
		// Define
		// d12_1 =  dot(w2, e12)
		// d12_2 = -dot(w1, e12)
		// d12 = d12_1 + d12_2
		//
		// Solution
		// a1 = d12_1 / d12
		// a2 = d12_2 / d12
		internal void Solve2()
		{
			Vector2 w1 = _v1.w;
			Vector2 w2 = _v2.w;
			Vector2 e12 = w2 - w1;

			// w1 region
			float d12_2 = -Vector2.Dot(w1, e12);
			if (d12_2 <= 0.0f)
			{
				// a2 <= 0, so we clamp it to 0
				_v1.a = 1.0f;
				_count = 1;
				return;
			}

			// w2 region
			float d12_1 = Vector2.Dot(w2, e12);
			if (d12_1 <= 0.0f)
			{
				// a1 <= 0, so we clamp it to 0
				_v2.a = 1.0f;
				_count = 1;
				_v1 = _v2;
				return;
			}

			// Must be in e12 region.
			float inv_d12 = 1.0f / (d12_1 + d12_2);
			_v1.a = d12_1 * inv_d12;
			_v2.a = d12_2 * inv_d12;
			_count = 2;
		}
		internal void WriteCache(SimplexCache cache)
		{
			cache.Metric = GetMetric();
			cache.Count = (UInt16)_count;
			SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
			for (int i = 0; i < _count; ++i)
			{
				cache.IndexA[i] = (Byte)(vertices[i].indexA);
				cache.IndexB[i] = (Byte)(vertices[i].indexB);
			}
		}
		internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
		{
			Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);

			// Copy data from cache.
			_count = cache.Count;
			SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
			for (int i = 0; i < _count; ++i)
			{
				SimplexVertex v = vertices[i];
				v.indexA = cache.IndexA[i];
				v.indexB = cache.IndexB[i];
				Vector2 wALocal = shapeA.GetVertex(v.indexA);
				Vector2 wBLocal = shapeB.GetVertex(v.indexB);
				v.wA = transformA.TransformPoint(wALocal);
				v.wB = transformB.TransformPoint(wBLocal);
				v.w = v.wB - v.wA;
				v.a = 0.0f;
			}

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

			// If the cache is empty or invalid ...
			if (_count == 0)
			{
				SimplexVertex v = vertices[0];
				v.indexA = 0;
				v.indexB = 0;
				Vector2 wALocal = shapeA.GetVertex(0);
				Vector2 wBLocal = shapeB.GetVertex(0);
				v.wA = transformA.TransformPoint(wALocal);
				v.wB = transformB.TransformPoint(wBLocal);
				v.w = v.wB - v.wA;
				_count = 1;
			}
		}
Exemplo n.º 7
0
        static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
        {
            output = new DistanceOutput();

            Transform transformA = input.TransformA;
            Transform transformB = input.TransformB;

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

#if ALLOWUNSAFE
            fixed(SimplexCache *sPtr = &cache)
            {
                simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
            }
#else
            simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif

            // Get simplex vertices as an array.
#if ALLOWUNSAFE
            SimplexVertex *vertices = &simplex._v1;
#else
            SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
            int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
            int[] lastA = new int[4];
            int[] lastB = new int[4];
#endif // ALLOWUNSAFE
            int lastCount;

            // Main iteration loop.
            int       iter = 0;
            const int k_maxIterationCount = 20;
            while (iter < k_maxIterationCount)
            {
                // Copy simplex so we can identify duplicates.
                lastCount = simplex._count;
                int i;
                for (i = 0; i < lastCount; ++i)
                {
                    lastA[i] = vertices[i].indexA;
                    lastB[i] = vertices[i].indexB;
                }

                switch (simplex._count)
                {
                case 1:
                    break;

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
#if DEBUG
                    Box2DXDebug.Assert(false);
#endif
                    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();
                float   distanceSqr = p.sqrMagnitude;

                // Ensure the search direction is numerically fit.
                if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
                {
                    // 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.
#if ALLOWUNSAFE
                SimplexVertex *vertex = vertices + simplex._count;
                vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex->wA     = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
                //Vec2 wBLocal;
                vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex->wB     = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
                vertex->w      = vertex->wB - vertex->wA;
#else
                SimplexVertex vertex = vertices[simplex._count - 1];
                vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex.wA     = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
                //Vec2 wBLocal;
                vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex.wB     = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE

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

                // Check for convergence.
#if ALLOWUNSAFE
                float lowerBound = Vector2.Dot(p, vertex->w);
#else
                float lowerBound = Vector2.Dot(p, vertex.w);
#endif
                float       upperBound       = distanceSqr;
                const float k_relativeTolSqr = 0.01f * 0.01f;                   // 1:100
                if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
                {
                    // Converged!
                    break;
                }

                // Check for duplicate support points.
                bool duplicate = false;
                for (i = 0; i < lastCount; ++i)
                {
#if ALLOWUNSAFE
                    if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
                    if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
                    {
                        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;
            }


#if ALLOWUNSAFE
            fixed(DistanceOutput *doPtr = &output)
            {
                // Prepare output.
                simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
                doPtr->Distance   = Vector2.Distance(doPtr->PointA, doPtr->PointB);
                doPtr->Iterations = iter;
            }

            fixed(SimplexCache *sPtr = &cache)
            {
                // Cache the simplex.
                simplex.WriteCache(sPtr);
            }
#else
            // Prepare output.
            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
            output.Distance   = Vector2.Distance(output.PointA, output.PointB);
            output.Iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(cache);
#endif

            // Apply radii if requested.
            if (input.UseRadii)
            {
                float rA = shapeA._radius;
                float rB = shapeB._radius;

                if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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;
                }
            }
        }
Exemplo n.º 8
0
        // Possible regions:
        // - points[2]
        // - edge points[0]-points[2]
        // - edge points[1]-points[2]
        // - inside the triangle
        internal void Solve3()
        {
            Vector2 w1 = _v1.w;
            Vector2 w2 = _v2.w;
            Vector2 w3 = _v3.w;

            // Edge12
            // [1      1     ][a1] = [1]
            // [w1.e12 w2.e12][a2] = [0]
            // a3 = 0
            Vector2 e12   = w2 - w1;
            float   w1e12 = Vector2.Dot(w1, e12);
            float   w2e12 = Vector2.Dot(w2, e12);
            float   d12_1 = w2e12;
            float   d12_2 = -w1e12;

            // Edge13
            // [1      1     ][a1] = [1]
            // [w1.e13 w3.e13][a3] = [0]
            // a2 = 0
            Vector2 e13   = w3 - w1;
            float   w1e13 = Vector2.Dot(w1, e13);
            float   w3e13 = Vector2.Dot(w3, e13);
            float   d13_1 = w3e13;
            float   d13_2 = -w1e13;

            // Edge23
            // [1      1     ][a2] = [1]
            // [w2.e23 w3.e23][a3] = [0]
            // a1 = 0
            Vector2 e23   = w3 - w2;
            float   w2e23 = Vector2.Dot(w2, e23);
            float   w3e23 = Vector2.Dot(w3, e23);
            float   d23_1 = w3e23;
            float   d23_2 = -w2e23;

            // Triangle123
            float n123 = e12.Cross(e13);

            float d123_1 = n123 * w2.Cross(w3);
            float d123_2 = n123 * w3.Cross(w1);
            float d123_3 = n123 * w1.Cross(w2);

            // w1 region
            if (d12_2 <= 0.0f && d13_2 <= 0.0f)
            {
                _v1.a  = 1.0f;
                _count = 1;
                return;
            }

            // e12
            if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
            {
                float inv_d12 = 1.0f / (d12_1 + d12_2);
                _v1.a  = d12_1 * inv_d12;
                _v2.a  = d12_1 * inv_d12;
                _count = 2;
                return;
            }

            // e13
            if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
            {
                float inv_d13 = 1.0f / (d13_1 + d13_2);
                _v1.a  = d13_1 * inv_d13;
                _v3.a  = d13_2 * inv_d13;
                _count = 2;
                _v2    = _v3;
                return;
            }

            // w2 region
            if (d12_1 <= 0.0f && d23_2 <= 0.0f)
            {
                _v2.a  = 1.0f;
                _count = 1;
                _v1    = _v2;
                return;
            }

            // w3 region
            if (d13_1 <= 0.0f && d23_1 <= 0.0f)
            {
                _v3.a  = 1.0f;
                _count = 1;
                _v1    = _v3;
                return;
            }

            // e23
            if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
            {
                float inv_d23 = 1.0f / (d23_1 + d23_2);
                _v2.a  = d23_1 * inv_d23;
                _v3.a  = d23_2 * inv_d23;
                _count = 2;
                _v1    = _v3;
                return;
            }

            // Must be in triangle123
            float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);

            _v1.a  = d123_1 * inv_d123;
            _v2.a  = d123_2 * inv_d123;
            _v3.a  = d123_3 * inv_d123;
            _count = 3;
        }
 public Simplex()
 {
     for (int i = 0; i < 3; i++)
     {
         Vertices[i] = new SimplexVertex();
     }
 }
Exemplo n.º 10
0
        /// <summary>
        /// Compute the closest points between two shapes. Supports any combination of:
        /// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
        /// On the first call set b2SimplexCache.count to zero.
        /// </summary>
        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, ref transformA, proxyB, ref transformB);

            // Get simplex vertices as an array.
            SimplexVertex[] vertices   = simplex.Vertices;
            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], saveB = new int[3];
            int   saveCount = 0;

            Vec2  closestPoint = simplex.GetClosestPoint();
            float distanceSqr1 = closestPoint.LengthSquared();
            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] = vertices[i].IndexA;
                    saveB[i] = vertices[i].IndexB;
                }

                switch (simplex.Count)
                {
                case 1:
                    break;

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
                    Box2DXDebug.Assert(false);
                    break;
                }

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

                // Compute closest point.
                Vec2  p           = simplex.GetClosestPoint();
                float distanceSqr = 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() < Settings.FLT_EPSILON * Settings.FLT_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.Count];
                vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
                vertex.WA     = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA));

                vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
                vertex.WB     = Math.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.Count;
            }

            GjkMaxIters = Math.Max(GjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
            output.Distance   = Vec2.Distance(output.PointA, output.PointB);
            output.Iterations = iter;

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

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

                if (output.Distance > rA + rB && output.Distance > Settings.FLT_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;
                }
            }
        }