Ejemplo n.º 1
0
 /// <summary>
 ///     Writes the cache using the specified cache
 /// </summary>
 /// <param name="cache">The cache</param>
 internal void WriteCache(ref SimplexCache cache)
 {
     cache.Metric = GetMetric();
     cache.Count  = (ushort)Count;
     for (int i = 0; i < Count; ++i)
     {
         cache.IndexA[i] = (byte)V[i].IndexA;
         cache.IndexB[i] = (byte)V[i].IndexB;
     }
 }
Ejemplo n.º 2
0
        /// <summary>
        ///     Reads the cache using the specified cache
        /// </summary>
        /// <param name="cache">The cache</param>
        /// <param name="proxyA">The proxy</param>
        /// <param name="transformA">The transform</param>
        /// <param name="proxyB">The proxy</param>
        /// <param name="transformB">The transform</param>
        internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA,
                                ref DistanceProxy proxyB, ref Transform transformB)
        {
            Debug.Assert(cache.Count <= 3);

            // Copy data from cache.
            Count = cache.Count;
            for (int i = 0; i < Count; ++i)
            {
                SimplexVertex v = V[i];
                v.IndexA = cache.IndexA[i];
                v.IndexB = cache.IndexB[i];
                Vector2 wALocal = proxyA.Vertices[v.IndexA];
                Vector2 wBLocal = proxyB.Vertices[v.IndexB];
                v.Wa = MathUtils.Mul(ref transformA, wALocal);
                v.Wb = MathUtils.Mul(ref transformB, wBLocal);
                v.W  = v.Wb - v.Wa;
                v.A  = 0.0f;
                V[i] = v;
            }

            // 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 < MathConstants.Epsilon)
                {
                    // Reset the simplex.
                    Count = 0;
                }
            }

            // If the cache is empty or invalid ...
            if (Count == 0)
            {
                SimplexVertex v = V[0];
                v.IndexA = 0;
                v.IndexB = 0;
                Vector2 wALocal = proxyA.Vertices[0];
                Vector2 wBLocal = proxyB.Vertices[0];
                v.Wa  = MathUtils.Mul(ref transformA, wALocal);
                v.Wb  = MathUtils.Mul(ref transformB, wBLocal);
                v.W   = v.Wb - v.Wa;
                v.A   = 1.0f;
                V[0]  = v;
                Count = 1;
            }
        }