/// <summary> /// Reads the cache using the specified cache /// </summary> /// <param name="cache">The cache</param> /// <param name="shapeA">The shape</param> /// <param name="transformA">The transform</param> /// <param name="shapeB">The shape</param> /// <param name="transformB">The transform</param> internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB) { Box2DxDebug.Assert(0 <= cache->Count && cache->Count <= 3); // Copy data from cache. Count = cache->Count; SimplexVertex **vertices = stackalloc SimplexVertex *[3]; fixed(SimplexVertex *v1Ptr = &V1, v2Ptr = &V2, v3Ptr = &V3) { vertices[0] = v1Ptr; vertices[1] = v2Ptr; vertices[2] = v3Ptr; for (int i = 0; i < Count; ++i) { SimplexVertex *v = vertices[i]; v->IndexA = cache->IndexA[i]; v->IndexB = cache->IndexB[i]; Vec2 wALocal = shapeA.GetVertex(v->IndexA); Vec2 wBLocal = shapeB.GetVertex(v->IndexB); v->Wa = Math.Mul(transformA, wALocal); v->Wb = Math.Mul(transformB, 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 < Settings.FltEpsilon) { // 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; Vec2 wALocal = shapeA.GetVertex(0); Vec2 wBLocal = shapeB.GetVertex(0); v->Wa = Math.Mul(transformA, wALocal); v->Wb = Math.Mul(transformB, wBLocal); v->W = v->Wb - v->Wa; Count = 1; } } }
internal unsafe 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 = stackalloc SimplexVertex *[3]; fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) { vertices[0] = v1Ptr; vertices[1] = v2Ptr; vertices[2] = v3Ptr; 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; } } }
internal unsafe void WriteCache(SimplexCache *cache) { cache->Metric = GetMetric(); cache->Count = (UInt16)_count; SimplexVertex **vertices = stackalloc SimplexVertex *[3]; fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) { vertices[0] = v1Ptr; vertices[1] = v2Ptr; vertices[2] = v3Ptr; for (int i = 0; i < _count; ++i) { cache->IndexA[i] = (Byte)(vertices[i]->indexA); cache->IndexB[i] = (Byte)(vertices[i]->indexB); } } }
/// <summary> /// Writes the cache using the specified cache /// </summary> /// <param name="cache">The cache</param> internal unsafe void WriteCache(SimplexCache *cache) { cache->Metric = GetMetric(); cache->Count = (ushort)Count; SimplexVertex **vertices = stackalloc SimplexVertex *[3]; fixed(SimplexVertex *v1Ptr = &V1, v2Ptr = &V2, v3Ptr = &V3) { vertices[0] = v1Ptr; vertices[1] = v2Ptr; vertices[2] = v3Ptr; for (int i = 0; i < Count; ++i) { cache->IndexA[i] = (byte)vertices[i]->IndexA; cache->IndexB[i] = (byte)vertices[i]->IndexB; } } }