public static float FindMinSeparation(out int indexA, out int indexB, float t) { Transform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case SeparationFunctionType.Points: { Vector2 axisA = Complex.Divide(ref _axis, ref xfA.q); Vector2 axisB = -Complex.Divide(ref _axis, ref xfB.q); indexA = _proxyA.GetSupport(axisA); indexB = _proxyB.GetSupport(axisB); Vector2 localPointA = _proxyA.Vertices[indexA]; Vector2 localPointB = _proxyB.Vertices[indexB]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); float separation = Vector2.Dot(pointB - pointA, _axis); return(separation); } case SeparationFunctionType.FaceA: { Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q); Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA); Vector2 axisB = -Complex.Divide(ref normal, ref xfB.q); indexA = -1; indexB = _proxyB.GetSupport(axisB); Vector2 localPointB = _proxyB.Vertices[indexB]; Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q); Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB); Vector2 axisA = -Complex.Divide(ref normal, ref xfA.q); indexB = -1; indexA = _proxyA.GetSupport(axisA); Vector2 localPointA = _proxyA.Vertices[indexA]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); indexA = -1; indexB = -1; return(0.0f); } }
public float FindMinSeparation(out int indexA, out int indexB, float t) { Transform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case SeparationFunctionType.Points: { Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis); Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis); indexA = _proxyA.GetSupport(axisA); indexB = _proxyB.GetSupport(axisB); Vector2 localPointA = _proxyA.GetVertex(indexA); Vector2 localPointB = _proxyB.GetVertex(indexB); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, _axis); return(separation); } case SeparationFunctionType.FaceA: { Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal); indexA = -1; indexB = _proxyB.GetSupport(axisB); Vector2 localPointB = _proxyB.GetVertex(indexB); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal); indexB = -1; indexA = _proxyA.GetSupport(axisA); Vector2 localPointA = _proxyA.GetVertex(indexA); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); indexA = -1; indexB = -1; return(0.0f); } }
public static float FindMinSeparation(out int indexA, out int indexB, float t) { Transform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case SeparationFunctionType.Points: { var axisA = MathUtils.MulT(ref xfA.Q, _axis); var axisB = MathUtils.MulT(ref xfB.Q, -_axis); indexA = _proxyA.GetSupport(axisA); indexB = _proxyB.GetSupport(axisB); var localPointA = _proxyA.vertices[indexA]; var localPointB = _proxyB.vertices[indexB]; var pointA = MathUtils.Mul(ref xfA, localPointA); var pointB = MathUtils.Mul(ref xfB, localPointB); var separation = Vector2.Dot(pointB - pointA, _axis); return(separation); } case SeparationFunctionType.FaceA: { var normal = MathUtils.Mul(ref xfA.Q, _axis); var pointA = MathUtils.Mul(ref xfA, _localPoint); var axisB = MathUtils.MulT(ref xfB.Q, -normal); indexA = -1; indexB = _proxyB.GetSupport(axisB); var localPointB = _proxyB.vertices[indexB]; var pointB = MathUtils.Mul(ref xfB, localPointB); var separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { var normal = MathUtils.Mul(ref xfB.Q, _axis); var pointB = MathUtils.Mul(ref xfB, _localPoint); var axisA = MathUtils.MulT(ref xfA.Q, -normal); indexB = -1; indexA = _proxyA.GetSupport(axisA); var localPointA = _proxyA.vertices[indexA]; var pointA = MathUtils.Mul(ref xfA, localPointA); var separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); indexA = -1; indexB = -1; return(0.0f); } }