/// <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; } } }
internal unsafe void Initialize(SimplexCache *cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB) { ShapeA = shapeA; ShapeB = shapeB; int count = cache->Count; Box2DNetDebug.Assert(0 < count && count < 3); if (count == 1) { FaceType = Type.Points; Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]); Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); Vec2 pointA = Common.Math.Mul(transformA, localPointA); Vec2 pointB = Common.Math.Mul(transformB, localPointB); Axis = pointB - pointA; Axis.Normalize(); } else if (cache->IndexB[0] == cache->IndexB[1]) { // Two points on A and one on B FaceType = Type.FaceA; Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]); Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]); Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); LocalPoint = 0.5f * (localPointA1 + localPointA2); Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); Axis.Normalize(); Vec2 normal = Common.Math.Mul(transformA.R, Axis); Vec2 pointA = Common.Math.Mul(transformA, LocalPoint); Vec2 pointB = Common.Math.Mul(transformB, localPointB); float s = Vec2.Dot(pointB - pointA, normal); if (s < 0.0f) { Axis = -Axis; } } else { // Two points on B and one or two points on A. // We ignore the second point on A. FaceType = Type.FaceB; Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]); Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]); Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]); LocalPoint = 0.5f * (localPointB1 + localPointB2); Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); Axis.Normalize(); Vec2 normal = Common.Math.Mul(transformB.R, Axis); Vec2 pointB = Common.Math.Mul(transformB, LocalPoint); Vec2 pointA = Common.Math.Mul(transformA, localPointA); float s = Vec2.Dot(pointA - pointB, normal); if (s < 0.0f) { Axis = -Axis; } } }
internal unsafe void Initialize(SimplexCache *cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB) { ShapeA = shapeA; ShapeB = shapeB; int count = cache->Count; Box2DNetDebug.Assert(0 < count && count < 3); if (count == 1) { FaceType = Type.Points; Vector2 localPointA = ShapeA.GetVertex(cache->IndexA[0]); Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); Vector2 pointA = TransformA.TransformPoint(localPointA); Vector2 pointB = TransformB.TransformPoint(localPointB); Axis = pointB - pointA; Axis.Normalize(); } else if (cache->IndexB[0] == cache->IndexB[1]) { // Two points on A and one on B FaceType = Type.FaceA; Vector2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]); Vector2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]); Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); LocalPoint = 0.5f * (localPointA1 + localPointA2); Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f); Axis.Normalize(); Vector2 normal = TransformA.TransformDirection(Axis); Vector2 pointA = TransformA.TransformPoint(LocalPoint); Vector2 pointB = TransformB.TransformPoint(localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { Axis = -Axis; } } else { // Two points on B and one or two points on A. // We ignore the second point on A. FaceType = Type.FaceB; Vector2 localPointA = shapeA.GetVertex(cache->IndexA[0]); Vector2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]); Vector2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]); LocalPoint = 0.5f * (localPointB1 + localPointB2); Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f); Axis.Normalize(); Vector2 normal = TransformB.TransformDirection(Axis); Vector2 pointB = TransformB.TransformPoint(LocalPoint); Vector2 pointA = TransformA.TransformPoint(localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { Axis = -Axis; } } }