public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.Zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); return; } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } }
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.Zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.vertices[cache.IndexA[0]]; Vector2 localPointB = _proxyB.vertices[cache.IndexB[0]]; Vector2 pointA = MathUtils.mul(ref xfA, localPointA); Vector2 pointB = MathUtils.mul(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB.vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.mul(ref xfB.q, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.mul(ref xfB, _localPoint); Vector2 localPointA = proxyA.vertices[cache.IndexA[0]]; Vector2 pointA = MathUtils.mul(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.vertices[cache.IndexA[0]]; Vector2 localPointA2 = _proxyA.vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.mul(ref xfA.q, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.mul(ref xfA, _localPoint); Vector2 localPointB = _proxyB.vertices[cache.IndexB[0]]; Vector2 pointB = MathUtils.mul(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } //FPE note: the returned value that used to be here has been removed, as it was not used. }
public SeparationFunction(ref SimplexCache cache, Shape shapeA, ref XForm transformA, Shape shapeB, ref XForm transformB) { _localPoint = Vector2.Zero; _shapeA = shapeA; _shapeB = shapeB; int count = cache.count; Debug.Assert(0 < count && count < 3); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]); Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.indexB[0] == cache.indexB[1]) { // Two points on A and one on B _type = SeparationFunctionType.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 = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.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 = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis); Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on B and two points on A. // The faces are parallel. Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]); Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]); Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]); Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]); Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1); Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1); Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1); Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1); float a = Vector2.Dot(dA, dA); float e = Vector2.Dot(dB, dB); Vector2 r = pA - pB; float c = Vector2.Dot(dA, r); float f = Vector2.Dot(dB, r); float b = Vector2.Dot(dA, dB); float denom = a * e - b * b; float s = 0.0f; if (denom != 0.0f) { s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f); } float t = (b * s + f) / e; if (t < 0.0f) { t = 0.0f; s = MathUtils.Clamp(-c / a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f); } Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1); Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1); if (s == 0.0f || s == 1.0f) { _type = SeparationFunctionType.FaceB; _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); _localPoint = localPointB; Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float sgn = Vector2.Dot(pointA - pointB, normal); if (sgn < 0.0f) { _axis = -_axis; } } else { _type = SeparationFunctionType.FaceA; _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); _localPoint = localPointA; Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis); Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA); Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB); float sgn = Vector2.Dot(pointB - pointA, normal); if (sgn < 0.0f) { _axis = -_axis; } } } }
public SeparationFunction(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.Zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.GetVertex(cache.IndexA[0]); Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); return; } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]); Vector2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]); Vector2 a = localPointB2 - localPointB1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 localPointA = proxyA.GetVertex(cache.IndexA[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vector2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vector2 a = localPointA2 - localPointA1; _axis = new Vector2(a.Y, -a.X); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } }